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/>.
26 #include "common/maths.h"
27 #include "common/axis.h"
28 #include "common/color.h"
29 #include "common/encoding.h"
30 #include "common/utils.h"
32 #include "drivers/gpio.h"
33 #include "drivers/sensor.h"
34 #include "drivers/system.h"
35 #include "drivers/serial.h"
36 #include "drivers/compass.h"
37 #include "drivers/timer.h"
38 #include "drivers/pwm_rx.h"
39 #include "drivers/accgyro.h"
40 #include "drivers/light_led.h"
41 #include "drivers/sound_beeper.h"
43 #include "sensors/sensors.h"
44 #include "sensors/boardalignment.h"
45 #include "sensors/sonar.h"
46 #include "sensors/compass.h"
47 #include "sensors/acceleration.h"
48 #include "sensors/barometer.h"
49 #include "sensors/gyro.h"
50 #include "sensors/battery.h"
52 #include "io/beeper.h"
53 #include "io/display.h"
54 #include "io/escservo.h"
55 #include "io/rc_controls.h"
56 #include "io/gimbal.h"
58 #include "io/ledstrip.h"
59 #include "io/serial.h"
60 #include "io/serial_cli.h"
61 #include "io/serial_msp.h"
62 #include "io/statusindicator.h"
67 #include "telemetry/telemetry.h"
69 #include "flight/mixer.h"
70 #include "flight/altitudehold.h"
71 #include "flight/failsafe.h"
72 #include "flight/imu.h"
73 #include "flight/navigation.h"
75 #include "config/runtime_config.h"
76 #include "config/config.h"
77 #include "config/config_profile.h"
78 #include "config/config_master.h"
81 #include "blackbox_io.h"
83 #define BLACKBOX_I_INTERVAL 32
84 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
86 #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
88 #define STATIC_ASSERT(condition, name ) \
89 typedef char assert_failed_ ## name [(condition) ? 1 : -1 ]
91 // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
93 #define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
94 #define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)
95 #define CONDITION(x) CONCAT(FLIGHT_LOG_FIELD_CONDITION_, x)
96 #define UNSIGNED FLIGHT_LOG_FIELD_UNSIGNED
97 #define SIGNED FLIGHT_LOG_FIELD_SIGNED
99 static const char blackboxHeader
[] =
100 "H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
101 "H Blackbox version:1\n"
103 "H I interval:" STR(BLACKBOX_I_INTERVAL
) "\n";
105 static const char* const blackboxMainHeaderNames
[] = {
115 static const char* const blackboxGPSGHeaderNames
[] = {
122 static const char* const blackboxGPSHHeaderNames
[] = {
130 /* All field definition structs should look like this (but with longer arrs): */
131 typedef struct blackboxFieldDefinition_t
{
133 // If the field name has a number to be included in square brackets [1] afterwards, set it here, or -1 for no brackets:
134 int8_t fieldNameIndex
;
136 // Each member of this array will be the value to print for this field for the given header index
138 } blackboxFieldDefinition_t
;
140 typedef struct blackboxMainFieldDefinition_t
{
142 int8_t fieldNameIndex
;
149 uint8_t condition
; // Decide whether this field should appear in the log
150 } blackboxMainFieldDefinition_t
;
152 typedef struct blackboxGPSFieldDefinition_t
{
154 int8_t fieldNameIndex
;
159 uint8_t condition
; // Decide whether this field should appear in the log
160 } blackboxGPSFieldDefinition_t
;
163 * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
164 * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
165 * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
166 * the encoding we've promised here).
168 static const blackboxMainFieldDefinition_t blackboxMainFields
[] = {
169 /* loopIteration doesn't appear in P frames since it always increments */
170 {"loopIteration",-1, UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(INC
), .Pencode
= FLIGHT_LOG_FIELD_ENCODING_NULL
, CONDITION(ALWAYS
)},
171 /* Time advances pretty steadily so the P-frame prediction is a straight line */
172 {"time", -1, UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(STRAIGHT_LINE
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
173 {"axisP", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
174 {"axisP", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
175 {"axisP", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
176 /* I terms get special packed encoding in P frames: */
177 {"axisI", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
178 {"axisI", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
179 {"axisI", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
180 {"axisD", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_0
)},
181 {"axisD", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_1
)},
182 {"axisD", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_2
)},
183 /* rcCommands are encoded together as a group in P-frames: */
184 {"rcCommand", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
185 {"rcCommand", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
186 {"rcCommand", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
187 /* Throttle is always in the range [minthrottle..maxthrottle]: */
188 {"rcCommand", 3, UNSIGNED
, .Ipredict
= PREDICT(MINTHROTTLE
), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
190 {"vbatLatest", -1, UNSIGNED
, .Ipredict
= PREDICT(VBATREF
), .Iencode
= ENCODING(NEG_14BIT
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_VBAT
},
191 {"amperageLatest",-1, UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE
},
194 {"magADC", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
195 {"magADC", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
196 {"magADC", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
199 {"BaroAlt", -1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_BARO
},
202 /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
203 {"gyroData", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
204 {"gyroData", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
205 {"gyroData", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
206 {"accSmooth", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
207 {"accSmooth", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
208 {"accSmooth", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
209 /* 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): */
210 {"motor", 0, UNSIGNED
, .Ipredict
= PREDICT(MINTHROTTLE
), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_1
)},
211 /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
212 {"motor", 1, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_2
)},
213 {"motor", 2, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_3
)},
214 {"motor", 3, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_4
)},
215 {"motor", 4, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_5
)},
216 {"motor", 5, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_6
)},
217 {"motor", 6, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_7
)},
218 {"motor", 7, UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_8
)},
220 /* Tricopter tail servo */
221 {"servo", 5, UNSIGNED
, .Ipredict
= PREDICT(1500), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(TRICOPTER
)}
225 // GPS position/vel frame
226 static const blackboxGPSFieldDefinition_t blackboxGpsGFields
[] = {
227 {"time", -1, UNSIGNED
, PREDICT(LAST_MAIN_FRAME_TIME
), ENCODING(UNSIGNED_VB
), CONDITION(NOT_LOGGING_EVERY_FRAME
)},
228 {"GPS_numSat", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)},
229 {"GPS_coord", 0, SIGNED
, PREDICT(HOME_COORD
), ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
230 {"GPS_coord", 1, SIGNED
, PREDICT(HOME_COORD
), ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
231 {"GPS_altitude", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)},
232 {"GPS_speed", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)},
233 {"GPS_ground_course", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)}
237 static const blackboxGPSFieldDefinition_t blackboxGpsHFields
[] = {
238 {"GPS_home", 0, SIGNED
, PREDICT(0), ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
239 {"GPS_home", 1, SIGNED
, PREDICT(0), ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)}
243 typedef enum BlackboxState
{
244 BLACKBOX_STATE_DISABLED
= 0,
245 BLACKBOX_STATE_STOPPED
,
246 BLACKBOX_STATE_SEND_HEADER
,
247 BLACKBOX_STATE_SEND_FIELDINFO
,
248 BLACKBOX_STATE_SEND_GPS_H_HEADERS
,
249 BLACKBOX_STATE_SEND_GPS_G_HEADERS
,
250 BLACKBOX_STATE_SEND_SYSINFO
,
251 BLACKBOX_STATE_PRERUN
,
252 BLACKBOX_STATE_RUNNING
,
253 BLACKBOX_STATE_SHUTTING_DOWN
256 typedef struct gpsState_t
{
257 int32_t GPS_home
[2], GPS_coord
[2];
262 extern uint8_t motorCount
;
265 extern uint32_t currentTime
;
267 static BlackboxState blackboxState
= BLACKBOX_STATE_DISABLED
;
270 uint32_t headerIndex
;
272 /* Since these fields are used during different blackbox states (never simultaneously) we can
273 * overlap them to save on RAM
282 // Cache for FLIGHT_LOG_FIELD_CONDITION_* test results:
283 static uint32_t blackboxConditionCache
;
285 STATIC_ASSERT((sizeof(blackboxConditionCache
) * 8) >= FLIGHT_LOG_FIELD_CONDITION_NEVER
, too_many_flight_log_conditions
);
287 static uint32_t blackboxIteration
;
288 static uint32_t blackboxPFrameIndex
, blackboxIFrameIndex
;
291 * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
292 * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs
295 static uint16_t vbatReference
;
297 static gpsState_t gpsHistory
;
299 // Keep a history of length 2, plus a buffer for MW to store the new values into
300 static blackboxValues_t blackboxHistoryRing
[3];
302 // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
303 static blackboxValues_t
* blackboxHistory
[3];
305 static bool testBlackboxConditionUncached(FlightLogFieldCondition condition
)
308 case FLIGHT_LOG_FIELD_CONDITION_ALWAYS
:
311 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1
:
312 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2
:
313 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3
:
314 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4
:
315 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5
:
316 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6
:
317 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7
:
318 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8
:
319 return motorCount
>= condition
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1
+ 1;
321 case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
:
322 return masterConfig
.mixerMode
== MIXER_TRI
;
324 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
:
325 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1
:
326 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2
:
327 return currentProfile
->pidProfile
.D8
[condition
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
] != 0;
329 case FLIGHT_LOG_FIELD_CONDITION_MAG
:
331 return sensors(SENSOR_MAG
);
336 case FLIGHT_LOG_FIELD_CONDITION_BARO
:
338 return sensors(SENSOR_BARO
);
343 case FLIGHT_LOG_FIELD_CONDITION_VBAT
:
344 return feature(FEATURE_VBAT
);
346 case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE
:
347 return feature(FEATURE_CURRENT_METER
);
349 case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME
:
350 return masterConfig
.blackbox_rate_num
< masterConfig
.blackbox_rate_denom
;
352 case FLIGHT_LOG_FIELD_CONDITION_NEVER
:
359 static void blackboxBuildConditionCache()
361 FlightLogFieldCondition cond
;
363 blackboxConditionCache
= 0;
365 for (cond
= FLIGHT_LOG_FIELD_CONDITION_FIRST
; cond
<= FLIGHT_LOG_FIELD_CONDITION_LAST
; cond
++) {
366 if (testBlackboxConditionUncached(cond
)) {
367 blackboxConditionCache
|= 1 << cond
;
372 static bool testBlackboxCondition(FlightLogFieldCondition condition
)
374 return (blackboxConditionCache
& (1 << condition
)) != 0;
377 static void blackboxSetState(BlackboxState newState
)
379 //Perform initial setup required for the new state
381 case BLACKBOX_STATE_SEND_HEADER
:
382 xmitState
.headerIndex
= 0;
383 xmitState
.u
.startTime
= millis();
385 case BLACKBOX_STATE_SEND_FIELDINFO
:
386 case BLACKBOX_STATE_SEND_GPS_G_HEADERS
:
387 case BLACKBOX_STATE_SEND_GPS_H_HEADERS
:
388 xmitState
.headerIndex
= 0;
389 xmitState
.u
.fieldIndex
= -1;
391 case BLACKBOX_STATE_SEND_SYSINFO
:
392 xmitState
.headerIndex
= 0;
394 case BLACKBOX_STATE_RUNNING
:
395 blackboxIteration
= 0;
396 blackboxPFrameIndex
= 0;
397 blackboxIFrameIndex
= 0;
399 case BLACKBOX_STATE_SHUTTING_DOWN
:
400 xmitState
.u
.startTime
= millis();
401 blackboxDeviceFlush();
406 blackboxState
= newState
;
409 static void writeIntraframe(void)
411 blackboxValues_t
*blackboxCurrent
= blackboxHistory
[0];
416 blackboxWriteUnsignedVB(blackboxIteration
);
417 blackboxWriteUnsignedVB(blackboxCurrent
->time
);
419 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
420 blackboxWriteSignedVB(blackboxCurrent
->axisPID_P
[x
]);
423 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
424 blackboxWriteSignedVB(blackboxCurrent
->axisPID_I
[x
]);
427 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
428 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
+ x
)) {
429 blackboxWriteSignedVB(blackboxCurrent
->axisPID_D
[x
]);
433 for (x
= 0; x
< 3; x
++) {
434 blackboxWriteSignedVB(blackboxCurrent
->rcCommand
[x
]);
437 blackboxWriteUnsignedVB(blackboxCurrent
->rcCommand
[3] - masterConfig
.escAndServoConfig
.minthrottle
); //Throttle lies in range [minthrottle..maxthrottle]
439 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT
)) {
441 * Our voltage is expected to decrease over the course of the flight, so store our difference from
444 * Write 14 bits even if the number is negative (which would otherwise result in 32 bits)
446 blackboxWriteUnsignedVB((vbatReference
- blackboxCurrent
->vbatLatest
) & 0x3FFF);
449 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE
)) {
450 // 12bit value directly from ADC
451 blackboxWriteUnsignedVB(blackboxCurrent
->amperageLatest
);
455 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG
)) {
456 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
457 blackboxWriteSignedVB(blackboxCurrent
->magADC
[x
]);
463 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO
)) {
464 blackboxWriteSignedVB(blackboxCurrent
->BaroAlt
);
468 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
469 blackboxWriteSignedVB(blackboxCurrent
->gyroData
[x
]);
472 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
473 blackboxWriteSignedVB(blackboxCurrent
->accSmooth
[x
]);
476 //Motors can be below minthrottle when disarmed, but that doesn't happen much
477 blackboxWriteUnsignedVB(blackboxCurrent
->motor
[0] - masterConfig
.escAndServoConfig
.minthrottle
);
479 //Motors tend to be similar to each other
480 for (x
= 1; x
< motorCount
; x
++) {
481 blackboxWriteSignedVB(blackboxCurrent
->motor
[x
] - blackboxCurrent
->motor
[0]);
484 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
)) {
485 blackboxWriteSignedVB(blackboxHistory
[0]->servo
[5] - 1500);
488 //Rotate our history buffers:
490 //The current state becomes the new "before" state
491 blackboxHistory
[1] = blackboxHistory
[0];
492 //And since we have no other history, we also use it for the "before, before" state
493 blackboxHistory
[2] = blackboxHistory
[0];
494 //And advance the current state over to a blank space ready to be filled
495 blackboxHistory
[0] = ((blackboxHistory
[0] - blackboxHistoryRing
+ 1) % 3) + blackboxHistoryRing
;
498 static void writeInterframe(void)
503 blackboxValues_t
*blackboxCurrent
= blackboxHistory
[0];
504 blackboxValues_t
*blackboxLast
= blackboxHistory
[1];
508 //No need to store iteration count since its delta is always 1
511 * Since the difference between the difference between successive times will be nearly zero (due to consistent
512 * looptime spacing), use second-order differences.
514 blackboxWriteSignedVB((int32_t) (blackboxHistory
[0]->time
- 2 * blackboxHistory
[1]->time
+ blackboxHistory
[2]->time
));
516 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
517 blackboxWriteSignedVB(blackboxCurrent
->axisPID_P
[x
] - blackboxLast
->axisPID_P
[x
]);
520 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
521 deltas
[x
] = blackboxCurrent
->axisPID_I
[x
] - blackboxLast
->axisPID_I
[x
];
525 * The PID I field changes very slowly, most of the time +-2, so use an encoding
526 * that can pack all three fields into one byte in that situation.
528 blackboxWriteTag2_3S32(deltas
);
531 * The PID D term is frequently set to zero for yaw, which makes the result from the calculation
532 * always zero. So don't bother recording D results when PID D terms are zero.
534 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
535 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
+ x
)) {
536 blackboxWriteSignedVB(blackboxCurrent
->axisPID_D
[x
] - blackboxLast
->axisPID_D
[x
]);
541 * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
542 * can pack multiple values per byte:
544 for (x
= 0; x
< 4; x
++) {
545 deltas
[x
] = blackboxCurrent
->rcCommand
[x
] - blackboxLast
->rcCommand
[x
];
548 blackboxWriteTag8_4S16(deltas
);
550 //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, Amperage, MAG, BARO
551 int optionalFieldCount
= 0;
553 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT
)) {
554 deltas
[optionalFieldCount
++] = (int32_t) blackboxCurrent
->vbatLatest
- blackboxLast
->vbatLatest
;
557 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE
)) {
558 deltas
[optionalFieldCount
++] = (int32_t) blackboxCurrent
->amperageLatest
- blackboxLast
->amperageLatest
;
562 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG
)) {
563 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
564 deltas
[optionalFieldCount
++] = blackboxCurrent
->magADC
[x
] - blackboxLast
->magADC
[x
];
570 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO
)) {
571 deltas
[optionalFieldCount
++] = blackboxCurrent
->BaroAlt
- blackboxLast
->BaroAlt
;
574 blackboxWriteTag8_8SVB(deltas
, optionalFieldCount
);
576 //Since gyros, accs and motors are noisy, base the prediction on the average of the history:
577 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
578 blackboxWriteSignedVB(blackboxHistory
[0]->gyroData
[x
] - (blackboxHistory
[1]->gyroData
[x
] + blackboxHistory
[2]->gyroData
[x
]) / 2);
581 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
582 blackboxWriteSignedVB(blackboxHistory
[0]->accSmooth
[x
] - (blackboxHistory
[1]->accSmooth
[x
] + blackboxHistory
[2]->accSmooth
[x
]) / 2);
585 for (x
= 0; x
< motorCount
; x
++) {
586 blackboxWriteSignedVB(blackboxHistory
[0]->motor
[x
] - (blackboxHistory
[1]->motor
[x
] + blackboxHistory
[2]->motor
[x
]) / 2);
589 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
)) {
590 blackboxWriteSignedVB(blackboxCurrent
->servo
[5] - blackboxLast
->servo
[5]);
593 //Rotate our history buffers
594 blackboxHistory
[2] = blackboxHistory
[1];
595 blackboxHistory
[1] = blackboxHistory
[0];
596 blackboxHistory
[0] = ((blackboxHistory
[0] - blackboxHistoryRing
+ 1) % 3) + blackboxHistoryRing
;
599 static int gcd(int num
, int denom
)
605 return gcd(denom
, num
% denom
);
608 static void validateBlackboxConfig()
612 if (masterConfig
.blackbox_rate_num
== 0 || masterConfig
.blackbox_rate_denom
== 0
613 || masterConfig
.blackbox_rate_num
>= masterConfig
.blackbox_rate_denom
) {
614 masterConfig
.blackbox_rate_num
= 1;
615 masterConfig
.blackbox_rate_denom
= 1;
617 /* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
618 * itself more frequently)
620 div
= gcd(masterConfig
.blackbox_rate_num
, masterConfig
.blackbox_rate_denom
);
622 masterConfig
.blackbox_rate_num
/= div
;
623 masterConfig
.blackbox_rate_denom
/= div
;
626 if (masterConfig
.blackbox_device
>= BLACKBOX_DEVICE_END
) {
627 masterConfig
.blackbox_device
= BLACKBOX_DEVICE_SERIAL
;
632 * Start Blackbox logging if it is not already running. Intended to be called upon arming.
634 void startBlackbox(void)
636 if (blackboxState
== BLACKBOX_STATE_STOPPED
) {
637 validateBlackboxConfig();
639 if (!blackboxDeviceOpen()) {
640 blackboxSetState(BLACKBOX_STATE_DISABLED
);
644 memset(&gpsHistory
, 0, sizeof(gpsHistory
));
646 blackboxHistory
[0] = &blackboxHistoryRing
[0];
647 blackboxHistory
[1] = &blackboxHistoryRing
[1];
648 blackboxHistory
[2] = &blackboxHistoryRing
[2];
650 vbatReference
= vbatLatestADC
;
652 //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
655 * We use conditional tests to decide whether or not certain fields should be logged. Since our headers
656 * must always agree with the logged data, the results of these tests must not change during logging. So
659 blackboxBuildConditionCache();
661 blackboxSetState(BLACKBOX_STATE_SEND_HEADER
);
666 * Begin Blackbox shutdown.
668 void finishBlackbox(void)
670 if (blackboxState
== BLACKBOX_STATE_RUNNING
) {
671 blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END
, NULL
);
673 blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN
);
674 } else if (blackboxState
!= BLACKBOX_STATE_DISABLED
&& blackboxState
!= BLACKBOX_STATE_STOPPED
675 && blackboxState
!= BLACKBOX_STATE_SHUTTING_DOWN
) {
677 * We're shutting down in the middle of transmitting headers, so we can't log a "log completed" event.
678 * Just give the port back and stop immediately.
680 blackboxDeviceClose();
681 blackboxSetState(BLACKBOX_STATE_STOPPED
);
686 static void writeGPSHomeFrame()
690 blackboxWriteSignedVB(GPS_home
[0]);
691 blackboxWriteSignedVB(GPS_home
[1]);
692 //TODO it'd be great if we could grab the GPS current time and write that too
694 gpsHistory
.GPS_home
[0] = GPS_home
[0];
695 gpsHistory
.GPS_home
[1] = GPS_home
[1];
698 static void writeGPSFrame()
703 * If we're logging every frame, then a GPS frame always appears just after a frame with the
704 * currentTime timestamp in the log, so the reader can just use that timestamp for the GPS frame.
706 * If we're not logging every frame, we need to store the time of this GPS frame.
708 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME
)) {
709 // Predict the time of the last frame in the main log
710 blackboxWriteUnsignedVB(currentTime
- blackboxHistory
[1]->time
);
713 blackboxWriteUnsignedVB(GPS_numSat
);
714 blackboxWriteSignedVB(GPS_coord
[0] - gpsHistory
.GPS_home
[0]);
715 blackboxWriteSignedVB(GPS_coord
[1] - gpsHistory
.GPS_home
[1]);
716 blackboxWriteUnsignedVB(GPS_altitude
);
717 blackboxWriteUnsignedVB(GPS_speed
);
718 blackboxWriteUnsignedVB(GPS_ground_course
);
720 gpsHistory
.GPS_numSat
= GPS_numSat
;
721 gpsHistory
.GPS_coord
[0] = GPS_coord
[0];
722 gpsHistory
.GPS_coord
[1] = GPS_coord
[1];
727 * Fill the current state of the blackbox using values read from the flight controller
729 static void loadBlackboxState(void)
731 blackboxValues_t
*blackboxCurrent
= blackboxHistory
[0];
734 blackboxCurrent
->time
= currentTime
;
736 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
737 blackboxCurrent
->axisPID_P
[i
] = axisPID_P
[i
];
739 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
740 blackboxCurrent
->axisPID_I
[i
] = axisPID_I
[i
];
742 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
743 blackboxCurrent
->axisPID_D
[i
] = axisPID_D
[i
];
746 for (i
= 0; i
< 4; i
++) {
747 blackboxCurrent
->rcCommand
[i
] = rcCommand
[i
];
750 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
751 blackboxCurrent
->gyroData
[i
] = gyroData
[i
];
754 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
755 blackboxCurrent
->accSmooth
[i
] = accSmooth
[i
];
758 for (i
= 0; i
< motorCount
; i
++) {
759 blackboxCurrent
->motor
[i
] = motor
[i
];
762 blackboxCurrent
->vbatLatest
= vbatLatestADC
;
763 blackboxCurrent
->amperageLatest
= amperageLatestADC
;
766 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
767 blackboxCurrent
->magADC
[i
] = magADC
[i
];
772 blackboxCurrent
->BaroAlt
= BaroAlt
;
776 //Tail servo for tricopters
777 blackboxCurrent
->servo
[5] = servo
[5];
782 * Transmit the header information for the given field definitions. Transmitted header lines look like:
784 * H Field I name:a,b,c
785 * H Field I predictor:0,1,2
787 * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
788 * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
790 * Set xmitState.headerIndex to 0 and xmitState.u.fieldIndex to -1 before calling for the first time.
792 * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the
793 * fieldDefinition and secondCondition arrays.
795 * Returns true if there is still header left to transmit (so call again to continue transmission).
797 static bool sendFieldDefinition(const char * const *headerNames
, unsigned int headerCount
, const void *fieldDefinitions
,
798 const void *secondFieldDefinition
, int fieldCount
, const uint8_t *conditions
, const uint8_t *secondCondition
)
800 const blackboxFieldDefinition_t
*def
;
802 static bool needComma
= false;
803 size_t definitionStride
= (char*) secondFieldDefinition
- (char*) fieldDefinitions
;
804 size_t conditionsStride
= (char*) secondCondition
- (char*) conditions
;
807 * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit
810 if (xmitState
.u
.fieldIndex
== -1) {
811 if (xmitState
.headerIndex
>= headerCount
) {
812 return false; //Someone probably called us again after we had already completed transmission
815 charsWritten
= blackboxPrint("H Field ");
816 charsWritten
+= blackboxPrint(headerNames
[xmitState
.headerIndex
]);
820 xmitState
.u
.fieldIndex
++;
825 for (; xmitState
.u
.fieldIndex
< fieldCount
&& charsWritten
< blackboxWriteChunkSize
; xmitState
.u
.fieldIndex
++) {
826 def
= (const blackboxFieldDefinition_t
*) ((const char*)fieldDefinitions
+ definitionStride
* xmitState
.u
.fieldIndex
);
828 if (!conditions
|| testBlackboxCondition(conditions
[conditionsStride
* xmitState
.u
.fieldIndex
])) {
836 // The first header is a field name
837 if (xmitState
.headerIndex
== 0) {
838 charsWritten
+= blackboxPrint(def
->name
);
840 // Do we need to print an index in brackets after the name?
841 if (def
->fieldNameIndex
!= -1) {
843 // Assume the field index is a single digit:
844 blackboxWrite(def
->fieldNameIndex
+ '0');
849 //The other headers are integers
850 if (def
->arr
[xmitState
.headerIndex
- 1] >= 10) {
851 blackboxWrite(def
->arr
[xmitState
.headerIndex
- 1] / 10 + '0');
852 blackboxWrite(def
->arr
[xmitState
.headerIndex
- 1] % 10 + '0');
855 blackboxWrite(def
->arr
[xmitState
.headerIndex
- 1] + '0');
862 // Did we complete this line?
863 if (xmitState
.u
.fieldIndex
== fieldCount
) {
865 xmitState
.headerIndex
++;
866 xmitState
.u
.fieldIndex
= -1;
869 return xmitState
.headerIndex
< headerCount
;
873 * Transmit a portion of the system information headers. Call the first time with xmitState.headerIndex == 0. Returns
874 * true iff transmission is complete, otherwise call again later to continue transmission.
876 static bool blackboxWriteSysinfo()
878 if (xmitState
.headerIndex
== 0) {
879 xmitState
.u
.serialBudget
= 0;
880 xmitState
.headerIndex
= 1;
883 // How many bytes can we afford to transmit this loop?
884 xmitState
.u
.serialBudget
= MIN(xmitState
.u
.serialBudget
+ blackboxWriteChunkSize
, 64);
886 // Most headers will consume at least 20 bytes so wait until we've built up that much link budget
887 if (xmitState
.u
.serialBudget
< 20) {
891 switch (xmitState
.headerIndex
) {
893 //Shouldn't ever get here
896 xmitState
.u
.serialBudget
-= blackboxPrint("H Firmware type:Cleanflight\n");
899 blackboxPrintf("H Firmware revision:%s\n", shortGitRevision
);
901 xmitState
.u
.serialBudget
-= strlen("H Firmware revision:\n") + strlen(shortGitRevision
);
904 blackboxPrintf("H Firmware date:%s %s\n", buildDate
, buildTime
);
906 xmitState
.u
.serialBudget
-= strlen("H Firmware date: \n") + strlen(buildDate
) + strlen(buildTime
);
909 blackboxPrintf("H P interval:%d/%d\n", masterConfig
.blackbox_rate_num
, masterConfig
.blackbox_rate_denom
);
911 /* Don't need to be super exact about the budget so don't mind the fact that we're using the length of
912 * the placeholder "%d" instead of the actual integer size.
914 xmitState
.u
.serialBudget
-= strlen("H P interval:%d/%d\n");
917 blackboxPrintf("H rcRate:%d\n", masterConfig
.controlRateProfiles
[masterConfig
.current_profile_index
].rcRate8
);
919 xmitState
.u
.serialBudget
-= strlen("H rcRate:%d\n");
922 blackboxPrintf("H minthrottle:%d\n", masterConfig
.escAndServoConfig
.minthrottle
);
924 xmitState
.u
.serialBudget
-= strlen("H minthrottle:%d\n");
927 blackboxPrintf("H maxthrottle:%d\n", masterConfig
.escAndServoConfig
.maxthrottle
);
929 xmitState
.u
.serialBudget
-= strlen("H maxthrottle:%d\n");
932 blackboxPrintf("H gyro.scale:0x%x\n", castFloatBytesToInt(gyro
.scale
));
934 xmitState
.u
.serialBudget
-= strlen("H gyro.scale:0x\n") + 6;
937 blackboxPrintf("H acc_1G:%u\n", acc_1G
);
939 xmitState
.u
.serialBudget
-= strlen("H acc_1G:%u\n");
942 blackboxPrintf("H vbatscale:%u\n", masterConfig
.batteryConfig
.vbatscale
);
944 xmitState
.u
.serialBudget
-= strlen("H vbatscale:%u\n");
947 blackboxPrintf("H vbatcellvoltage:%u,%u,%u\n", masterConfig
.batteryConfig
.vbatmincellvoltage
,
948 masterConfig
.batteryConfig
.vbatwarningcellvoltage
, masterConfig
.batteryConfig
.vbatmaxcellvoltage
);
950 xmitState
.u
.serialBudget
-= strlen("H vbatcellvoltage:%u,%u,%u\n");
953 blackboxPrintf("H vbatref:%u\n", vbatReference
);
955 xmitState
.u
.serialBudget
-= strlen("H vbatref:%u\n");
958 blackboxPrintf("H currentMeter:%d,%d\n", masterConfig
.batteryConfig
.currentMeterOffset
, masterConfig
.batteryConfig
.currentMeterScale
);
960 xmitState
.u
.serialBudget
-= strlen("H currentMeter:%d,%d\n");
966 xmitState
.headerIndex
++;
971 * Write the given event to the log immediately
973 void blackboxLogEvent(FlightLogEvent event
, flightLogEventData_t
*data
)
975 if (blackboxState
!= BLACKBOX_STATE_RUNNING
) {
979 //Shared header for event frames
981 blackboxWrite(event
);
983 //Now serialize the data for this specific frame type
985 case FLIGHT_LOG_EVENT_SYNC_BEEP
:
986 blackboxWriteUnsignedVB(data
->syncBeep
.time
);
988 case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START
:
989 blackboxWrite(data
->autotuneCycleStart
.phase
);
990 blackboxWrite(data
->autotuneCycleStart
.cycle
| (data
->autotuneCycleStart
.rising
? 0x80 : 0));
991 blackboxWrite(data
->autotuneCycleStart
.p
);
992 blackboxWrite(data
->autotuneCycleStart
.i
);
993 blackboxWrite(data
->autotuneCycleStart
.d
);
995 case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT
:
996 blackboxWrite(data
->autotuneCycleResult
.flags
);
997 blackboxWrite(data
->autotuneCycleStart
.p
);
998 blackboxWrite(data
->autotuneCycleStart
.i
);
999 blackboxWrite(data
->autotuneCycleStart
.d
);
1001 case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS
:
1002 blackboxWriteS16(data
->autotuneTargets
.currentAngle
);
1003 blackboxWrite((uint8_t) data
->autotuneTargets
.targetAngle
);
1004 blackboxWrite((uint8_t) data
->autotuneTargets
.targetAngleAtPeak
);
1005 blackboxWriteS16(data
->autotuneTargets
.firstPeakAngle
);
1006 blackboxWriteS16(data
->autotuneTargets
.secondPeakAngle
);
1008 case FLIGHT_LOG_EVENT_LOG_END
:
1009 blackboxPrint("End of log");
1015 // Beep the buzzer and write the current time to the log as a synchronization point
1016 static void blackboxPlaySyncBeep()
1018 flightLogEvent_syncBeep_t eventData
;
1020 eventData
.time
= micros();
1023 * The regular beep routines aren't going to work for us, because they queue up the beep to be executed later.
1024 * Our beep is timing sensitive, so start beeping now without setting the beeperIsOn flag.
1028 // Have the regular beeper code turn off the beep for us eventually, since that's not timing-sensitive
1029 queueConfirmationBeep(1);
1031 blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP
, (flightLogEventData_t
*) &eventData
);
1035 * Call each flight loop iteration to perform blackbox logging.
1037 void handleBlackbox(void)
1041 switch (blackboxState
) {
1042 case BLACKBOX_STATE_SEND_HEADER
:
1043 //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
1046 * Once the UART has had time to init, transmit the header in chunks so we don't overflow our transmit
1049 if (millis() > xmitState
.u
.startTime
+ 100) {
1050 for (i
= 0; i
< blackboxWriteChunkSize
&& blackboxHeader
[xmitState
.headerIndex
] != '\0'; i
++, xmitState
.headerIndex
++) {
1051 blackboxWrite(blackboxHeader
[xmitState
.headerIndex
]);
1054 if (blackboxHeader
[xmitState
.headerIndex
] == '\0') {
1055 blackboxSetState(BLACKBOX_STATE_SEND_FIELDINFO
);
1059 case BLACKBOX_STATE_SEND_FIELDINFO
:
1060 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1061 if (!sendFieldDefinition(blackboxMainHeaderNames
, ARRAY_LENGTH(blackboxMainHeaderNames
), blackboxMainFields
, blackboxMainFields
+ 1,
1062 ARRAY_LENGTH(blackboxMainFields
), &blackboxMainFields
[0].condition
, &blackboxMainFields
[1].condition
)) {
1064 if (feature(FEATURE_GPS
)) {
1065 blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADERS
);
1068 blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO
);
1072 case BLACKBOX_STATE_SEND_GPS_H_HEADERS
:
1073 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1074 if (!sendFieldDefinition(blackboxGPSHHeaderNames
, ARRAY_LENGTH(blackboxGPSHHeaderNames
), blackboxGpsHFields
, blackboxGpsHFields
+ 1,
1075 ARRAY_LENGTH(blackboxGpsHFields
), &blackboxGpsHFields
[0].condition
, &blackboxGpsHFields
[1].condition
)) {
1076 blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADERS
);
1079 case BLACKBOX_STATE_SEND_GPS_G_HEADERS
:
1080 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1081 if (!sendFieldDefinition(blackboxGPSGHeaderNames
, ARRAY_LENGTH(blackboxGPSGHeaderNames
), blackboxGpsGFields
, blackboxGpsGFields
+ 1,
1082 ARRAY_LENGTH(blackboxGpsGFields
), &blackboxGpsGFields
[0].condition
, &blackboxGpsGFields
[1].condition
)) {
1083 blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO
);
1087 case BLACKBOX_STATE_SEND_SYSINFO
:
1088 //On entry of this state, xmitState.headerIndex is 0
1090 //Keep writing chunks of the system info headers until it returns true to signal completion
1091 if (blackboxWriteSysinfo()) {
1092 blackboxSetState(BLACKBOX_STATE_PRERUN
);
1095 case BLACKBOX_STATE_PRERUN
:
1096 blackboxSetState(BLACKBOX_STATE_RUNNING
);
1098 blackboxPlaySyncBeep();
1100 case BLACKBOX_STATE_RUNNING
:
1101 // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
1103 // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
1104 if (blackboxPFrameIndex
== 0) {
1105 // Copy current system values into the blackbox
1106 loadBlackboxState();
1109 /* Adding a magic shift of "masterConfig.blackbox_rate_num - 1" in here creates a better spread of
1110 * recorded / skipped frames when the I frame's position is considered:
1112 if ((blackboxPFrameIndex
+ masterConfig
.blackbox_rate_num
- 1) % masterConfig
.blackbox_rate_denom
< masterConfig
.blackbox_rate_num
) {
1113 loadBlackboxState();
1117 if (feature(FEATURE_GPS
)) {
1119 * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the
1120 * GPS home position.
1122 * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
1123 * still be interpreted correctly.
1125 if (GPS_home
[0] != gpsHistory
.GPS_home
[0] || GPS_home
[1] != gpsHistory
.GPS_home
[1]
1126 || (blackboxPFrameIndex
== BLACKBOX_I_INTERVAL
/ 2 && blackboxIFrameIndex
% 128 == 0)) {
1128 writeGPSHomeFrame();
1130 } else if (GPS_numSat
!= gpsHistory
.GPS_numSat
|| GPS_coord
[0] != gpsHistory
.GPS_coord
[0]
1131 || GPS_coord
[1] != gpsHistory
.GPS_coord
[1]) {
1132 //We could check for velocity changes as well but I doubt it changes independent of position
1139 blackboxIteration
++;
1140 blackboxPFrameIndex
++;
1142 if (blackboxPFrameIndex
== BLACKBOX_I_INTERVAL
) {
1143 blackboxPFrameIndex
= 0;
1144 blackboxIFrameIndex
++;
1147 case BLACKBOX_STATE_SHUTTING_DOWN
:
1148 //On entry of this state, startTime is set and a flush is performed
1151 * Wait for the log we've transmitted to make its way to the logger before we release the serial port,
1152 * since releasing the port clears the Tx buffer.
1154 * Don't wait longer than it could possibly take if something funky happens.
1156 if (millis() > xmitState
.u
.startTime
+ BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS
|| blackboxDeviceFlush()) {
1157 blackboxDeviceClose();
1158 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1165 // Did we run out of room on the device? Stop!
1166 if (isBlackboxDeviceFull()) {
1167 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1171 static bool canUseBlackboxWithCurrentConfiguration(void)
1173 return feature(FEATURE_BLACKBOX
);
1177 * Call during system startup to initialize the blackbox.
1179 void initBlackbox(void)
1181 if (canUseBlackboxWithCurrentConfiguration()) {
1182 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1184 blackboxSetState(BLACKBOX_STATE_DISABLED
);