2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
28 #include "blackbox_io.h"
30 #include "build/debug.h"
31 #include "build/version.h"
33 #include "common/axis.h"
34 #include "common/encoding.h"
35 #include "common/maths.h"
36 #include "common/utils.h"
38 #include "config/config_profile.h"
39 #include "config/feature.h"
40 #include "config/parameter_group.h"
41 #include "config/parameter_group_ids.h"
43 #include "drivers/sensor.h"
44 #include "drivers/compass.h"
45 #include "drivers/system.h"
46 #include "drivers/pwm_output.h"
48 #include "fc/config.h"
49 #include "fc/controlrate_profile.h"
50 #include "fc/rc_controls.h"
51 #include "fc/runtime_config.h"
53 #include "flight/failsafe.h"
54 #include "flight/mixer.h"
55 #include "flight/navigation.h"
56 #include "flight/pid.h"
57 #include "flight/servos.h"
59 #include "io/beeper.h"
61 #include "io/serial.h"
65 #include "sensors/acceleration.h"
66 #include "sensors/barometer.h"
67 #include "sensors/battery.h"
68 #include "sensors/compass.h"
69 #include "sensors/compass.h"
70 #include "sensors/gyro.h"
71 #include "sensors/sonar.h"
73 #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
74 #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH
75 #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT)
76 #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_SDCARD
78 #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_SERIAL
81 PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t
, blackboxConfig
, PG_BLACKBOX_CONFIG
, 0);
83 PG_RESET_TEMPLATE(blackboxConfig_t
, blackboxConfig
,
84 .device
= DEFAULT_BLACKBOX_DEVICE
,
87 .on_motor_test
= 0 // default off
90 #define BLACKBOX_I_INTERVAL 32
91 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
92 #define SLOW_FRAME_INTERVAL 4096
94 #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
96 #define STATIC_ASSERT(condition, name ) \
97 typedef char assert_failed_ ## name [(condition) ? 1 : -1 ]
99 // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
101 #define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
102 #define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)
103 #define CONDITION(x) CONCAT(FLIGHT_LOG_FIELD_CONDITION_, x)
104 #define UNSIGNED FLIGHT_LOG_FIELD_UNSIGNED
105 #define SIGNED FLIGHT_LOG_FIELD_SIGNED
107 static const char blackboxHeader
[] =
108 "H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
110 "H I interval:" STR(BLACKBOX_I_INTERVAL
) "\n";
112 static const char* const blackboxFieldHeaderNames
[] = {
121 /* All field definition structs should look like this (but with longer arrs): */
122 typedef struct blackboxFieldDefinition_s
{
124 // If the field name has a number to be included in square brackets [1] afterwards, set it here, or -1 for no brackets:
125 int8_t fieldNameIndex
;
127 // Each member of this array will be the value to print for this field for the given header index
129 } blackboxFieldDefinition_t
;
131 #define BLACKBOX_DELTA_FIELD_HEADER_COUNT ARRAY_LENGTH(blackboxFieldHeaderNames)
132 #define BLACKBOX_SIMPLE_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
133 #define BLACKBOX_CONDITIONAL_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
135 typedef struct blackboxSimpleFieldDefinition_s
{
137 int8_t fieldNameIndex
;
142 } blackboxSimpleFieldDefinition_t
;
144 typedef struct blackboxConditionalFieldDefinition_s
{
146 int8_t fieldNameIndex
;
151 uint8_t condition
; // Decide whether this field should appear in the log
152 } blackboxConditionalFieldDefinition_t
;
154 typedef struct blackboxDeltaFieldDefinition_s
{
156 int8_t fieldNameIndex
;
163 uint8_t condition
; // Decide whether this field should appear in the log
164 } blackboxDeltaFieldDefinition_t
;
167 * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
168 * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
169 * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
170 * the encoding we've promised here).
172 static const blackboxDeltaFieldDefinition_t blackboxMainFields
[] = {
173 /* loopIteration doesn't appear in P frames since it always increments */
174 {"loopIteration",-1, UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(INC
), .Pencode
= FLIGHT_LOG_FIELD_ENCODING_NULL
, CONDITION(ALWAYS
)},
175 /* Time advances pretty steadily so the P-frame prediction is a straight line */
176 {"time", -1, UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(STRAIGHT_LINE
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
177 {"axisP", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
178 {"axisP", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
179 {"axisP", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
180 /* I terms get special packed encoding in P frames: */
181 {"axisI", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
182 {"axisI", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
183 {"axisI", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
184 {"axisD", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_0
)},
185 {"axisD", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_1
)},
186 {"axisD", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_2
)},
187 /* rcCommands are encoded together as a group in P-frames: */
188 {"rcCommand", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
189 {"rcCommand", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
190 {"rcCommand", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
191 /* Throttle is always in the range [minthrottle..maxthrottle]: */
192 {"rcCommand", 3, UNSIGNED
, .Ipredict
= PREDICT(MINTHROTTLE
), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
194 {"vbatLatest", -1, UNSIGNED
, .Ipredict
= PREDICT(VBATREF
), .Iencode
= ENCODING(NEG_14BIT
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_VBAT
},
195 {"amperageLatest",-1, UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC
},
198 {"magADC", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
199 {"magADC", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
200 {"magADC", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
203 {"BaroAlt", -1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_BARO
},
206 {"sonarRaw", -1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_SONAR
},
208 {"rssi", -1, UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_RSSI
},
210 /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
211 {"gyroADC", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
212 {"gyroADC", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
213 {"gyroADC", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
214 {"accSmooth", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
215 {"accSmooth", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
216 {"accSmooth", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
217 {"debug", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
218 {"debug", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
219 {"debug", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
220 {"debug", 3, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
221 /* Motors only rarely drops under minthrottle (when stick falls below mincommand), so predict minthrottle for it and use *unsigned* encoding (which is large for negative numbers but more compact for positive ones): */
222 {"motor", 0, UNSIGNED
, .Ipredict
= PREDICT(MINMOTOR
), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_1
)},
223 /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
224 {"motor", 1, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_2
)},
225 {"motor", 2, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_3
)},
226 {"motor", 3, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_4
)},
227 {"motor", 4, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_5
)},
228 {"motor", 5, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_6
)},
229 {"motor", 6, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_7
)},
230 {"motor", 7, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_8
)},
232 /* Tricopter tail servo */
233 {"servo", 5, UNSIGNED
, .Ipredict
= PREDICT(1500), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(TRICOPTER
)}
237 // GPS position/vel frame
238 static const blackboxConditionalFieldDefinition_t blackboxGpsGFields
[] = {
239 {"time", -1, UNSIGNED
, PREDICT(LAST_MAIN_FRAME_TIME
), ENCODING(UNSIGNED_VB
), CONDITION(NOT_LOGGING_EVERY_FRAME
)},
240 {"GPS_numSat", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)},
241 {"GPS_coord", 0, SIGNED
, PREDICT(HOME_COORD
), ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
242 {"GPS_coord", 1, SIGNED
, PREDICT(HOME_COORD
), ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
243 {"GPS_altitude", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)},
244 {"GPS_speed", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)},
245 {"GPS_ground_course", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)}
249 static const blackboxSimpleFieldDefinition_t blackboxGpsHFields
[] = {
250 {"GPS_home", 0, SIGNED
, PREDICT(0), ENCODING(SIGNED_VB
)},
251 {"GPS_home", 1, SIGNED
, PREDICT(0), ENCODING(SIGNED_VB
)}
255 // Rarely-updated fields
256 static const blackboxSimpleFieldDefinition_t blackboxSlowFields
[] = {
257 {"flightModeFlags", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
)},
258 {"stateFlags", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
)},
260 {"failsafePhase", -1, UNSIGNED
, PREDICT(0), ENCODING(TAG2_3S32
)},
261 {"rxSignalReceived", -1, UNSIGNED
, PREDICT(0), ENCODING(TAG2_3S32
)},
262 {"rxFlightChannelsValid", -1, UNSIGNED
, PREDICT(0), ENCODING(TAG2_3S32
)}
265 typedef enum BlackboxState
{
266 BLACKBOX_STATE_DISABLED
= 0,
267 BLACKBOX_STATE_STOPPED
, //1
268 BLACKBOX_STATE_PREPARE_LOG_FILE
, //2
269 BLACKBOX_STATE_SEND_HEADER
, //3
270 BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER
, //4
271 BLACKBOX_STATE_SEND_GPS_H_HEADER
, //5
272 BLACKBOX_STATE_SEND_GPS_G_HEADER
, //6
273 BLACKBOX_STATE_SEND_SLOW_HEADER
, //7
274 BLACKBOX_STATE_SEND_SYSINFO
, //8
275 BLACKBOX_STATE_PAUSED
, //9
276 BLACKBOX_STATE_RUNNING
, //10
277 BLACKBOX_STATE_SHUTTING_DOWN
, //11
278 BLACKBOX_STATE_START_ERASE
, //12
279 BLACKBOX_STATE_ERASING
, //13
280 BLACKBOX_STATE_ERASED
//14
284 typedef struct blackboxMainState_s
{
287 int32_t axisPID_P
[XYZ_AXIS_COUNT
], axisPID_I
[XYZ_AXIS_COUNT
], axisPID_D
[XYZ_AXIS_COUNT
];
289 int16_t rcCommand
[4];
290 int16_t gyroADC
[XYZ_AXIS_COUNT
];
291 int16_t accSmooth
[XYZ_AXIS_COUNT
];
293 int16_t motor
[MAX_SUPPORTED_MOTORS
];
294 int16_t servo
[MAX_SUPPORTED_SERVOS
];
297 uint16_t amperageLatest
;
303 int16_t magADC
[XYZ_AXIS_COUNT
];
309 } blackboxMainState_t
;
311 typedef struct blackboxGpsState_s
{
312 int32_t GPS_home
[2], GPS_coord
[2];
314 } blackboxGpsState_t
;
316 // This data is updated really infrequently:
317 typedef struct blackboxSlowState_s
{
318 uint32_t flightModeFlags
; // extend this data size (from uint16_t)
320 uint8_t failsafePhase
;
321 bool rxSignalReceived
;
322 bool rxFlightChannelsValid
;
323 } __attribute__((__packed__
)) blackboxSlowState_t
; // We pack this struct so that padding doesn't interfere with memcmp()
326 extern uint16_t motorOutputHigh
, motorOutputLow
;
329 extern uint32_t rcModeActivationMask
;
331 static BlackboxState blackboxState
= BLACKBOX_STATE_DISABLED
;
333 static uint32_t blackboxLastArmingBeep
= 0;
334 static uint32_t blackboxLastFlightModeFlags
= 0; // New event tracking of flight modes
338 uint32_t headerIndex
;
340 /* Since these fields are used during different blackbox states (never simultaneously) we can
341 * overlap them to save on RAM
349 // Cache for FLIGHT_LOG_FIELD_CONDITION_* test results:
350 static uint32_t blackboxConditionCache
;
352 STATIC_ASSERT((sizeof(blackboxConditionCache
) * 8) >= FLIGHT_LOG_FIELD_CONDITION_NEVER
, too_many_flight_log_conditions
);
354 static uint32_t blackboxIteration
;
355 static uint16_t blackboxPFrameIndex
, blackboxIFrameIndex
;
356 static uint16_t blackboxSlowFrameIterationTimer
;
357 static bool blackboxLoggedAnyFrames
;
360 * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
361 * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs
364 static uint16_t vbatReference
;
366 static blackboxGpsState_t gpsHistory
;
367 static blackboxSlowState_t slowHistory
;
369 // Keep a history of length 2, plus a buffer for MW to store the new values into
370 static blackboxMainState_t blackboxHistoryRing
[3];
372 // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
373 static blackboxMainState_t
* blackboxHistory
[3];
375 static bool blackboxModeActivationConditionPresent
= false;
378 * Return true if it is safe to edit the Blackbox configuration.
380 bool blackboxMayEditConfig()
382 return blackboxState
<= BLACKBOX_STATE_STOPPED
;
385 static bool blackboxIsOnlyLoggingIntraframes() {
386 return blackboxConfig()->rate_num
== 1 && blackboxConfig()->rate_denom
== 32;
389 static bool testBlackboxConditionUncached(FlightLogFieldCondition condition
)
392 case FLIGHT_LOG_FIELD_CONDITION_ALWAYS
:
395 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1
:
396 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2
:
397 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3
:
398 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4
:
399 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5
:
400 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6
:
401 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7
:
402 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8
:
403 return getMotorCount() >= condition
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1
+ 1;
405 case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
:
406 return mixerConfig()->mixerMode
== MIXER_TRI
|| mixerConfig()->mixerMode
== MIXER_CUSTOM_TRI
;
408 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
:
409 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1
:
410 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2
:
411 return currentPidProfile
->D8
[condition
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
] != 0;
413 case FLIGHT_LOG_FIELD_CONDITION_MAG
:
415 return sensors(SENSOR_MAG
);
420 case FLIGHT_LOG_FIELD_CONDITION_BARO
:
422 return sensors(SENSOR_BARO
);
427 case FLIGHT_LOG_FIELD_CONDITION_VBAT
:
428 return batteryConfig()->voltageMeterSource
!= VOLTAGE_METER_NONE
;
430 case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC
:
431 return batteryConfig()->currentMeterSource
== CURRENT_METER_ADC
;
433 case FLIGHT_LOG_FIELD_CONDITION_SONAR
:
435 return feature(FEATURE_SONAR
);
440 case FLIGHT_LOG_FIELD_CONDITION_RSSI
:
441 return rxConfig()->rssi_channel
> 0 || feature(FEATURE_RSSI_ADC
);
443 case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME
:
444 return blackboxConfig()->rate_num
< blackboxConfig()->rate_denom
;
446 case FLIGHT_LOG_FIELD_CONDITION_NEVER
:
453 static void blackboxBuildConditionCache()
455 FlightLogFieldCondition cond
;
457 blackboxConditionCache
= 0;
459 for (cond
= FLIGHT_LOG_FIELD_CONDITION_FIRST
; cond
<= FLIGHT_LOG_FIELD_CONDITION_LAST
; cond
++) {
460 if (testBlackboxConditionUncached(cond
)) {
461 blackboxConditionCache
|= 1 << cond
;
466 static bool testBlackboxCondition(FlightLogFieldCondition condition
)
468 return (blackboxConditionCache
& (1 << condition
)) != 0;
471 static void blackboxSetState(BlackboxState newState
)
473 //Perform initial setup required for the new state
475 case BLACKBOX_STATE_PREPARE_LOG_FILE
:
476 blackboxLoggedAnyFrames
= false;
478 case BLACKBOX_STATE_SEND_HEADER
:
479 blackboxHeaderBudget
= 0;
480 xmitState
.headerIndex
= 0;
481 xmitState
.u
.startTime
= millis();
483 case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER
:
484 case BLACKBOX_STATE_SEND_GPS_G_HEADER
:
485 case BLACKBOX_STATE_SEND_GPS_H_HEADER
:
486 case BLACKBOX_STATE_SEND_SLOW_HEADER
:
487 xmitState
.headerIndex
= 0;
488 xmitState
.u
.fieldIndex
= -1;
490 case BLACKBOX_STATE_SEND_SYSINFO
:
491 xmitState
.headerIndex
= 0;
493 case BLACKBOX_STATE_RUNNING
:
494 blackboxSlowFrameIterationTimer
= SLOW_FRAME_INTERVAL
; //Force a slow frame to be written on the first iteration
496 case BLACKBOX_STATE_SHUTTING_DOWN
:
497 xmitState
.u
.startTime
= millis();
502 blackboxState
= newState
;
505 static void writeIntraframe(void)
507 blackboxMainState_t
*blackboxCurrent
= blackboxHistory
[0];
511 blackboxWriteUnsignedVB(blackboxIteration
);
512 blackboxWriteUnsignedVB(blackboxCurrent
->time
);
514 blackboxWriteSignedVBArray(blackboxCurrent
->axisPID_P
, XYZ_AXIS_COUNT
);
515 blackboxWriteSignedVBArray(blackboxCurrent
->axisPID_I
, XYZ_AXIS_COUNT
);
517 // Don't bother writing the current D term if the corresponding PID setting is zero
518 for (int x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
519 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
+ x
)) {
520 blackboxWriteSignedVB(blackboxCurrent
->axisPID_D
[x
]);
524 // Write roll, pitch and yaw first:
525 blackboxWriteSigned16VBArray(blackboxCurrent
->rcCommand
, 3);
528 * Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
529 * Throttle lies in range [minthrottle..maxthrottle]:
531 blackboxWriteUnsignedVB(blackboxCurrent
->rcCommand
[THROTTLE
] - motorConfig()->minthrottle
);
533 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT
)) {
535 * Our voltage is expected to decrease over the course of the flight, so store our difference from
538 * Write 14 bits even if the number is negative (which would otherwise result in 32 bits)
540 blackboxWriteUnsignedVB((vbatReference
- blackboxCurrent
->vbatLatest
) & 0x3FFF);
543 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC
)) {
544 // 12bit value directly from ADC
545 blackboxWriteUnsignedVB(blackboxCurrent
->amperageLatest
);
549 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG
)) {
550 blackboxWriteSigned16VBArray(blackboxCurrent
->magADC
, XYZ_AXIS_COUNT
);
555 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO
)) {
556 blackboxWriteSignedVB(blackboxCurrent
->BaroAlt
);
561 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR
)) {
562 blackboxWriteSignedVB(blackboxCurrent
->sonarRaw
);
566 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RSSI
)) {
567 blackboxWriteUnsignedVB(blackboxCurrent
->rssi
);
570 blackboxWriteSigned16VBArray(blackboxCurrent
->gyroADC
, XYZ_AXIS_COUNT
);
571 blackboxWriteSigned16VBArray(blackboxCurrent
->accSmooth
, XYZ_AXIS_COUNT
);
572 blackboxWriteSigned16VBArray(blackboxCurrent
->debug
, 4);
574 //Motors can be below minimum output when disarmed, but that doesn't happen much
575 blackboxWriteUnsignedVB(blackboxCurrent
->motor
[0] - motorOutputLow
);
577 //Motors tend to be similar to each other so use the first motor's value as a predictor of the others
578 const int motorCount
= getMotorCount();
579 for (int x
= 1; x
< motorCount
; x
++) {
580 blackboxWriteSignedVB(blackboxCurrent
->motor
[x
] - blackboxCurrent
->motor
[0]);
583 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
)) {
584 //Assume the tail spends most of its time around the center
585 blackboxWriteSignedVB(blackboxCurrent
->servo
[5] - 1500);
588 //Rotate our history buffers:
590 //The current state becomes the new "before" state
591 blackboxHistory
[1] = blackboxHistory
[0];
592 //And since we have no other history, we also use it for the "before, before" state
593 blackboxHistory
[2] = blackboxHistory
[0];
594 //And advance the current state over to a blank space ready to be filled
595 blackboxHistory
[0] = ((blackboxHistory
[0] - blackboxHistoryRing
+ 1) % 3) + blackboxHistoryRing
;
597 blackboxLoggedAnyFrames
= true;
600 static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHistory
, int count
)
602 int16_t *curr
= (int16_t*) ((char*) (blackboxHistory
[0]) + arrOffsetInHistory
);
603 int16_t *prev1
= (int16_t*) ((char*) (blackboxHistory
[1]) + arrOffsetInHistory
);
604 int16_t *prev2
= (int16_t*) ((char*) (blackboxHistory
[2]) + arrOffsetInHistory
);
606 for (int i
= 0; i
< count
; i
++) {
607 // Predictor is the average of the previous two history states
608 int32_t predictor
= (prev1
[i
] + prev2
[i
]) / 2;
610 blackboxWriteSignedVB(curr
[i
] - predictor
);
614 static void writeInterframe(void)
619 blackboxMainState_t
*blackboxCurrent
= blackboxHistory
[0];
620 blackboxMainState_t
*blackboxLast
= blackboxHistory
[1];
624 //No need to store iteration count since its delta is always 1
627 * Since the difference between the difference between successive times will be nearly zero (due to consistent
628 * looptime spacing), use second-order differences.
630 blackboxWriteSignedVB((int32_t) (blackboxHistory
[0]->time
- 2 * blackboxHistory
[1]->time
+ blackboxHistory
[2]->time
));
632 arraySubInt32(deltas
, blackboxCurrent
->axisPID_P
, blackboxLast
->axisPID_P
, XYZ_AXIS_COUNT
);
633 blackboxWriteSignedVBArray(deltas
, XYZ_AXIS_COUNT
);
636 * The PID I field changes very slowly, most of the time +-2, so use an encoding
637 * that can pack all three fields into one byte in that situation.
639 arraySubInt32(deltas
, blackboxCurrent
->axisPID_I
, blackboxLast
->axisPID_I
, XYZ_AXIS_COUNT
);
640 blackboxWriteTag2_3S32(deltas
);
643 * The PID D term is frequently set to zero for yaw, which makes the result from the calculation
644 * always zero. So don't bother recording D results when PID D terms are zero.
646 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
647 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
+ x
)) {
648 blackboxWriteSignedVB(blackboxCurrent
->axisPID_D
[x
] - blackboxLast
->axisPID_D
[x
]);
653 * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
654 * can pack multiple values per byte:
656 for (x
= 0; x
< 4; x
++) {
657 deltas
[x
] = blackboxCurrent
->rcCommand
[x
] - blackboxLast
->rcCommand
[x
];
660 blackboxWriteTag8_4S16(deltas
);
662 //Check for sensors that are updated periodically (so deltas are normally zero)
663 int optionalFieldCount
= 0;
665 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT
)) {
666 deltas
[optionalFieldCount
++] = (int32_t) blackboxCurrent
->vbatLatest
- blackboxLast
->vbatLatest
;
669 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC
)) {
670 deltas
[optionalFieldCount
++] = (int32_t) blackboxCurrent
->amperageLatest
- blackboxLast
->amperageLatest
;
674 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG
)) {
675 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
676 deltas
[optionalFieldCount
++] = blackboxCurrent
->magADC
[x
] - blackboxLast
->magADC
[x
];
682 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO
)) {
683 deltas
[optionalFieldCount
++] = blackboxCurrent
->BaroAlt
- blackboxLast
->BaroAlt
;
688 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR
)) {
689 deltas
[optionalFieldCount
++] = blackboxCurrent
->sonarRaw
- blackboxLast
->sonarRaw
;
693 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RSSI
)) {
694 deltas
[optionalFieldCount
++] = (int32_t) blackboxCurrent
->rssi
- blackboxLast
->rssi
;
697 blackboxWriteTag8_8SVB(deltas
, optionalFieldCount
);
699 //Since gyros, accs and motors are noisy, base their predictions on the average of the history:
700 blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t
, gyroADC
), XYZ_AXIS_COUNT
);
701 blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t
, accSmooth
), XYZ_AXIS_COUNT
);
702 blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t
, debug
), 4);
703 blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t
, motor
), getMotorCount());
705 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
)) {
706 blackboxWriteSignedVB(blackboxCurrent
->servo
[5] - blackboxLast
->servo
[5]);
709 //Rotate our history buffers
710 blackboxHistory
[2] = blackboxHistory
[1];
711 blackboxHistory
[1] = blackboxHistory
[0];
712 blackboxHistory
[0] = ((blackboxHistory
[0] - blackboxHistoryRing
+ 1) % 3) + blackboxHistoryRing
;
714 blackboxLoggedAnyFrames
= true;
717 /* Write the contents of the global "slowHistory" to the log as an "S" frame. Because this data is logged so
718 * infrequently, delta updates are not reasonable, so we log independent frames. */
719 static void writeSlowFrame(void)
725 blackboxWriteUnsignedVB(slowHistory
.flightModeFlags
);
726 blackboxWriteUnsignedVB(slowHistory
.stateFlags
);
729 * Most of the time these three values will be able to pack into one byte for us:
731 values
[0] = slowHistory
.failsafePhase
;
732 values
[1] = slowHistory
.rxSignalReceived
? 1 : 0;
733 values
[2] = slowHistory
.rxFlightChannelsValid
? 1 : 0;
734 blackboxWriteTag2_3S32(values
);
736 blackboxSlowFrameIterationTimer
= 0;
740 * Load rarely-changing values from the FC into the given structure
742 static void loadSlowState(blackboxSlowState_t
*slow
)
744 slow
->flightModeFlags
= rcModeActivationMask
; //was flightModeFlags;
745 slow
->stateFlags
= stateFlags
;
746 slow
->failsafePhase
= failsafePhase();
747 slow
->rxSignalReceived
= rxIsReceivingSignal();
748 slow
->rxFlightChannelsValid
= rxAreFlightChannelsValid();
752 * If the data in the slow frame has changed, log a slow frame.
754 * If allowPeriodicWrite is true, the frame is also logged if it has been more than SLOW_FRAME_INTERVAL logging iterations
755 * since the field was last logged.
757 static void writeSlowFrameIfNeeded(bool allowPeriodicWrite
)
759 // Write the slow frame peridocially so it can be recovered if we ever lose sync
760 bool shouldWrite
= allowPeriodicWrite
&& blackboxSlowFrameIterationTimer
>= SLOW_FRAME_INTERVAL
;
763 loadSlowState(&slowHistory
);
765 blackboxSlowState_t newSlowState
;
767 loadSlowState(&newSlowState
);
769 // Only write a slow frame if it was different from the previous state
770 if (memcmp(&newSlowState
, &slowHistory
, sizeof(slowHistory
)) != 0) {
771 // Use the new state as our new history
772 memcpy(&slowHistory
, &newSlowState
, sizeof(slowHistory
));
782 static int gcd(int num
, int denom
)
788 return gcd(denom
, num
% denom
);
791 void validateBlackboxConfig()
795 if (blackboxConfig()->rate_num
== 0 || blackboxConfig()->rate_denom
== 0
796 || blackboxConfig()->rate_num
>= blackboxConfig()->rate_denom
) {
797 blackboxConfigMutable()->rate_num
= 1;
798 blackboxConfigMutable()->rate_denom
= 1;
800 /* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
801 * itself more frequently)
803 div
= gcd(blackboxConfig()->rate_num
, blackboxConfig()->rate_denom
);
805 blackboxConfigMutable()->rate_num
/= div
;
806 blackboxConfigMutable()->rate_denom
/= div
;
809 // If we've chosen an unsupported device, change the device to serial
810 switch (blackboxConfig()->device
) {
812 case BLACKBOX_DEVICE_FLASH
:
815 case BLACKBOX_DEVICE_SDCARD
:
817 case BLACKBOX_DEVICE_SERIAL
:
818 // Device supported, leave the setting alone
822 blackboxConfigMutable()->device
= BLACKBOX_DEVICE_SERIAL
;
827 * Start Blackbox logging if it is not already running. Intended to be called upon arming.
829 void startBlackbox(void)
831 validateBlackboxConfig();
833 if (!blackboxDeviceOpen()) {
834 blackboxSetState(BLACKBOX_STATE_DISABLED
);
838 memset(&gpsHistory
, 0, sizeof(gpsHistory
));
840 blackboxHistory
[0] = &blackboxHistoryRing
[0];
841 blackboxHistory
[1] = &blackboxHistoryRing
[1];
842 blackboxHistory
[2] = &blackboxHistoryRing
[2];
844 vbatReference
= getBatteryVoltageLatest();
846 //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
849 * We use conditional tests to decide whether or not certain fields should be logged. Since our headers
850 * must always agree with the logged data, the results of these tests must not change during logging. So
853 blackboxBuildConditionCache();
855 blackboxModeActivationConditionPresent
= isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX
);
857 blackboxIteration
= 0;
858 blackboxPFrameIndex
= 0;
859 blackboxIFrameIndex
= 0;
862 * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
863 * it finally plays the beep for this arming event.
865 blackboxLastArmingBeep
= getArmingBeepTimeMicros();
866 blackboxLastFlightModeFlags
= rcModeActivationMask
; // record startup status
868 blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE
);
872 * Begin Blackbox shutdown.
874 void finishBlackbox(void)
876 switch (blackboxState
) {
877 case BLACKBOX_STATE_DISABLED
:
878 case BLACKBOX_STATE_STOPPED
:
879 case BLACKBOX_STATE_SHUTTING_DOWN
:
880 // We're already stopped/shutting down
883 case BLACKBOX_STATE_RUNNING
:
884 case BLACKBOX_STATE_PAUSED
:
885 blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END
, NULL
);
889 blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN
);
894 * Test Motors Blackbox Logging
896 bool startedLoggingInTestMode
= false;
898 void startInTestMode(void)
900 if(!startedLoggingInTestMode
) {
901 if (blackboxConfig()->device
== BLACKBOX_DEVICE_SERIAL
) {
902 serialPort_t
*sharedBlackboxAndMspPort
= findSharedSerialPort(FUNCTION_BLACKBOX
, FUNCTION_MSP
);
903 if (sharedBlackboxAndMspPort
) {
904 return; // When in test mode, we cannot share the MSP and serial logger port!
908 startedLoggingInTestMode
= true;
911 void stopInTestMode(void)
913 if(startedLoggingInTestMode
) {
915 startedLoggingInTestMode
= false;
919 * We are going to monitor the MSP_SET_MOTOR target variables motor_disarmed[] for values other than minthrottle
920 * on reading a value (i.e. the user is testing the motors), then we enable test mode logging;
921 * we monitor when the values return to minthrottle and start a delay timer (5 seconds); if
922 * the test motors are left at minimum throttle for this delay timer, then we assume we are done testing and
923 * shutdown the logger.
925 * Of course, after the 5 seconds and shutdown of the logger, the system will be re-enabled to allow the
926 * test mode to trigger again; its just that the data will be in a second, third, fourth etc log file.
928 bool inMotorTestMode(void) {
929 static uint32_t resetTime
= 0;
930 uint16_t inactiveMotorCommand
;
931 if (feature(FEATURE_3D
)) {
932 inactiveMotorCommand
= flight3DConfig()->neutral3d
;
934 } else if (isMotorProtocolDshot()) {
935 inactiveMotorCommand
= DSHOT_DISARM_COMMAND
;
938 inactiveMotorCommand
= motorConfig()->mincommand
;
942 bool atLeastOneMotorActivated
= false;
944 // set disarmed motor values
945 for (i
= 0; i
< MAX_SUPPORTED_MOTORS
; i
++)
946 atLeastOneMotorActivated
|= (motor_disarmed
[i
] != inactiveMotorCommand
);
948 if(atLeastOneMotorActivated
) {
949 resetTime
= millis() + 5000; // add 5 seconds
952 // Monitor the duration at minimum
953 return (millis() < resetTime
);
959 static void writeGPSHomeFrame()
963 blackboxWriteSignedVB(GPS_home
[0]);
964 blackboxWriteSignedVB(GPS_home
[1]);
965 //TODO it'd be great if we could grab the GPS current time and write that too
967 gpsHistory
.GPS_home
[0] = GPS_home
[0];
968 gpsHistory
.GPS_home
[1] = GPS_home
[1];
971 static void writeGPSFrame(timeUs_t currentTimeUs
)
976 * If we're logging every frame, then a GPS frame always appears just after a frame with the
977 * currentTime timestamp in the log, so the reader can just use that timestamp for the GPS frame.
979 * If we're not logging every frame, we need to store the time of this GPS frame.
981 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME
)) {
982 // Predict the time of the last frame in the main log
983 blackboxWriteUnsignedVB(currentTimeUs
- blackboxHistory
[1]->time
);
986 blackboxWriteUnsignedVB(GPS_numSat
);
987 blackboxWriteSignedVB(GPS_coord
[0] - gpsHistory
.GPS_home
[0]);
988 blackboxWriteSignedVB(GPS_coord
[1] - gpsHistory
.GPS_home
[1]);
989 blackboxWriteUnsignedVB(GPS_altitude
);
990 blackboxWriteUnsignedVB(GPS_speed
);
991 blackboxWriteUnsignedVB(GPS_ground_course
);
993 gpsHistory
.GPS_numSat
= GPS_numSat
;
994 gpsHistory
.GPS_coord
[0] = GPS_coord
[0];
995 gpsHistory
.GPS_coord
[1] = GPS_coord
[1];
1000 * Fill the current state of the blackbox using values read from the flight controller
1002 static void loadMainState(timeUs_t currentTimeUs
)
1004 blackboxMainState_t
*blackboxCurrent
= blackboxHistory
[0];
1007 blackboxCurrent
->time
= currentTimeUs
;
1009 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
1010 blackboxCurrent
->axisPID_P
[i
] = axisPID_P
[i
];
1012 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
1013 blackboxCurrent
->axisPID_I
[i
] = axisPID_I
[i
];
1015 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
1016 blackboxCurrent
->axisPID_D
[i
] = axisPID_D
[i
];
1019 for (i
= 0; i
< 4; i
++) {
1020 blackboxCurrent
->rcCommand
[i
] = rcCommand
[i
];
1023 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
1024 blackboxCurrent
->gyroADC
[i
] = lrintf(gyro
.gyroADCf
[i
]);
1027 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
1028 blackboxCurrent
->accSmooth
[i
] = acc
.accSmooth
[i
];
1031 for (i
= 0; i
< 4; i
++) {
1032 blackboxCurrent
->debug
[i
] = debug
[i
];
1035 const int motorCount
= getMotorCount();
1036 for (i
= 0; i
< motorCount
; i
++) {
1037 blackboxCurrent
->motor
[i
] = motor
[i
];
1040 blackboxCurrent
->vbatLatest
= getBatteryVoltageLatest();
1041 blackboxCurrent
->amperageLatest
= getAmperageLatest();
1044 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
1045 blackboxCurrent
->magADC
[i
] = mag
.magADC
[i
];
1050 blackboxCurrent
->BaroAlt
= baro
.BaroAlt
;
1054 // Store the raw sonar value without applying tilt correction
1055 blackboxCurrent
->sonarRaw
= sonarRead();
1058 blackboxCurrent
->rssi
= rssi
;
1061 //Tail servo for tricopters
1062 blackboxCurrent
->servo
[5] = servo
[5];
1067 * Transmit the header information for the given field definitions. Transmitted header lines look like:
1069 * H Field I name:a,b,c
1070 * H Field I predictor:0,1,2
1072 * For all header types, provide a "mainFrameChar" which is the name for the field and will be used to refer to it in the
1073 * header (e.g. P, I etc). For blackboxDeltaField_t fields, also provide deltaFrameChar, otherwise set this to zero.
1075 * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
1076 * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
1078 * Set xmitState.headerIndex to 0 and xmitState.u.fieldIndex to -1 before calling for the first time.
1080 * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the
1081 * fieldDefinition and secondCondition arrays.
1083 * Returns true if there is still header left to transmit (so call again to continue transmission).
1085 static bool sendFieldDefinition(char mainFrameChar
, char deltaFrameChar
, const void *fieldDefinitions
,
1086 const void *secondFieldDefinition
, int fieldCount
, const uint8_t *conditions
, const uint8_t *secondCondition
)
1088 const blackboxFieldDefinition_t
*def
;
1089 unsigned int headerCount
;
1090 static bool needComma
= false;
1091 size_t definitionStride
= (char*) secondFieldDefinition
- (char*) fieldDefinitions
;
1092 size_t conditionsStride
= (char*) secondCondition
- (char*) conditions
;
1094 if (deltaFrameChar
) {
1095 headerCount
= BLACKBOX_DELTA_FIELD_HEADER_COUNT
;
1097 headerCount
= BLACKBOX_SIMPLE_FIELD_HEADER_COUNT
;
1101 * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit
1105 // On our first call we need to print the name of the header and a colon
1106 if (xmitState
.u
.fieldIndex
== -1) {
1107 if (xmitState
.headerIndex
>= headerCount
) {
1108 return false; //Someone probably called us again after we had already completed transmission
1111 uint32_t charsToBeWritten
= strlen("H Field x :") + strlen(blackboxFieldHeaderNames
[xmitState
.headerIndex
]);
1113 if (blackboxDeviceReserveBufferSpace(charsToBeWritten
) != BLACKBOX_RESERVE_SUCCESS
) {
1114 return true; // Try again later
1117 blackboxHeaderBudget
-= blackboxPrintf("H Field %c %s:", xmitState
.headerIndex
>= BLACKBOX_SIMPLE_FIELD_HEADER_COUNT
? deltaFrameChar
: mainFrameChar
, blackboxFieldHeaderNames
[xmitState
.headerIndex
]);
1119 xmitState
.u
.fieldIndex
++;
1123 // The longest we expect an integer to be as a string:
1124 const uint32_t LONGEST_INTEGER_STRLEN
= 2;
1126 for (; xmitState
.u
.fieldIndex
< fieldCount
; xmitState
.u
.fieldIndex
++) {
1127 def
= (const blackboxFieldDefinition_t
*) ((const char*)fieldDefinitions
+ definitionStride
* xmitState
.u
.fieldIndex
);
1129 if (!conditions
|| testBlackboxCondition(conditions
[conditionsStride
* xmitState
.u
.fieldIndex
])) {
1130 // First (over)estimate the length of the string we want to print
1132 int32_t bytesToWrite
= 1; // Leading comma
1134 // The first header is a field name
1135 if (xmitState
.headerIndex
== 0) {
1136 bytesToWrite
+= strlen(def
->name
) + strlen("[]") + LONGEST_INTEGER_STRLEN
;
1138 //The other headers are integers
1139 bytesToWrite
+= LONGEST_INTEGER_STRLEN
;
1142 // Now perform the write if the buffer is large enough
1143 if (blackboxDeviceReserveBufferSpace(bytesToWrite
) != BLACKBOX_RESERVE_SUCCESS
) {
1144 // Ran out of space!
1148 blackboxHeaderBudget
-= bytesToWrite
;
1156 // The first header is a field name
1157 if (xmitState
.headerIndex
== 0) {
1158 blackboxPrint(def
->name
);
1160 // Do we need to print an index in brackets after the name?
1161 if (def
->fieldNameIndex
!= -1) {
1162 blackboxPrintf("[%d]", def
->fieldNameIndex
);
1165 //The other headers are integers
1166 blackboxPrintf("%d", def
->arr
[xmitState
.headerIndex
- 1]);
1171 // Did we complete this line?
1172 if (xmitState
.u
.fieldIndex
== fieldCount
&& blackboxDeviceReserveBufferSpace(1) == BLACKBOX_RESERVE_SUCCESS
) {
1173 blackboxHeaderBudget
--;
1174 blackboxWrite('\n');
1175 xmitState
.headerIndex
++;
1176 xmitState
.u
.fieldIndex
= -1;
1179 return xmitState
.headerIndex
< headerCount
;
1182 #ifndef BLACKBOX_PRINT_HEADER_LINE
1183 #define BLACKBOX_PRINT_HEADER_LINE(x, ...) case __COUNTER__: \
1184 blackboxPrintfHeaderLine(x, __VA_ARGS__); \
1186 #define BLACKBOX_PRINT_HEADER_LINE_CUSTOM(...) case __COUNTER__: \
1192 * Transmit a portion of the system information headers. Call the first time with xmitState.headerIndex == 0. Returns
1193 * true iff transmission is complete, otherwise call again later to continue transmission.
1195 static bool blackboxWriteSysinfo()
1197 // Make sure we have enough room in the buffer for our longest line (as of this writing, the "Firmware date" line)
1198 if (blackboxDeviceReserveBufferSpace(64) != BLACKBOX_RESERVE_SUCCESS
) {
1202 const controlRateConfig_t
*currentControlRateProfile
= controlRateProfiles(systemConfig()->activeRateProfile
);
1203 switch (xmitState
.headerIndex
) {
1204 BLACKBOX_PRINT_HEADER_LINE("Firmware type:%s", "Cleanflight");
1205 BLACKBOX_PRINT_HEADER_LINE("Firmware revision:%s %s (%s) %s", FC_FIRMWARE_NAME
, FC_VERSION_STRING
, shortGitRevision
, targetName
);
1206 BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate
, buildTime
);
1207 BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", systemConfig()->name
);
1208 BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", blackboxConfig()->rate_num
, blackboxConfig()->rate_denom
);
1209 BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", motorConfig()->minthrottle
);
1210 BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", motorConfig()->maxthrottle
);
1211 BLACKBOX_PRINT_HEADER_LINE("gyro_scale:0x%x", castFloatBytesToInt(1.0f
));
1212 BLACKBOX_PRINT_HEADER_LINE("motorOutput:%d,%d", motorOutputLow
,motorOutputHigh
);
1213 BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc
.dev
.acc_1G
);
1215 BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
1216 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT
)) {
1217 blackboxPrintfHeaderLine("vbatscale:%u", voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT
)->vbatscale
);
1219 xmitState
.headerIndex
+= 2; // Skip the next two vbat fields too
1223 BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage:%u,%u,%u", batteryConfig()->vbatmincellvoltage
,
1224 batteryConfig()->vbatwarningcellvoltage
,
1225 batteryConfig()->vbatmaxcellvoltage
);
1226 BLACKBOX_PRINT_HEADER_LINE("vbatref:%u", vbatReference
);
1228 BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
1229 if (batteryConfig()->currentMeterSource
== CURRENT_METER_ADC
) {
1230 blackboxPrintfHeaderLine("currentSensor:%d,%d",currentSensorADCConfig()->offset
, currentSensorADCConfig()->scale
);
1234 BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro
.targetLooptime
);
1235 BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", gyroConfig()->gyro_sync_denom
);
1236 BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", pidConfig()->pid_process_denom
);
1237 BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", currentControlRateProfile
->rcRate8
);
1238 BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", currentControlRateProfile
->rcExpo8
);
1239 BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", currentControlRateProfile
->rcYawRate8
);
1240 BLACKBOX_PRINT_HEADER_LINE("rcYawExpo:%d", currentControlRateProfile
->rcYawExpo8
);
1241 BLACKBOX_PRINT_HEADER_LINE("thrMid:%d", currentControlRateProfile
->thrMid8
);
1242 BLACKBOX_PRINT_HEADER_LINE("thrExpo:%d", currentControlRateProfile
->thrExpo8
);
1243 BLACKBOX_PRINT_HEADER_LINE("dynThrPID:%d", currentControlRateProfile
->dynThrPID
);
1244 BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint:%d", currentControlRateProfile
->tpa_breakpoint
);
1245 BLACKBOX_PRINT_HEADER_LINE("rates:%d,%d,%d", currentControlRateProfile
->rates
[ROLL
],
1246 currentControlRateProfile
->rates
[PITCH
],
1247 currentControlRateProfile
->rates
[YAW
]);
1248 BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", currentPidProfile
->P8
[ROLL
],
1249 currentPidProfile
->I8
[ROLL
],
1250 currentPidProfile
->D8
[ROLL
]);
1251 BLACKBOX_PRINT_HEADER_LINE("pitchPID:%d,%d,%d", currentPidProfile
->P8
[PITCH
],
1252 currentPidProfile
->I8
[PITCH
],
1253 currentPidProfile
->D8
[PITCH
]);
1254 BLACKBOX_PRINT_HEADER_LINE("yawPID:%d,%d,%d", currentPidProfile
->P8
[YAW
],
1255 currentPidProfile
->I8
[YAW
],
1256 currentPidProfile
->D8
[YAW
]);
1257 BLACKBOX_PRINT_HEADER_LINE("altPID:%d,%d,%d", currentPidProfile
->P8
[PIDALT
],
1258 currentPidProfile
->I8
[PIDALT
],
1259 currentPidProfile
->D8
[PIDALT
]);
1260 BLACKBOX_PRINT_HEADER_LINE("posPID:%d,%d,%d", currentPidProfile
->P8
[PIDPOS
],
1261 currentPidProfile
->I8
[PIDPOS
],
1262 currentPidProfile
->D8
[PIDPOS
]);
1263 BLACKBOX_PRINT_HEADER_LINE("posrPID:%d,%d,%d", currentPidProfile
->P8
[PIDPOSR
],
1264 currentPidProfile
->I8
[PIDPOSR
],
1265 currentPidProfile
->D8
[PIDPOSR
]);
1266 BLACKBOX_PRINT_HEADER_LINE("navrPID:%d,%d,%d", currentPidProfile
->P8
[PIDNAVR
],
1267 currentPidProfile
->I8
[PIDNAVR
],
1268 currentPidProfile
->D8
[PIDNAVR
]);
1269 BLACKBOX_PRINT_HEADER_LINE("levelPID:%d,%d,%d", currentPidProfile
->P8
[PIDLEVEL
],
1270 currentPidProfile
->I8
[PIDLEVEL
],
1271 currentPidProfile
->D8
[PIDLEVEL
]);
1272 BLACKBOX_PRINT_HEADER_LINE("magPID:%d", currentPidProfile
->P8
[PIDMAG
]);
1273 BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", currentPidProfile
->P8
[PIDVEL
],
1274 currentPidProfile
->I8
[PIDVEL
],
1275 currentPidProfile
->D8
[PIDVEL
]);
1276 BLACKBOX_PRINT_HEADER_LINE("dterm_filter_type:%d", currentPidProfile
->dterm_filter_type
);
1277 BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", currentPidProfile
->dterm_lpf_hz
);
1278 BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", currentPidProfile
->yaw_lpf_hz
);
1279 BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz:%d", currentPidProfile
->dterm_notch_hz
);
1280 BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff:%d", currentPidProfile
->dterm_notch_cutoff
);
1281 BLACKBOX_PRINT_HEADER_LINE("itermWindupPointPercent:%d", currentPidProfile
->itermWindupPointPercent
);
1282 BLACKBOX_PRINT_HEADER_LINE("dterm_average_count:%d", currentPidProfile
->dterm_average_count
);
1283 BLACKBOX_PRINT_HEADER_LINE("vbat_pid_compensation:%d", currentPidProfile
->vbatPidCompensation
);
1284 BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle:%d", currentPidProfile
->pidAtMinThrottle
);
1286 // Betaflight PID controller parameters
1287 BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold:%d", currentPidProfile
->itermThrottleThreshold
);
1288 BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain:%d", castFloatBytesToInt(currentPidProfile
->itermAcceleratorGain
));
1289 BLACKBOX_PRINT_HEADER_LINE("setpointRelaxRatio:%d", currentPidProfile
->setpointRelaxRatio
);
1290 BLACKBOX_PRINT_HEADER_LINE("dtermSetpointWeight:%d", currentPidProfile
->dtermSetpointWeight
);
1291 BLACKBOX_PRINT_HEADER_LINE("yawRateAccelLimit:%d", castFloatBytesToInt(currentPidProfile
->yawRateAccelLimit
));
1292 BLACKBOX_PRINT_HEADER_LINE("rateAccelLimit:%d", castFloatBytesToInt(currentPidProfile
->rateAccelLimit
));
1293 BLACKBOX_PRINT_HEADER_LINE("pidSumLimit:%d", castFloatBytesToInt(currentPidProfile
->pidSumLimit
));
1294 BLACKBOX_PRINT_HEADER_LINE("pidSumLimitYaw:%d", castFloatBytesToInt(currentPidProfile
->pidSumLimitYaw
));
1295 // End of Betaflight controller parameters
1297 BLACKBOX_PRINT_HEADER_LINE("deadband:%d", rcControlsConfig()->deadband
);
1298 BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", rcControlsConfig()->yaw_deadband
);
1299 BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", gyroConfig()->gyro_lpf
);
1300 BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", gyroConfig()->gyro_soft_lpf_type
);
1301 BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", gyroConfig()->gyro_soft_lpf_hz
);
1302 BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", gyroConfig()->gyro_soft_notch_hz_1
,
1303 gyroConfig()->gyro_soft_notch_hz_2
);
1304 BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1
,
1305 gyroConfig()->gyro_soft_notch_cutoff_2
);
1306 BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(accelerometerConfig()->acc_lpf_hz
* 100.0f
));
1307 BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", accelerometerConfig()->acc_hardware
);
1308 BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", barometerConfig()->baro_hardware
);
1309 BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", compassConfig()->mag_hardware
);
1310 BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", armingConfig()->gyro_cal_on_first_arm
);
1311 BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d", rxConfig()->rcInterpolation
);
1312 BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d", rxConfig()->rcInterpolationInterval
);
1313 BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", rxConfig()->airModeActivateThreshold
);
1314 BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d", rxConfig()->serialrx_provider
);
1315 BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", motorConfig()->dev
.useUnsyncedPwm
);
1316 BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", motorConfig()->dev
.motorPwmProtocol
);
1317 BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", motorConfig()->dev
.motorPwmRate
);
1318 BLACKBOX_PRINT_HEADER_LINE("digitalIdleOffset:%d", (int)(motorConfig()->digitalIdleOffsetPercent
* 100.0f
));
1319 BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d", systemConfig()->debug_mode
);
1320 BLACKBOX_PRINT_HEADER_LINE("features:%d", featureConfig()->enabledFeatures
);
1326 xmitState
.headerIndex
++;
1331 * Write the given event to the log immediately
1333 void blackboxLogEvent(FlightLogEvent event
, flightLogEventData_t
*data
)
1335 // Only allow events to be logged after headers have been written
1336 if (!(blackboxState
== BLACKBOX_STATE_RUNNING
|| blackboxState
== BLACKBOX_STATE_PAUSED
)) {
1340 //Shared header for event frames
1342 blackboxWrite(event
);
1344 //Now serialize the data for this specific frame type
1346 case FLIGHT_LOG_EVENT_SYNC_BEEP
:
1347 blackboxWriteUnsignedVB(data
->syncBeep
.time
);
1349 case FLIGHT_LOG_EVENT_FLIGHTMODE
: // New flightmode flags write
1350 blackboxWriteUnsignedVB(data
->flightMode
.flags
);
1351 blackboxWriteUnsignedVB(data
->flightMode
.lastFlags
);
1353 case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT
:
1354 if (data
->inflightAdjustment
.floatFlag
) {
1355 blackboxWrite(data
->inflightAdjustment
.adjustmentFunction
+ FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG
);
1356 blackboxWriteFloat(data
->inflightAdjustment
.newFloatValue
);
1358 blackboxWrite(data
->inflightAdjustment
.adjustmentFunction
);
1359 blackboxWriteSignedVB(data
->inflightAdjustment
.newValue
);
1362 case FLIGHT_LOG_EVENT_LOGGING_RESUME
:
1363 blackboxWriteUnsignedVB(data
->loggingResume
.logIteration
);
1364 blackboxWriteUnsignedVB(data
->loggingResume
.currentTime
);
1366 case FLIGHT_LOG_EVENT_LOG_END
:
1367 blackboxPrint("End of log");
1373 /* If an arming beep has played since it was last logged, write the time of the arming beep to the log as a synchronization point */
1374 static void blackboxCheckAndLogArmingBeep()
1376 flightLogEvent_syncBeep_t eventData
;
1378 // Use != so that we can still detect a change if the counter wraps
1379 if (getArmingBeepTimeMicros() != blackboxLastArmingBeep
) {
1380 blackboxLastArmingBeep
= getArmingBeepTimeMicros();
1382 eventData
.time
= blackboxLastArmingBeep
;
1384 blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP
, (flightLogEventData_t
*) &eventData
);
1388 /* monitor the flight mode event status and trigger an event record if the state changes */
1389 static void blackboxCheckAndLogFlightMode()
1391 flightLogEvent_flightMode_t eventData
; // Add new data for current flight mode flags
1393 // Use != so that we can still detect a change if the counter wraps
1394 if (rcModeActivationMask
!= blackboxLastFlightModeFlags
) {
1395 eventData
.lastFlags
= blackboxLastFlightModeFlags
;
1396 blackboxLastFlightModeFlags
= rcModeActivationMask
;
1397 eventData
.flags
= rcModeActivationMask
;
1399 blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE
, (flightLogEventData_t
*) &eventData
);
1404 * Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control
1405 * the portion of logged loop iterations.
1407 static bool blackboxShouldLogPFrame(uint32_t pFrameIndex
)
1409 /* Adding a magic shift of "blackboxConfig()->rate_num - 1" in here creates a better spread of
1410 * recorded / skipped frames when the I frame's position is considered:
1412 return (pFrameIndex
+ blackboxConfig()->rate_num
- 1) % blackboxConfig()->rate_denom
< blackboxConfig()->rate_num
;
1415 static bool blackboxShouldLogIFrame() {
1416 return blackboxPFrameIndex
== 0;
1419 // Called once every FC loop in order to keep track of how many FC loop iterations have passed
1420 static void blackboxAdvanceIterationTimers()
1422 blackboxSlowFrameIterationTimer
++;
1423 blackboxIteration
++;
1424 blackboxPFrameIndex
++;
1426 if (blackboxPFrameIndex
== BLACKBOX_I_INTERVAL
) {
1427 blackboxPFrameIndex
= 0;
1428 blackboxIFrameIndex
++;
1432 // Called once every FC loop in order to log the current state
1433 static void blackboxLogIteration(timeUs_t currentTimeUs
)
1435 // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
1436 if (blackboxShouldLogIFrame()) {
1438 * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
1439 * an additional item to write at the same time). Unless we're *only* logging "I" frames, then we have no choice.
1441 writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes());
1443 loadMainState(currentTimeUs
);
1446 blackboxCheckAndLogArmingBeep();
1447 blackboxCheckAndLogFlightMode(); // Check for FlightMode status change event
1449 if (blackboxShouldLogPFrame(blackboxPFrameIndex
)) {
1451 * We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
1452 * So only log slow frames during loop iterations where we log a main frame.
1454 writeSlowFrameIfNeeded(true);
1456 loadMainState(currentTimeUs
);
1460 if (feature(FEATURE_GPS
)) {
1462 * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the
1463 * GPS home position.
1465 * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
1466 * still be interpreted correctly.
1468 if (GPS_home
[0] != gpsHistory
.GPS_home
[0] || GPS_home
[1] != gpsHistory
.GPS_home
[1]
1469 || (blackboxPFrameIndex
== BLACKBOX_I_INTERVAL
/ 2 && blackboxIFrameIndex
% 128 == 0)) {
1471 writeGPSHomeFrame();
1472 writeGPSFrame(currentTimeUs
);
1473 } else if (GPS_numSat
!= gpsHistory
.GPS_numSat
|| GPS_coord
[0] != gpsHistory
.GPS_coord
[0]
1474 || GPS_coord
[1] != gpsHistory
.GPS_coord
[1]) {
1475 //We could check for velocity changes as well but I doubt it changes independent of position
1476 writeGPSFrame(currentTimeUs
);
1482 //Flush every iteration so that our runtime variance is minimized
1483 blackboxDeviceFlush();
1487 * Call each flight loop iteration to perform blackbox logging.
1489 void handleBlackbox(timeUs_t currentTimeUs
)
1493 switch (blackboxState
) {
1494 case BLACKBOX_STATE_STOPPED
:
1495 if (ARMING_FLAG(ARMED
)) {
1500 if (IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE
)) {
1501 blackboxSetState(BLACKBOX_STATE_START_ERASE
);
1505 case BLACKBOX_STATE_PREPARE_LOG_FILE
:
1506 if (blackboxDeviceBeginLog()) {
1507 blackboxSetState(BLACKBOX_STATE_SEND_HEADER
);
1510 case BLACKBOX_STATE_SEND_HEADER
:
1511 blackboxReplenishHeaderBudget();
1512 //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
1515 * Once the UART has had time to init, transmit the header in chunks so we don't overflow its transmit
1516 * buffer, overflow the OpenLog's buffer, or keep the main loop busy for too long.
1518 if (millis() > xmitState
.u
.startTime
+ 100) {
1519 if (blackboxDeviceReserveBufferSpace(BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION
) == BLACKBOX_RESERVE_SUCCESS
) {
1520 for (i
= 0; i
< BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION
&& blackboxHeader
[xmitState
.headerIndex
] != '\0'; i
++, xmitState
.headerIndex
++) {
1521 blackboxWrite(blackboxHeader
[xmitState
.headerIndex
]);
1522 blackboxHeaderBudget
--;
1525 if (blackboxHeader
[xmitState
.headerIndex
] == '\0') {
1526 blackboxSetState(BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER
);
1531 case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER
:
1532 blackboxReplenishHeaderBudget();
1533 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1534 if (!sendFieldDefinition('I', 'P', blackboxMainFields
, blackboxMainFields
+ 1, ARRAY_LENGTH(blackboxMainFields
),
1535 &blackboxMainFields
[0].condition
, &blackboxMainFields
[1].condition
)) {
1537 if (feature(FEATURE_GPS
)) {
1538 blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADER
);
1541 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER
);
1545 case BLACKBOX_STATE_SEND_GPS_H_HEADER
:
1546 blackboxReplenishHeaderBudget();
1547 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1548 if (!sendFieldDefinition('H', 0, blackboxGpsHFields
, blackboxGpsHFields
+ 1, ARRAY_LENGTH(blackboxGpsHFields
),
1550 blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADER
);
1553 case BLACKBOX_STATE_SEND_GPS_G_HEADER
:
1554 blackboxReplenishHeaderBudget();
1555 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1556 if (!sendFieldDefinition('G', 0, blackboxGpsGFields
, blackboxGpsGFields
+ 1, ARRAY_LENGTH(blackboxGpsGFields
),
1557 &blackboxGpsGFields
[0].condition
, &blackboxGpsGFields
[1].condition
)) {
1558 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER
);
1562 case BLACKBOX_STATE_SEND_SLOW_HEADER
:
1563 blackboxReplenishHeaderBudget();
1564 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1565 if (!sendFieldDefinition('S', 0, blackboxSlowFields
, blackboxSlowFields
+ 1, ARRAY_LENGTH(blackboxSlowFields
),
1567 blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO
);
1570 case BLACKBOX_STATE_SEND_SYSINFO
:
1571 blackboxReplenishHeaderBudget();
1572 //On entry of this state, xmitState.headerIndex is 0
1574 //Keep writing chunks of the system info headers until it returns true to signal completion
1575 if (blackboxWriteSysinfo()) {
1578 * Wait for header buffers to drain completely before data logging begins to ensure reliable header delivery
1579 * (overflowing circular buffers causes all data to be discarded, so the first few logged iterations
1580 * could wipe out the end of the header if we weren't careful)
1582 if (blackboxDeviceFlushForce()) {
1583 blackboxSetState(BLACKBOX_STATE_RUNNING
);
1587 case BLACKBOX_STATE_PAUSED
:
1588 // Only allow resume to occur during an I-frame iteration, so that we have an "I" base to work from
1589 if (IS_RC_MODE_ACTIVE(BOXBLACKBOX
) && blackboxShouldLogIFrame()) {
1590 // Write a log entry so the decoder is aware that our large time/iteration skip is intended
1591 flightLogEvent_loggingResume_t resume
;
1593 resume
.logIteration
= blackboxIteration
;
1594 resume
.currentTime
= currentTimeUs
;
1596 blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME
, (flightLogEventData_t
*) &resume
);
1597 blackboxSetState(BLACKBOX_STATE_RUNNING
);
1599 blackboxLogIteration(currentTimeUs
);
1601 // Keep the logging timers ticking so our log iteration continues to advance
1602 blackboxAdvanceIterationTimers();
1604 case BLACKBOX_STATE_RUNNING
:
1605 // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
1606 // Prevent the Pausing of the log on the mode switch if in Motor Test Mode
1607 if (blackboxModeActivationConditionPresent
&& !IS_RC_MODE_ACTIVE(BOXBLACKBOX
) && !startedLoggingInTestMode
) {
1608 blackboxSetState(BLACKBOX_STATE_PAUSED
);
1610 blackboxLogIteration(currentTimeUs
);
1612 blackboxAdvanceIterationTimers();
1614 case BLACKBOX_STATE_SHUTTING_DOWN
:
1615 //On entry of this state, startTime is set
1618 * Wait for the log we've transmitted to make its way to the logger before we release the serial port,
1619 * since releasing the port clears the Tx buffer.
1621 * Don't wait longer than it could possibly take if something funky happens.
1623 if (blackboxDeviceEndLog(blackboxLoggedAnyFrames
) && (millis() > xmitState
.u
.startTime
+ BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS
|| blackboxDeviceFlushForce())) {
1624 blackboxDeviceClose();
1625 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1629 case BLACKBOX_STATE_START_ERASE
:
1631 blackboxSetState(BLACKBOX_STATE_ERASING
);
1632 beeper(BEEPER_BLACKBOX_ERASE
);
1634 case BLACKBOX_STATE_ERASING
:
1635 if (isBlackboxErased()) {
1637 blackboxSetState(BLACKBOX_STATE_ERASED
);
1638 beeper(BEEPER_BLACKBOX_ERASE
);
1641 case BLACKBOX_STATE_ERASED
:
1642 if (!IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE
)) {
1643 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1651 // Did we run out of room on the device? Stop!
1652 if (isBlackboxDeviceFull()) {
1654 if (blackboxState
!= BLACKBOX_STATE_ERASING
1655 && blackboxState
!= BLACKBOX_STATE_START_ERASE
1656 && blackboxState
!= BLACKBOX_STATE_ERASED
) {
1658 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1659 // ensure we reset the test mode flag if we stop due to full memory card
1660 if (startedLoggingInTestMode
) startedLoggingInTestMode
= false;
1664 } else { // Only log in test mode if there is room!
1665 if(blackboxConfig()->on_motor_test
) {
1666 // Handle Motor Test Mode
1667 if(inMotorTestMode()) {
1668 if(blackboxState
==BLACKBOX_STATE_STOPPED
)
1671 if(blackboxState
!=BLACKBOX_STATE_STOPPED
)
1678 static bool canUseBlackboxWithCurrentConfiguration(void)
1681 return feature(FEATURE_BLACKBOX
) &&
1682 !(blackboxConfig()->device
== BLACKBOX_DEVICE_SDCARD
&& !feature(FEATURE_SDCARD
));
1684 return feature(FEATURE_BLACKBOX
);
1689 * Call during system startup to initialize the blackbox.
1691 void initBlackbox(void)
1693 if (canUseBlackboxWithCurrentConfiguration()) {
1694 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1696 blackboxSetState(BLACKBOX_STATE_DISABLED
);