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/>.
24 #include "common/maths.h"
25 #include "common/axis.h"
26 #include "common/color.h"
28 #include "drivers/gpio.h"
29 #include "drivers/sensor.h"
30 #include "drivers/system.h"
31 #include "drivers/serial.h"
32 #include "drivers/compass.h"
33 #include "drivers/timer.h"
34 #include "drivers/pwm_rx.h"
35 #include "drivers/accgyro.h"
36 #include "drivers/light_led.h"
37 #include "drivers/sound_beeper.h"
39 #include "sensors/sensors.h"
40 #include "sensors/boardalignment.h"
41 #include "sensors/sonar.h"
42 #include "sensors/compass.h"
43 #include "sensors/acceleration.h"
44 #include "sensors/barometer.h"
45 #include "sensors/gyro.h"
46 #include "sensors/battery.h"
48 #include "io/beeper.h"
49 #include "io/display.h"
50 #include "io/escservo.h"
51 #include "io/rc_controls.h"
52 #include "io/gimbal.h"
54 #include "io/ledstrip.h"
55 #include "io/serial.h"
56 #include "io/serial_cli.h"
57 #include "io/serial_msp.h"
58 #include "io/statusindicator.h"
63 #include "telemetry/telemetry.h"
65 #include "flight/mixer.h"
66 #include "flight/altitudehold.h"
67 #include "flight/failsafe.h"
68 #include "flight/imu.h"
69 #include "flight/navigation.h"
71 #include "config/runtime_config.h"
72 #include "config/config.h"
73 #include "config/config_profile.h"
74 #include "config/config_master.h"
77 #include "blackbox_io.h"
79 #define BLACKBOX_I_INTERVAL 32
81 #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
83 #define STATIC_ASSERT(condition, name ) \
84 typedef char assert_failed_ ## name [(condition) ? 1 : -1 ]
86 // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
87 #define STR_HELPER(x) #x
88 #define STR(x) STR_HELPER(x)
90 #define CONCAT_HELPER(x,y) x ## y
91 #define CONCAT(x,y) CONCAT_HELPER(x, y)
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
{
134 } blackboxFieldDefinition_t
;
136 typedef struct blackboxMainFieldDefinition_t
{
143 uint8_t condition
; // Decide whether this field should appear in the log
144 } blackboxMainFieldDefinition_t
;
146 typedef struct blackboxGPSFieldDefinition_t
{
151 uint8_t condition
; // Decide whether this field should appear in the log
152 } blackboxGPSFieldDefinition_t
;
155 * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
156 * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
157 * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
158 * the encoding we've promised here).
160 static const blackboxMainFieldDefinition_t blackboxMainFields
[] = {
161 /* loopIteration doesn't appear in P frames since it always increments */
162 {"loopIteration", UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(INC
), .Pencode
= FLIGHT_LOG_FIELD_ENCODING_NULL
, CONDITION(ALWAYS
)},
163 /* Time advances pretty steadily so the P-frame prediction is a straight line */
164 {"time", UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(STRAIGHT_LINE
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
165 {"axisP[0]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
166 {"axisP[1]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
167 {"axisP[2]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
168 /* I terms get special packed encoding in P frames: */
169 {"axisI[0]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
170 {"axisI[1]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
171 {"axisI[2]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
172 {"axisD[0]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_0
)},
173 {"axisD[1]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_1
)},
174 {"axisD[2]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_2
)},
175 /* rcCommands are encoded together as a group in P-frames: */
176 {"rcCommand[0]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
177 {"rcCommand[1]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
178 {"rcCommand[2]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
179 /* Throttle is always in the range [minthrottle..maxthrottle]: */
180 {"rcCommand[3]", UNSIGNED
, .Ipredict
= PREDICT(MINTHROTTLE
), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
182 {"vbatLatest", UNSIGNED
, .Ipredict
= PREDICT(VBATREF
), .Iencode
= ENCODING(NEG_14BIT
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_VBAT
},
183 {"amperageLatest",UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE
},
185 {"magADC[0]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
186 {"magADC[1]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
187 {"magADC[2]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
190 {"BaroAlt", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_BARO
},
193 /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
194 {"gyroData[0]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
195 {"gyroData[1]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
196 {"gyroData[2]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
197 {"accSmooth[0]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
198 {"accSmooth[1]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
199 {"accSmooth[2]", SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
200 /* 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): */
201 {"motor[0]", UNSIGNED
, .Ipredict
= PREDICT(MINTHROTTLE
), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_1
)},
202 /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
203 {"motor[1]", UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_2
)},
204 {"motor[2]", UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_3
)},
205 {"motor[3]", UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_4
)},
206 {"motor[4]", UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_5
)},
207 {"motor[5]", UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_6
)},
208 {"motor[6]", UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_7
)},
209 {"motor[7]", UNSIGNED
, .Ipredict
= PREDICT(MOTOR_0
), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(AT_LEAST_MOTORS_8
)},
210 {"servo[5]", UNSIGNED
, .Ipredict
= PREDICT(1500), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(TRICOPTER
)}
214 // GPS position/vel frame
215 static const blackboxGPSFieldDefinition_t blackboxGpsGFields
[] = {
216 {"time", UNSIGNED
, PREDICT(LAST_MAIN_FRAME_TIME
), ENCODING(UNSIGNED_VB
), CONDITION(NOT_LOGGING_EVERY_FRAME
)},
217 {"GPS_numSat", UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)},
218 {"GPS_coord[0]", SIGNED
, PREDICT(HOME_COORD
), ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
219 {"GPS_coord[1]", SIGNED
, PREDICT(HOME_COORD
), ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
220 {"GPS_altitude", UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)},
221 {"GPS_speed", UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)},
222 {"GPS_ground_course",UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
), CONDITION(ALWAYS
)}
226 static const blackboxGPSFieldDefinition_t blackboxGpsHFields
[] = {
227 {"GPS_home[0]", SIGNED
, PREDICT(0), ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
228 {"GPS_home[1]", SIGNED
, PREDICT(0), ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)}
232 typedef enum BlackboxState
{
233 BLACKBOX_STATE_DISABLED
= 0,
234 BLACKBOX_STATE_STOPPED
,
235 BLACKBOX_STATE_SEND_HEADER
,
236 BLACKBOX_STATE_SEND_FIELDINFO
,
237 BLACKBOX_STATE_SEND_GPS_H_HEADERS
,
238 BLACKBOX_STATE_SEND_GPS_G_HEADERS
,
239 BLACKBOX_STATE_SEND_SYSINFO
,
240 BLACKBOX_STATE_PRERUN
,
241 BLACKBOX_STATE_RUNNING
,
242 BLACKBOX_STATE_SHUTTING_DOWN
245 typedef struct gpsState_t
{
246 int32_t GPS_home
[2], GPS_coord
[2];
251 extern uint8_t motorCount
;
254 extern uint32_t currentTime
;
256 static BlackboxState blackboxState
= BLACKBOX_STATE_DISABLED
;
259 uint32_t headerIndex
;
261 /* Since these fields are used during different blackbox states (never simultaneously) we can
262 * overlap them to save on RAM
271 // Cache for FLIGHT_LOG_FIELD_CONDITION_* test results:
272 static uint32_t blackboxConditionCache
;
274 STATIC_ASSERT((sizeof(blackboxConditionCache
) * 8) >= FLIGHT_LOG_FIELD_CONDITION_NEVER
, too_many_flight_log_conditions
);
276 static uint32_t blackboxIteration
;
277 static uint32_t blackboxPFrameIndex
, blackboxIFrameIndex
;
280 * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
281 * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs
284 static uint16_t vbatReference
;
285 static gpsState_t gpsHistory
;
287 // Keep a history of length 2, plus a buffer for MW to store the new values into
288 static blackboxValues_t blackboxHistoryRing
[3];
290 // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
291 static blackboxValues_t
* blackboxHistory
[3];
293 static bool testBlackboxConditionUncached(FlightLogFieldCondition condition
)
296 case FLIGHT_LOG_FIELD_CONDITION_ALWAYS
:
299 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1
:
300 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2
:
301 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3
:
302 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4
:
303 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5
:
304 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6
:
305 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7
:
306 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8
:
307 return motorCount
>= condition
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1
+ 1;
309 case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
:
310 return masterConfig
.mixerMode
== MIXER_TRI
;
312 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
:
313 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1
:
314 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2
:
315 return currentProfile
->pidProfile
.D8
[condition
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
] != 0;
317 case FLIGHT_LOG_FIELD_CONDITION_MAG
:
319 return sensors(SENSOR_MAG
);
324 case FLIGHT_LOG_FIELD_CONDITION_BARO
:
326 return sensors(SENSOR_BARO
);
331 case FLIGHT_LOG_FIELD_CONDITION_VBAT
:
332 return feature(FEATURE_VBAT
);
334 case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE
:
335 return feature(FEATURE_CURRENT_METER
);
337 case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME
:
338 return masterConfig
.blackbox_rate_num
< masterConfig
.blackbox_rate_denom
;
340 case FLIGHT_LOG_FIELD_CONDITION_NEVER
:
347 static void blackboxBuildConditionCache()
349 FlightLogFieldCondition cond
;
351 blackboxConditionCache
= 0;
353 for (cond
= FLIGHT_LOG_FIELD_CONDITION_FIRST
; cond
<= FLIGHT_LOG_FIELD_CONDITION_LAST
; cond
++) {
354 if (testBlackboxConditionUncached(cond
)) {
355 blackboxConditionCache
|= 1 << cond
;
360 static bool testBlackboxCondition(FlightLogFieldCondition condition
)
362 return (blackboxConditionCache
& (1 << condition
)) != 0;
365 static void blackboxSetState(BlackboxState newState
)
367 //Perform initial setup required for the new state
369 case BLACKBOX_STATE_SEND_HEADER
:
370 xmitState
.headerIndex
= 0;
371 xmitState
.u
.startTime
= millis();
373 case BLACKBOX_STATE_SEND_FIELDINFO
:
374 case BLACKBOX_STATE_SEND_GPS_G_HEADERS
:
375 case BLACKBOX_STATE_SEND_GPS_H_HEADERS
:
376 xmitState
.headerIndex
= 0;
377 xmitState
.u
.fieldIndex
= -1;
379 case BLACKBOX_STATE_SEND_SYSINFO
:
380 xmitState
.headerIndex
= 0;
382 case BLACKBOX_STATE_RUNNING
:
383 blackboxIteration
= 0;
384 blackboxPFrameIndex
= 0;
385 blackboxIFrameIndex
= 0;
387 case BLACKBOX_STATE_SHUTTING_DOWN
:
388 xmitState
.u
.startTime
= millis();
389 blackboxDeviceFlush();
394 blackboxState
= newState
;
397 static void writeIntraframe(void)
399 blackboxValues_t
*blackboxCurrent
= blackboxHistory
[0];
404 blackboxWriteUnsignedVB(blackboxIteration
);
405 blackboxWriteUnsignedVB(blackboxCurrent
->time
);
407 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
408 blackboxWriteSignedVB(blackboxCurrent
->axisPID_P
[x
]);
411 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
412 blackboxWriteSignedVB(blackboxCurrent
->axisPID_I
[x
]);
415 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
416 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
+ x
)) {
417 blackboxWriteSignedVB(blackboxCurrent
->axisPID_D
[x
]);
421 for (x
= 0; x
< 3; x
++) {
422 blackboxWriteSignedVB(blackboxCurrent
->rcCommand
[x
]);
425 blackboxWriteUnsignedVB(blackboxCurrent
->rcCommand
[3] - masterConfig
.escAndServoConfig
.minthrottle
); //Throttle lies in range [minthrottle..maxthrottle]
427 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT
)) {
429 * Our voltage is expected to decrease over the course of the flight, so store our difference from
432 * Write 14 bits even if the number is negative (which would otherwise result in 32 bits)
434 blackboxWriteUnsignedVB((vbatReference
- blackboxCurrent
->vbatLatest
) & 0x3FFF);
437 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE
)) {
438 // 12bit value directly from ADC
439 blackboxWriteUnsignedVB(blackboxCurrent
->amperageLatest
);
443 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG
)) {
444 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
445 blackboxWriteSignedVB(blackboxCurrent
->magADC
[x
]);
451 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO
)) {
452 blackboxWriteSignedVB(blackboxCurrent
->BaroAlt
);
456 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
457 blackboxWriteSignedVB(blackboxCurrent
->gyroData
[x
]);
460 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
461 blackboxWriteSignedVB(blackboxCurrent
->accSmooth
[x
]);
464 //Motors can be below minthrottle when disarmed, but that doesn't happen much
465 blackboxWriteUnsignedVB(blackboxCurrent
->motor
[0] - masterConfig
.escAndServoConfig
.minthrottle
);
467 //Motors tend to be similar to each other
468 for (x
= 1; x
< motorCount
; x
++) {
469 blackboxWriteSignedVB(blackboxCurrent
->motor
[x
] - blackboxCurrent
->motor
[0]);
472 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
)) {
473 blackboxWriteSignedVB(blackboxHistory
[0]->servo
[5] - 1500);
476 //Rotate our history buffers:
478 //The current state becomes the new "before" state
479 blackboxHistory
[1] = blackboxHistory
[0];
480 //And since we have no other history, we also use it for the "before, before" state
481 blackboxHistory
[2] = blackboxHistory
[0];
482 //And advance the current state over to a blank space ready to be filled
483 blackboxHistory
[0] = ((blackboxHistory
[0] - blackboxHistoryRing
+ 1) % 3) + blackboxHistoryRing
;
486 static void writeInterframe(void)
491 blackboxValues_t
*blackboxCurrent
= blackboxHistory
[0];
492 blackboxValues_t
*blackboxLast
= blackboxHistory
[1];
496 //No need to store iteration count since its delta is always 1
499 * Since the difference between the difference between successive times will be nearly zero (due to consistent
500 * looptime spacing), use second-order differences.
502 blackboxWriteSignedVB((int32_t) (blackboxHistory
[0]->time
- 2 * blackboxHistory
[1]->time
+ blackboxHistory
[2]->time
));
504 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
505 blackboxWriteSignedVB(blackboxCurrent
->axisPID_P
[x
] - blackboxLast
->axisPID_P
[x
]);
508 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
509 deltas
[x
] = blackboxCurrent
->axisPID_I
[x
] - blackboxLast
->axisPID_I
[x
];
513 * The PID I field changes very slowly, most of the time +-2, so use an encoding
514 * that can pack all three fields into one byte in that situation.
516 blackboxWriteTag2_3S32(deltas
);
519 * The PID D term is frequently set to zero for yaw, which makes the result from the calculation
520 * always zero. So don't bother recording D results when PID D terms are zero.
522 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
523 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
+ x
)) {
524 blackboxWriteSignedVB(blackboxCurrent
->axisPID_D
[x
] - blackboxLast
->axisPID_D
[x
]);
529 * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
530 * can pack multiple values per byte:
532 for (x
= 0; x
< 4; x
++) {
533 deltas
[x
] = blackboxCurrent
->rcCommand
[x
] - blackboxLast
->rcCommand
[x
];
536 blackboxWriteTag8_4S16(deltas
);
538 //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, Amperage, MAG, BARO
539 int optionalFieldCount
= 0;
541 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT
)) {
542 deltas
[optionalFieldCount
++] = (int32_t) blackboxCurrent
->vbatLatest
- blackboxLast
->vbatLatest
;
545 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE
)) {
546 deltas
[optionalFieldCount
++] = (int32_t) blackboxCurrent
->amperageLatest
- blackboxLast
->amperageLatest
;
550 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG
)) {
551 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
552 deltas
[optionalFieldCount
++] = blackboxCurrent
->magADC
[x
] - blackboxLast
->magADC
[x
];
558 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO
)) {
559 deltas
[optionalFieldCount
++] = blackboxCurrent
->BaroAlt
- blackboxLast
->BaroAlt
;
562 blackboxWriteTag8_8SVB(deltas
, optionalFieldCount
);
564 //Since gyros, accs and motors are noisy, base the prediction on the average of the history:
565 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
566 blackboxWriteSignedVB(blackboxHistory
[0]->gyroData
[x
] - (blackboxHistory
[1]->gyroData
[x
] + blackboxHistory
[2]->gyroData
[x
]) / 2);
569 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
570 blackboxWriteSignedVB(blackboxHistory
[0]->accSmooth
[x
] - (blackboxHistory
[1]->accSmooth
[x
] + blackboxHistory
[2]->accSmooth
[x
]) / 2);
573 for (x
= 0; x
< motorCount
; x
++) {
574 blackboxWriteSignedVB(blackboxHistory
[0]->motor
[x
] - (blackboxHistory
[1]->motor
[x
] + blackboxHistory
[2]->motor
[x
]) / 2);
577 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
)) {
578 blackboxWriteSignedVB(blackboxCurrent
->servo
[5] - blackboxLast
->servo
[5]);
581 //Rotate our history buffers
582 blackboxHistory
[2] = blackboxHistory
[1];
583 blackboxHistory
[1] = blackboxHistory
[0];
584 blackboxHistory
[0] = ((blackboxHistory
[0] - blackboxHistoryRing
+ 1) % 3) + blackboxHistoryRing
;
587 static int gcd(int num
, int denom
)
593 return gcd(denom
, num
% denom
);
596 static void validateBlackboxConfig()
600 if (masterConfig
.blackbox_rate_num
== 0 || masterConfig
.blackbox_rate_denom
== 0
601 || masterConfig
.blackbox_rate_num
>= masterConfig
.blackbox_rate_denom
) {
602 masterConfig
.blackbox_rate_num
= 1;
603 masterConfig
.blackbox_rate_denom
= 1;
605 div
= gcd(masterConfig
.blackbox_rate_num
, masterConfig
.blackbox_rate_denom
);
607 masterConfig
.blackbox_rate_num
/= div
;
608 masterConfig
.blackbox_rate_denom
/= div
;
612 void startBlackbox(void)
614 if (blackboxState
== BLACKBOX_STATE_STOPPED
) {
615 validateBlackboxConfig();
617 if (!blackboxDeviceOpen()) {
618 blackboxSetState(BLACKBOX_STATE_DISABLED
);
622 memset(&gpsHistory
, 0, sizeof(gpsHistory
));
624 blackboxHistory
[0] = &blackboxHistoryRing
[0];
625 blackboxHistory
[1] = &blackboxHistoryRing
[1];
626 blackboxHistory
[2] = &blackboxHistoryRing
[2];
628 vbatReference
= vbatLatestADC
;
630 //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
633 * We use conditional tests to decide whether or not certain fields should be logged. Since our headers
634 * must always agree with the logged data, the results of these tests must not change during logging. So
637 blackboxBuildConditionCache();
639 blackboxSetState(BLACKBOX_STATE_SEND_HEADER
);
643 void finishBlackbox(void)
645 if (blackboxState
== BLACKBOX_STATE_RUNNING
) {
646 blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END
, NULL
);
648 blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN
);
649 } else if (blackboxState
!= BLACKBOX_STATE_DISABLED
&& blackboxState
!= BLACKBOX_STATE_STOPPED
650 && blackboxState
!= BLACKBOX_STATE_SHUTTING_DOWN
) {
652 * We're shutting down in the middle of transmitting headers, so we can't log a "log completed" event.
653 * Just give the port back and stop immediately.
655 blackboxDeviceClose();
656 blackboxSetState(BLACKBOX_STATE_STOPPED
);
661 static void writeGPSHomeFrame()
665 blackboxWriteSignedVB(GPS_home
[0]);
666 blackboxWriteSignedVB(GPS_home
[1]);
667 //TODO it'd be great if we could grab the GPS current time and write that too
669 gpsHistory
.GPS_home
[0] = GPS_home
[0];
670 gpsHistory
.GPS_home
[1] = GPS_home
[1];
673 static void writeGPSFrame()
678 * If we're logging every frame, then a GPS frame always appears just after a frame with the
679 * currentTime timestamp in the log, so the reader can just use that timestamp for the GPS frame.
681 * If we're not logging every frame, we need to store the time of this GPS frame.
683 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME
)) {
684 // Predict the time of the last frame in the main log
685 blackboxWriteUnsignedVB(currentTime
- blackboxHistory
[1]->time
);
688 blackboxWriteUnsignedVB(GPS_numSat
);
689 blackboxWriteSignedVB(GPS_coord
[0] - gpsHistory
.GPS_home
[0]);
690 blackboxWriteSignedVB(GPS_coord
[1] - gpsHistory
.GPS_home
[1]);
691 blackboxWriteUnsignedVB(GPS_altitude
);
692 blackboxWriteUnsignedVB(GPS_speed
);
693 blackboxWriteUnsignedVB(GPS_ground_course
);
695 gpsHistory
.GPS_numSat
= GPS_numSat
;
696 gpsHistory
.GPS_coord
[0] = GPS_coord
[0];
697 gpsHistory
.GPS_coord
[1] = GPS_coord
[1];
702 * Fill the current state of the blackbox using values read from the flight controller
704 static void loadBlackboxState(void)
706 blackboxValues_t
*blackboxCurrent
= blackboxHistory
[0];
709 blackboxCurrent
->time
= currentTime
;
711 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
712 blackboxCurrent
->axisPID_P
[i
] = axisPID_P
[i
];
714 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
715 blackboxCurrent
->axisPID_I
[i
] = axisPID_I
[i
];
717 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
718 blackboxCurrent
->axisPID_D
[i
] = axisPID_D
[i
];
721 for (i
= 0; i
< 4; i
++) {
722 blackboxCurrent
->rcCommand
[i
] = rcCommand
[i
];
725 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
726 blackboxCurrent
->gyroData
[i
] = gyroData
[i
];
729 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
730 blackboxCurrent
->accSmooth
[i
] = accSmooth
[i
];
733 for (i
= 0; i
< motorCount
; i
++) {
734 blackboxCurrent
->motor
[i
] = motor
[i
];
737 blackboxCurrent
->vbatLatest
= vbatLatestADC
;
738 blackboxCurrent
->amperageLatest
= amperageLatestADC
;
741 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
742 blackboxCurrent
->magADC
[i
] = magADC
[i
];
747 blackboxCurrent
->BaroAlt
= BaroAlt
;
750 //Tail servo for tricopters
751 blackboxCurrent
->servo
[5] = servo
[5];
755 * Transmit the header information for the given field definitions. Transmitted header lines look like:
757 * H Field I name:a,b,c
758 * H Field I predictor:0,1,2
760 * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
761 * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
763 * Set xmitState.headerIndex to 0 and xmitState.u.fieldIndex to -1 before calling for the first time.
765 * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the
766 * fieldDefinition and secondCondition arrays.
768 * Returns true if there is still header left to transmit (so call again to continue transmission).
770 static bool sendFieldDefinition(const char * const *headerNames
, unsigned int headerCount
, const void *fieldDefinitions
,
771 const void *secondFieldDefinition
, int fieldCount
, const uint8_t *conditions
, const uint8_t *secondCondition
)
773 const blackboxFieldDefinition_t
*def
;
775 static bool needComma
= false;
776 size_t definitionStride
= (char*) secondFieldDefinition
- (char*) fieldDefinitions
;
777 size_t conditionsStride
= (char*) secondCondition
- (char*) conditions
;
780 * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit
783 if (xmitState
.u
.fieldIndex
== -1) {
784 if (xmitState
.headerIndex
>= headerCount
) {
785 return false; //Someone probably called us again after we had already completed transmission
788 charsWritten
= blackboxPrint("H Field ");
789 charsWritten
+= blackboxPrint(headerNames
[xmitState
.headerIndex
]);
790 charsWritten
+= blackboxPrint(":");
792 xmitState
.u
.fieldIndex
++;
797 for (; xmitState
.u
.fieldIndex
< fieldCount
&& charsWritten
< blackboxWriteChunkSize
; xmitState
.u
.fieldIndex
++) {
798 def
= (const blackboxFieldDefinition_t
*) ((const char*)fieldDefinitions
+ definitionStride
* xmitState
.u
.fieldIndex
);
800 if (!conditions
|| testBlackboxCondition(conditions
[conditionsStride
* xmitState
.u
.fieldIndex
])) {
808 // The first header is a field name
809 if (xmitState
.headerIndex
== 0) {
810 charsWritten
+= blackboxPrint(def
->name
);
812 //The other headers are integers
813 if (def
->arr
[xmitState
.headerIndex
- 1] >= 10) {
814 blackboxWrite(def
->arr
[xmitState
.headerIndex
- 1] / 10 + '0');
815 blackboxWrite(def
->arr
[xmitState
.headerIndex
- 1] % 10 + '0');
818 blackboxWrite(def
->arr
[xmitState
.headerIndex
- 1] + '0');
825 // Did we complete this line?
826 if (xmitState
.u
.fieldIndex
== fieldCount
) {
828 xmitState
.headerIndex
++;
829 xmitState
.u
.fieldIndex
= -1;
832 return xmitState
.headerIndex
< headerCount
;
836 * Transmit a portion of the system information headers. Call the first time with xmitState.headerIndex == 0. Returns
837 * true iff transmission is complete, otherwise call again later to continue transmission.
839 static bool blackboxWriteSysinfo()
841 union floatConvert_t
{
846 if (xmitState
.headerIndex
== 0) {
847 xmitState
.u
.serialBudget
= 0;
848 xmitState
.headerIndex
= 1;
851 // How many bytes can we afford to transmit this loop?
852 xmitState
.u
.serialBudget
= MIN(xmitState
.u
.serialBudget
+ blackboxWriteChunkSize
, 64);
854 // Most headers will consume at least 20 bytes so wait until we've built up that much link budget
855 if (xmitState
.u
.serialBudget
< 20) {
859 switch (xmitState
.headerIndex
) {
861 //Shouldn't ever get here
864 blackboxPrintf("H Firmware type:Cleanflight\n");
866 xmitState
.u
.serialBudget
-= strlen("H Firmware type:Cleanflight\n");
869 blackboxPrintf("H Firmware revision:%s\n", shortGitRevision
);
871 /* Don't need to be super exact about the budget so don't mind the fact that we're including the length of
872 * the placeholder "%s"
874 xmitState
.u
.serialBudget
-= strlen("H Firmware revision:%s\n") + strlen(shortGitRevision
);
877 blackboxPrintf("H Firmware date:%s %s\n", buildDate
, buildTime
);
879 xmitState
.u
.serialBudget
-= strlen("H Firmware date:%s %s\n") + strlen(buildDate
) + strlen(buildTime
);
882 blackboxPrintf("H P interval:%d/%d\n", masterConfig
.blackbox_rate_num
, masterConfig
.blackbox_rate_denom
);
884 xmitState
.u
.serialBudget
-= strlen("H P interval:%d/%d\n");
887 blackboxPrintf("H rcRate:%d\n", masterConfig
.controlRateProfiles
[masterConfig
.current_profile_index
].rcRate8
);
889 xmitState
.u
.serialBudget
-= strlen("H rcRate:%d\n");
892 blackboxPrintf("H minthrottle:%d\n", masterConfig
.escAndServoConfig
.minthrottle
);
894 xmitState
.u
.serialBudget
-= strlen("H minthrottle:%d\n");
897 blackboxPrintf("H maxthrottle:%d\n", masterConfig
.escAndServoConfig
.maxthrottle
);
899 xmitState
.u
.serialBudget
-= strlen("H maxthrottle:%d\n");
902 floatConvert
.f
= gyro
.scale
;
903 blackboxPrintf("H gyro.scale:0x%x\n", floatConvert
.u
);
905 xmitState
.u
.serialBudget
-= strlen("H gyro.scale:0x%x\n") + 6;
908 blackboxPrintf("H acc_1G:%u\n", acc_1G
);
910 xmitState
.u
.serialBudget
-= strlen("H acc_1G:%u\n");
913 blackboxPrintf("H vbatscale:%u\n", masterConfig
.batteryConfig
.vbatscale
);
915 xmitState
.u
.serialBudget
-= strlen("H vbatscale:%u\n");
918 blackboxPrintf("H vbatcellvoltage:%u,%u,%u\n", masterConfig
.batteryConfig
.vbatmincellvoltage
,
919 masterConfig
.batteryConfig
.vbatwarningcellvoltage
, masterConfig
.batteryConfig
.vbatmaxcellvoltage
);
921 xmitState
.u
.serialBudget
-= strlen("H vbatcellvoltage:%u,%u,%u\n");
924 blackboxPrintf("H vbatref:%u\n", vbatReference
);
926 xmitState
.u
.serialBudget
-= strlen("H vbatref:%u\n");
929 blackboxPrintf("H currentMeter:%d,%d\n", masterConfig
.batteryConfig
.currentMeterOffset
, masterConfig
.batteryConfig
.currentMeterScale
);
931 xmitState
.u
.serialBudget
-= strlen("H currentMeter:%d,%d\n");
937 xmitState
.headerIndex
++;
942 * Write the given event to the log immediately
944 void blackboxLogEvent(FlightLogEvent event
, flightLogEventData_t
*data
)
946 if (blackboxState
!= BLACKBOX_STATE_RUNNING
) {
950 //Shared header for event frames
952 blackboxWrite(event
);
954 //Now serialize the data for this specific frame type
956 case FLIGHT_LOG_EVENT_SYNC_BEEP
:
957 blackboxWriteUnsignedVB(data
->syncBeep
.time
);
959 case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START
:
960 blackboxWrite(data
->autotuneCycleStart
.phase
);
961 blackboxWrite(data
->autotuneCycleStart
.cycle
| (data
->autotuneCycleStart
.rising
? 0x80 : 0));
962 blackboxWrite(data
->autotuneCycleStart
.p
);
963 blackboxWrite(data
->autotuneCycleStart
.i
);
964 blackboxWrite(data
->autotuneCycleStart
.d
);
966 case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT
:
967 blackboxWrite(data
->autotuneCycleResult
.flags
);
968 blackboxWrite(data
->autotuneCycleStart
.p
);
969 blackboxWrite(data
->autotuneCycleStart
.i
);
970 blackboxWrite(data
->autotuneCycleStart
.d
);
972 case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS
:
973 blackboxWriteS16(data
->autotuneTargets
.currentAngle
);
974 blackboxWrite((uint8_t) data
->autotuneTargets
.targetAngle
);
975 blackboxWrite((uint8_t) data
->autotuneTargets
.targetAngleAtPeak
);
976 blackboxWriteS16(data
->autotuneTargets
.firstPeakAngle
);
977 blackboxWriteS16(data
->autotuneTargets
.secondPeakAngle
);
979 case FLIGHT_LOG_EVENT_LOG_END
:
980 blackboxPrint("End of log");
986 // Beep the buzzer and write the current time to the log as a synchronization point
987 static void blackboxPlaySyncBeep()
989 flightLogEvent_syncBeep_t eventData
;
991 eventData
.time
= micros();
994 * The regular beep routines aren't going to work for us, because they queue up the beep to be executed later.
995 * Our beep is timing sensitive, so start beeping now without setting the beeperIsOn flag.
999 // Have the regular beeper code turn off the beep for us eventually, since that's not timing-sensitive
1000 queueConfirmationBeep(1);
1002 blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP
, (flightLogEventData_t
*) &eventData
);
1005 void handleBlackbox(void)
1009 switch (blackboxState
) {
1010 case BLACKBOX_STATE_SEND_HEADER
:
1011 //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
1014 * Once the UART has had time to init, transmit the header in chunks so we don't overflow our transmit
1017 if (millis() > xmitState
.u
.startTime
+ 100) {
1018 for (i
= 0; i
< blackboxWriteChunkSize
&& blackboxHeader
[xmitState
.headerIndex
] != '\0'; i
++, xmitState
.headerIndex
++) {
1019 blackboxWrite(blackboxHeader
[xmitState
.headerIndex
]);
1022 if (blackboxHeader
[xmitState
.headerIndex
] == '\0') {
1023 blackboxSetState(BLACKBOX_STATE_SEND_FIELDINFO
);
1027 case BLACKBOX_STATE_SEND_FIELDINFO
:
1028 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1029 if (!sendFieldDefinition(blackboxMainHeaderNames
, ARRAY_LENGTH(blackboxMainHeaderNames
), blackboxMainFields
, blackboxMainFields
+ 1,
1030 ARRAY_LENGTH(blackboxMainFields
), &blackboxMainFields
[0].condition
, &blackboxMainFields
[1].condition
)) {
1032 if (feature(FEATURE_GPS
)) {
1033 blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADERS
);
1036 blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO
);
1040 case BLACKBOX_STATE_SEND_GPS_H_HEADERS
:
1041 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1042 if (!sendFieldDefinition(blackboxGPSHHeaderNames
, ARRAY_LENGTH(blackboxGPSHHeaderNames
), blackboxGpsHFields
, blackboxGpsHFields
+ 1,
1043 ARRAY_LENGTH(blackboxGpsHFields
), &blackboxGpsHFields
[0].condition
, &blackboxGpsHFields
[1].condition
)) {
1044 blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADERS
);
1047 case BLACKBOX_STATE_SEND_GPS_G_HEADERS
:
1048 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1049 if (!sendFieldDefinition(blackboxGPSGHeaderNames
, ARRAY_LENGTH(blackboxGPSGHeaderNames
), blackboxGpsGFields
, blackboxGpsGFields
+ 1,
1050 ARRAY_LENGTH(blackboxGpsGFields
), &blackboxGpsGFields
[0].condition
, &blackboxGpsGFields
[1].condition
)) {
1051 blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO
);
1055 case BLACKBOX_STATE_SEND_SYSINFO
:
1056 //On entry of this state, xmitState.headerIndex is 0
1058 //Keep writing chunks of the system info headers until it returns true to signal completion
1059 if (blackboxWriteSysinfo()) {
1060 blackboxSetState(BLACKBOX_STATE_PRERUN
);
1063 case BLACKBOX_STATE_PRERUN
:
1064 blackboxSetState(BLACKBOX_STATE_RUNNING
);
1066 blackboxPlaySyncBeep();
1068 case BLACKBOX_STATE_RUNNING
:
1069 // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
1071 // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
1072 if (blackboxPFrameIndex
== 0) {
1073 // Copy current system values into the blackbox
1074 loadBlackboxState();
1077 if ((blackboxPFrameIndex
+ masterConfig
.blackbox_rate_num
- 1) % masterConfig
.blackbox_rate_denom
< masterConfig
.blackbox_rate_num
) {
1078 loadBlackboxState();
1082 if (feature(FEATURE_GPS
)) {
1084 * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the
1085 * GPS home position.
1087 * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
1088 * still be interpreted correctly.
1090 if (GPS_home
[0] != gpsHistory
.GPS_home
[0] || GPS_home
[1] != gpsHistory
.GPS_home
[1]
1091 || (blackboxPFrameIndex
== BLACKBOX_I_INTERVAL
/ 2 && blackboxIFrameIndex
% 128 == 0)) {
1093 writeGPSHomeFrame();
1095 } else if (GPS_numSat
!= gpsHistory
.GPS_numSat
|| GPS_coord
[0] != gpsHistory
.GPS_coord
[0]
1096 || GPS_coord
[1] != gpsHistory
.GPS_coord
[1]) {
1097 //We could check for velocity changes as well but I doubt it changes independent of position
1104 blackboxIteration
++;
1105 blackboxPFrameIndex
++;
1107 if (blackboxPFrameIndex
== BLACKBOX_I_INTERVAL
) {
1108 blackboxPFrameIndex
= 0;
1109 blackboxIFrameIndex
++;
1112 case BLACKBOX_STATE_SHUTTING_DOWN
:
1113 //On entry of this state, startTime is set and a flush is performed
1116 * Wait for the log we've transmitted to make its way to the logger before we release the serial port,
1117 * since releasing the port clears the Tx buffer.
1119 * Don't wait longer than it could possibly take if something funky happens.
1121 if (millis() > xmitState
.u
.startTime
+ 200 || isBlackboxDeviceIdle()) {
1122 blackboxDeviceClose();
1123 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1131 static bool canUseBlackboxWithCurrentConfiguration(void)
1133 return feature(FEATURE_BLACKBOX
);
1136 void initBlackbox(void)
1138 if (canUseBlackboxWithCurrentConfiguration()) {
1139 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1141 blackboxSetState(BLACKBOX_STATE_DISABLED
);