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"
42 #include "sensors/sensors.h"
43 #include "sensors/boardalignment.h"
44 #include "sensors/sonar.h"
45 #include "sensors/compass.h"
46 #include "sensors/acceleration.h"
47 #include "sensors/barometer.h"
48 #include "sensors/gyro.h"
49 #include "sensors/battery.h"
51 #include "io/beeper.h"
52 #include "io/display.h"
53 #include "io/escservo.h"
54 #include "io/rc_controls.h"
55 #include "io/gimbal.h"
57 #include "io/ledstrip.h"
58 #include "io/serial.h"
59 #include "io/serial_cli.h"
60 #include "io/serial_msp.h"
61 #include "io/statusindicator.h"
66 #include "telemetry/telemetry.h"
68 #include "flight/mixer.h"
69 #include "flight/altitudehold.h"
70 #include "flight/failsafe.h"
71 #include "flight/imu.h"
72 #include "flight/navigation.h"
74 #include "config/runtime_config.h"
75 #include "config/config.h"
76 #include "config/config_profile.h"
77 #include "config/config_master.h"
80 #include "blackbox_io.h"
82 #define BLACKBOX_I_INTERVAL 32
83 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
84 #define SLOW_FRAME_INTERVAL 4096
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"
102 "H I interval:" STR(BLACKBOX_I_INTERVAL
) "\n";
104 static const char* const blackboxFieldHeaderNames
[] = {
113 /* All field definition structs should look like this (but with longer arrs): */
114 typedef struct blackboxFieldDefinition_t
{
116 // If the field name has a number to be included in square brackets [1] afterwards, set it here, or -1 for no brackets:
117 int8_t fieldNameIndex
;
119 // Each member of this array will be the value to print for this field for the given header index
121 } blackboxFieldDefinition_t
;
123 #define BLACKBOX_DELTA_FIELD_HEADER_COUNT ARRAY_LENGTH(blackboxFieldHeaderNames)
124 #define BLACKBOX_SIMPLE_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
125 #define BLACKBOX_CONDITIONAL_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
127 typedef struct blackboxSimpleFieldDefinition_t
{
129 int8_t fieldNameIndex
;
134 } blackboxSimpleFieldDefinition_t
;
136 typedef struct blackboxConditionalFieldDefinition_t
{
138 int8_t fieldNameIndex
;
143 uint8_t condition
; // Decide whether this field should appear in the log
144 } blackboxConditionalFieldDefinition_t
;
146 typedef struct blackboxDeltaFieldDefinition_t
{
148 int8_t fieldNameIndex
;
155 uint8_t condition
; // Decide whether this field should appear in the log
156 } blackboxDeltaFieldDefinition_t
;
159 * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
160 * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
161 * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
162 * the encoding we've promised here).
164 static const blackboxDeltaFieldDefinition_t blackboxMainFields
[] = {
165 /* loopIteration doesn't appear in P frames since it always increments */
166 {"loopIteration",-1, UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(INC
), .Pencode
= FLIGHT_LOG_FIELD_ENCODING_NULL
, CONDITION(ALWAYS
)},
167 /* Time advances pretty steadily so the P-frame prediction is a straight line */
168 {"time", -1, UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(STRAIGHT_LINE
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
169 {"axisP", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
170 {"axisP", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
171 {"axisP", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
172 /* I terms get special packed encoding in P frames: */
173 {"axisI", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
174 {"axisI", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
175 {"axisI", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG2_3S32
), CONDITION(ALWAYS
)},
176 {"axisD", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_0
)},
177 {"axisD", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_1
)},
178 {"axisD", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(NONZERO_PID_D_2
)},
179 /* rcCommands are encoded together as a group in P-frames: */
180 {"rcCommand", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
181 {"rcCommand", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
182 {"rcCommand", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
183 /* Throttle is always in the range [minthrottle..maxthrottle]: */
184 {"rcCommand", 3, UNSIGNED
, .Ipredict
= PREDICT(MINTHROTTLE
), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_4S16
), CONDITION(ALWAYS
)},
186 {"vbatLatest", -1, UNSIGNED
, .Ipredict
= PREDICT(VBATREF
), .Iencode
= ENCODING(NEG_14BIT
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_VBAT
},
187 {"amperageLatest",-1, UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC
},
190 {"magADC", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
191 {"magADC", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
192 {"magADC", 2, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_MAG
},
195 {"BaroAlt", -1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_BARO
},
198 {"sonarRaw", -1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_SONAR
},
200 {"rssi", -1, UNSIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(UNSIGNED_VB
), .Ppredict
= PREDICT(PREVIOUS
), .Pencode
= ENCODING(TAG8_8SVB
), FLIGHT_LOG_FIELD_CONDITION_RSSI
},
202 /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
203 {"gyroADC", 0, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
204 {"gyroADC", 1, SIGNED
, .Ipredict
= PREDICT(0), .Iencode
= ENCODING(SIGNED_VB
), .Ppredict
= PREDICT(AVERAGE_2
), .Pencode
= ENCODING(SIGNED_VB
), CONDITION(ALWAYS
)},
205 {"gyroADC", 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 blackboxConditionalFieldDefinition_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 blackboxSimpleFieldDefinition_t blackboxGpsHFields
[] = {
238 {"GPS_home", 0, SIGNED
, PREDICT(0), ENCODING(SIGNED_VB
)},
239 {"GPS_home", 1, SIGNED
, PREDICT(0), ENCODING(SIGNED_VB
)}
243 // Rarely-updated fields
244 static const blackboxSimpleFieldDefinition_t blackboxSlowFields
[] = {
245 {"flightModeFlags", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
)},
246 {"stateFlags", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
)},
247 {"failsafePhase", -1, UNSIGNED
, PREDICT(0), ENCODING(UNSIGNED_VB
)}
250 typedef enum BlackboxState
{
251 BLACKBOX_STATE_DISABLED
= 0,
252 BLACKBOX_STATE_STOPPED
,
253 BLACKBOX_STATE_SEND_HEADER
,
254 BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER
,
255 BLACKBOX_STATE_SEND_GPS_H_HEADER
,
256 BLACKBOX_STATE_SEND_GPS_G_HEADER
,
257 BLACKBOX_STATE_SEND_SLOW_HEADER
,
258 BLACKBOX_STATE_SEND_SYSINFO
,
259 BLACKBOX_STATE_RUNNING
,
260 BLACKBOX_STATE_SHUTTING_DOWN
263 typedef struct blackboxMainState_t
{
266 int32_t axisPID_P
[XYZ_AXIS_COUNT
], axisPID_I
[XYZ_AXIS_COUNT
], axisPID_D
[XYZ_AXIS_COUNT
];
268 int16_t rcCommand
[4];
269 int16_t gyroADC
[XYZ_AXIS_COUNT
];
270 int16_t accSmooth
[XYZ_AXIS_COUNT
];
271 int16_t motor
[MAX_SUPPORTED_MOTORS
];
272 int16_t servo
[MAX_SUPPORTED_SERVOS
];
275 uint16_t amperageLatest
;
281 int16_t magADC
[XYZ_AXIS_COUNT
];
287 } blackboxMainState_t
;
289 typedef struct blackboxGpsState_t
{
290 int32_t GPS_home
[2], GPS_coord
[2];
292 } blackboxGpsState_t
;
294 // This data is updated really infrequently:
295 typedef struct blackboxSlowState_t
{
296 uint16_t flightModeFlags
;
298 uint8_t failsafePhase
;
299 } __attribute__((__packed__
)) blackboxSlowState_t
; // We pack this struct so that padding doesn't interfere with memcmp()
302 extern uint8_t motorCount
;
305 extern uint32_t currentTime
;
308 extern uint16_t rssi
;
310 static BlackboxState blackboxState
= BLACKBOX_STATE_DISABLED
;
312 static uint32_t blackboxLastArmingBeep
= 0;
315 uint32_t headerIndex
;
317 /* Since these fields are used during different blackbox states (never simultaneously) we can
318 * overlap them to save on RAM
327 // Cache for FLIGHT_LOG_FIELD_CONDITION_* test results:
328 static uint32_t blackboxConditionCache
;
330 STATIC_ASSERT((sizeof(blackboxConditionCache
) * 8) >= FLIGHT_LOG_FIELD_CONDITION_NEVER
, too_many_flight_log_conditions
);
332 static uint32_t blackboxIteration
;
333 static uint16_t blackboxPFrameIndex
, blackboxIFrameIndex
;
334 static uint16_t blackboxSlowFrameIterationTimer
;
337 * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
338 * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs
341 static uint16_t vbatReference
;
343 static blackboxGpsState_t gpsHistory
;
344 static blackboxSlowState_t slowHistory
;
346 // Keep a history of length 2, plus a buffer for MW to store the new values into
347 static blackboxMainState_t blackboxHistoryRing
[3];
349 // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
350 static blackboxMainState_t
* blackboxHistory
[3];
352 static bool testBlackboxConditionUncached(FlightLogFieldCondition condition
)
355 case FLIGHT_LOG_FIELD_CONDITION_ALWAYS
:
358 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1
:
359 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2
:
360 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3
:
361 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4
:
362 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5
:
363 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6
:
364 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7
:
365 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8
:
366 return motorCount
>= condition
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1
+ 1;
368 case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
:
369 return masterConfig
.mixerMode
== MIXER_TRI
|| masterConfig
.mixerMode
== MIXER_CUSTOM_TRI
;
371 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
:
372 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1
:
373 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2
:
374 if (IS_PID_CONTROLLER_FP_BASED(currentProfile
->pidProfile
.pidController
)) {
375 return currentProfile
->pidProfile
.D_f
[condition
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
] != 0;
377 return currentProfile
->pidProfile
.D8
[condition
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
] != 0;
380 case FLIGHT_LOG_FIELD_CONDITION_MAG
:
382 return sensors(SENSOR_MAG
);
387 case FLIGHT_LOG_FIELD_CONDITION_BARO
:
389 return sensors(SENSOR_BARO
);
394 case FLIGHT_LOG_FIELD_CONDITION_VBAT
:
395 return feature(FEATURE_VBAT
);
397 case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC
:
398 return feature(FEATURE_CURRENT_METER
) && masterConfig
.batteryConfig
.currentMeterType
== CURRENT_SENSOR_ADC
;
400 case FLIGHT_LOG_FIELD_CONDITION_SONAR
:
402 return feature(FEATURE_SONAR
);
407 case FLIGHT_LOG_FIELD_CONDITION_RSSI
:
408 return masterConfig
.rxConfig
.rssi_channel
> 0 || feature(FEATURE_RSSI_ADC
);
410 case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME
:
411 return masterConfig
.blackbox_rate_num
< masterConfig
.blackbox_rate_denom
;
413 case FLIGHT_LOG_FIELD_CONDITION_NEVER
:
420 static void blackboxBuildConditionCache()
422 FlightLogFieldCondition cond
;
424 blackboxConditionCache
= 0;
426 for (cond
= FLIGHT_LOG_FIELD_CONDITION_FIRST
; cond
<= FLIGHT_LOG_FIELD_CONDITION_LAST
; cond
++) {
427 if (testBlackboxConditionUncached(cond
)) {
428 blackboxConditionCache
|= 1 << cond
;
433 static bool testBlackboxCondition(FlightLogFieldCondition condition
)
435 return (blackboxConditionCache
& (1 << condition
)) != 0;
438 static void blackboxSetState(BlackboxState newState
)
440 //Perform initial setup required for the new state
442 case BLACKBOX_STATE_SEND_HEADER
:
443 xmitState
.headerIndex
= 0;
444 xmitState
.u
.startTime
= millis();
446 case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER
:
447 case BLACKBOX_STATE_SEND_GPS_G_HEADER
:
448 case BLACKBOX_STATE_SEND_GPS_H_HEADER
:
449 case BLACKBOX_STATE_SEND_SLOW_HEADER
:
450 xmitState
.headerIndex
= 0;
451 xmitState
.u
.fieldIndex
= -1;
453 case BLACKBOX_STATE_SEND_SYSINFO
:
454 xmitState
.headerIndex
= 0;
456 case BLACKBOX_STATE_RUNNING
:
457 blackboxIteration
= 0;
458 blackboxPFrameIndex
= 0;
459 blackboxIFrameIndex
= 0;
460 blackboxSlowFrameIterationTimer
= SLOW_FRAME_INTERVAL
; //Force a slow frame to be written on the first iteration
462 case BLACKBOX_STATE_SHUTTING_DOWN
:
463 xmitState
.u
.startTime
= millis();
464 blackboxDeviceFlush();
469 blackboxState
= newState
;
472 static void writeIntraframe(void)
474 blackboxMainState_t
*blackboxCurrent
= blackboxHistory
[0];
479 blackboxWriteUnsignedVB(blackboxIteration
);
480 blackboxWriteUnsignedVB(blackboxCurrent
->time
);
482 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
483 blackboxWriteSignedVB(blackboxCurrent
->axisPID_P
[x
]);
486 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
487 blackboxWriteSignedVB(blackboxCurrent
->axisPID_I
[x
]);
490 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
491 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
+ x
)) {
492 blackboxWriteSignedVB(blackboxCurrent
->axisPID_D
[x
]);
496 for (x
= 0; x
< 3; x
++) {
497 blackboxWriteSignedVB(blackboxCurrent
->rcCommand
[x
]);
500 blackboxWriteUnsignedVB(blackboxCurrent
->rcCommand
[3] - masterConfig
.escAndServoConfig
.minthrottle
); //Throttle lies in range [minthrottle..maxthrottle]
502 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT
)) {
504 * Our voltage is expected to decrease over the course of the flight, so store our difference from
507 * Write 14 bits even if the number is negative (which would otherwise result in 32 bits)
509 blackboxWriteUnsignedVB((vbatReference
- blackboxCurrent
->vbatLatest
) & 0x3FFF);
512 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC
)) {
513 // 12bit value directly from ADC
514 blackboxWriteUnsignedVB(blackboxCurrent
->amperageLatest
);
518 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG
)) {
519 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
520 blackboxWriteSignedVB(blackboxCurrent
->magADC
[x
]);
526 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO
)) {
527 blackboxWriteSignedVB(blackboxCurrent
->BaroAlt
);
532 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR
)) {
533 blackboxWriteSignedVB(blackboxCurrent
->sonarRaw
);
537 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RSSI
)) {
538 blackboxWriteUnsignedVB(blackboxCurrent
->rssi
);
541 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
542 blackboxWriteSignedVB(blackboxCurrent
->gyroADC
[x
]);
545 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
546 blackboxWriteSignedVB(blackboxCurrent
->accSmooth
[x
]);
549 //Motors can be below minthrottle when disarmed, but that doesn't happen much
550 blackboxWriteUnsignedVB(blackboxCurrent
->motor
[0] - masterConfig
.escAndServoConfig
.minthrottle
);
552 //Motors tend to be similar to each other
553 for (x
= 1; x
< motorCount
; x
++) {
554 blackboxWriteSignedVB(blackboxCurrent
->motor
[x
] - blackboxCurrent
->motor
[0]);
557 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
)) {
558 blackboxWriteSignedVB(blackboxHistory
[0]->servo
[5] - 1500);
561 //Rotate our history buffers:
563 //The current state becomes the new "before" state
564 blackboxHistory
[1] = blackboxHistory
[0];
565 //And since we have no other history, we also use it for the "before, before" state
566 blackboxHistory
[2] = blackboxHistory
[0];
567 //And advance the current state over to a blank space ready to be filled
568 blackboxHistory
[0] = ((blackboxHistory
[0] - blackboxHistoryRing
+ 1) % 3) + blackboxHistoryRing
;
571 static void writeInterframe(void)
576 blackboxMainState_t
*blackboxCurrent
= blackboxHistory
[0];
577 blackboxMainState_t
*blackboxLast
= blackboxHistory
[1];
581 //No need to store iteration count since its delta is always 1
584 * Since the difference between the difference between successive times will be nearly zero (due to consistent
585 * looptime spacing), use second-order differences.
587 blackboxWriteSignedVB((int32_t) (blackboxHistory
[0]->time
- 2 * blackboxHistory
[1]->time
+ blackboxHistory
[2]->time
));
589 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
590 blackboxWriteSignedVB(blackboxCurrent
->axisPID_P
[x
] - blackboxLast
->axisPID_P
[x
]);
593 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
594 deltas
[x
] = blackboxCurrent
->axisPID_I
[x
] - blackboxLast
->axisPID_I
[x
];
598 * The PID I field changes very slowly, most of the time +-2, so use an encoding
599 * that can pack all three fields into one byte in that situation.
601 blackboxWriteTag2_3S32(deltas
);
604 * The PID D term is frequently set to zero for yaw, which makes the result from the calculation
605 * always zero. So don't bother recording D results when PID D terms are zero.
607 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
608 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0
+ x
)) {
609 blackboxWriteSignedVB(blackboxCurrent
->axisPID_D
[x
] - blackboxLast
->axisPID_D
[x
]);
614 * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
615 * can pack multiple values per byte:
617 for (x
= 0; x
< 4; x
++) {
618 deltas
[x
] = blackboxCurrent
->rcCommand
[x
] - blackboxLast
->rcCommand
[x
];
621 blackboxWriteTag8_4S16(deltas
);
623 //Check for sensors that are updated periodically (so deltas are normally zero)
624 int optionalFieldCount
= 0;
626 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT
)) {
627 deltas
[optionalFieldCount
++] = (int32_t) blackboxCurrent
->vbatLatest
- blackboxLast
->vbatLatest
;
630 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC
)) {
631 deltas
[optionalFieldCount
++] = (int32_t) blackboxCurrent
->amperageLatest
- blackboxLast
->amperageLatest
;
635 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG
)) {
636 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
637 deltas
[optionalFieldCount
++] = blackboxCurrent
->magADC
[x
] - blackboxLast
->magADC
[x
];
643 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO
)) {
644 deltas
[optionalFieldCount
++] = blackboxCurrent
->BaroAlt
- blackboxLast
->BaroAlt
;
649 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR
)) {
650 deltas
[optionalFieldCount
++] = blackboxCurrent
->sonarRaw
- blackboxLast
->sonarRaw
;
654 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RSSI
)) {
655 deltas
[optionalFieldCount
++] = (int32_t) blackboxCurrent
->rssi
- blackboxLast
->rssi
;
658 blackboxWriteTag8_8SVB(deltas
, optionalFieldCount
);
660 //Since gyros, accs and motors are noisy, base the prediction on the average of the history:
661 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
662 blackboxWriteSignedVB(blackboxHistory
[0]->gyroADC
[x
] - (blackboxHistory
[1]->gyroADC
[x
] + blackboxHistory
[2]->gyroADC
[x
]) / 2);
665 for (x
= 0; x
< XYZ_AXIS_COUNT
; x
++) {
666 blackboxWriteSignedVB(blackboxHistory
[0]->accSmooth
[x
] - (blackboxHistory
[1]->accSmooth
[x
] + blackboxHistory
[2]->accSmooth
[x
]) / 2);
669 for (x
= 0; x
< motorCount
; x
++) {
670 blackboxWriteSignedVB(blackboxHistory
[0]->motor
[x
] - (blackboxHistory
[1]->motor
[x
] + blackboxHistory
[2]->motor
[x
]) / 2);
673 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER
)) {
674 blackboxWriteSignedVB(blackboxCurrent
->servo
[5] - blackboxLast
->servo
[5]);
677 //Rotate our history buffers
678 blackboxHistory
[2] = blackboxHistory
[1];
679 blackboxHistory
[1] = blackboxHistory
[0];
680 blackboxHistory
[0] = ((blackboxHistory
[0] - blackboxHistoryRing
+ 1) % 3) + blackboxHistoryRing
;
683 /* Write the contents of the global "slowHistory" to the log as an "S" frame. Because this data is logged so
684 * infrequently, delta updates are not reasonable, so we log independent frames. */
685 static void writeSlowFrame(void)
689 blackboxWriteUnsignedVB(slowHistory
.flightModeFlags
);
690 blackboxWriteUnsignedVB(slowHistory
.stateFlags
);
691 blackboxWriteUnsignedVB(slowHistory
.failsafePhase
);
693 blackboxSlowFrameIterationTimer
= 0;
697 * Load rarely-changing values from the FC into the given structure
699 static void loadSlowState(blackboxSlowState_t
*slow
)
701 slow
->flightModeFlags
= flightModeFlags
;
702 slow
->stateFlags
= stateFlags
;
703 slow
->failsafePhase
= failsafePhase();
707 * If the data in the slow frame has changed, log a slow frame.
709 * If allowPeriodicWrite is true, the frame is also logged if it has been more than SLOW_FRAME_INTERVAL logging iterations
710 * since the field was last logged.
712 static void writeSlowFrameIfNeeded(bool allowPeriodicWrite
)
714 // Write the slow frame peridocially so it can be recovered if we ever lose sync
715 bool shouldWrite
= allowPeriodicWrite
&& blackboxSlowFrameIterationTimer
>= SLOW_FRAME_INTERVAL
;
718 loadSlowState(&slowHistory
);
720 blackboxSlowState_t newSlowState
;
722 loadSlowState(&newSlowState
);
724 // Only write a slow frame if it was different from the previous state
725 if (memcmp(&newSlowState
, &slowHistory
, sizeof(slowHistory
)) != 0) {
726 // Use the new state as our new history
727 memcpy(&slowHistory
, &newSlowState
, sizeof(slowHistory
));
737 static int gcd(int num
, int denom
)
743 return gcd(denom
, num
% denom
);
746 static void validateBlackboxConfig()
750 if (masterConfig
.blackbox_rate_num
== 0 || masterConfig
.blackbox_rate_denom
== 0
751 || masterConfig
.blackbox_rate_num
>= masterConfig
.blackbox_rate_denom
) {
752 masterConfig
.blackbox_rate_num
= 1;
753 masterConfig
.blackbox_rate_denom
= 1;
755 /* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
756 * itself more frequently)
758 div
= gcd(masterConfig
.blackbox_rate_num
, masterConfig
.blackbox_rate_denom
);
760 masterConfig
.blackbox_rate_num
/= div
;
761 masterConfig
.blackbox_rate_denom
/= div
;
764 if (masterConfig
.blackbox_device
>= BLACKBOX_DEVICE_END
) {
765 masterConfig
.blackbox_device
= BLACKBOX_DEVICE_SERIAL
;
770 * Start Blackbox logging if it is not already running. Intended to be called upon arming.
772 void startBlackbox(void)
774 if (blackboxState
== BLACKBOX_STATE_STOPPED
) {
775 validateBlackboxConfig();
777 if (!blackboxDeviceOpen()) {
778 blackboxSetState(BLACKBOX_STATE_DISABLED
);
782 memset(&gpsHistory
, 0, sizeof(gpsHistory
));
784 blackboxHistory
[0] = &blackboxHistoryRing
[0];
785 blackboxHistory
[1] = &blackboxHistoryRing
[1];
786 blackboxHistory
[2] = &blackboxHistoryRing
[2];
788 vbatReference
= vbatLatestADC
;
790 //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
793 * We use conditional tests to decide whether or not certain fields should be logged. Since our headers
794 * must always agree with the logged data, the results of these tests must not change during logging. So
797 blackboxBuildConditionCache();
800 * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
801 * it finally plays the beep for this arming event.
803 blackboxLastArmingBeep
= getArmingBeepTimeMicros();
805 blackboxSetState(BLACKBOX_STATE_SEND_HEADER
);
810 * Begin Blackbox shutdown.
812 void finishBlackbox(void)
814 if (blackboxState
== BLACKBOX_STATE_RUNNING
) {
815 blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END
, NULL
);
817 blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN
);
818 } else if (blackboxState
!= BLACKBOX_STATE_DISABLED
&& blackboxState
!= BLACKBOX_STATE_STOPPED
819 && blackboxState
!= BLACKBOX_STATE_SHUTTING_DOWN
) {
821 * We're shutting down in the middle of transmitting headers, so we can't log a "log completed" event.
822 * Just give the port back and stop immediately.
824 blackboxDeviceClose();
825 blackboxSetState(BLACKBOX_STATE_STOPPED
);
830 static void writeGPSHomeFrame()
834 blackboxWriteSignedVB(GPS_home
[0]);
835 blackboxWriteSignedVB(GPS_home
[1]);
836 //TODO it'd be great if we could grab the GPS current time and write that too
838 gpsHistory
.GPS_home
[0] = GPS_home
[0];
839 gpsHistory
.GPS_home
[1] = GPS_home
[1];
842 static void writeGPSFrame()
847 * If we're logging every frame, then a GPS frame always appears just after a frame with the
848 * currentTime timestamp in the log, so the reader can just use that timestamp for the GPS frame.
850 * If we're not logging every frame, we need to store the time of this GPS frame.
852 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME
)) {
853 // Predict the time of the last frame in the main log
854 blackboxWriteUnsignedVB(currentTime
- blackboxHistory
[1]->time
);
857 blackboxWriteUnsignedVB(GPS_numSat
);
858 blackboxWriteSignedVB(GPS_coord
[0] - gpsHistory
.GPS_home
[0]);
859 blackboxWriteSignedVB(GPS_coord
[1] - gpsHistory
.GPS_home
[1]);
860 blackboxWriteUnsignedVB(GPS_altitude
);
861 blackboxWriteUnsignedVB(GPS_speed
);
862 blackboxWriteUnsignedVB(GPS_ground_course
);
864 gpsHistory
.GPS_numSat
= GPS_numSat
;
865 gpsHistory
.GPS_coord
[0] = GPS_coord
[0];
866 gpsHistory
.GPS_coord
[1] = GPS_coord
[1];
871 * Fill the current state of the blackbox using values read from the flight controller
873 static void loadMainState(void)
875 blackboxMainState_t
*blackboxCurrent
= blackboxHistory
[0];
878 blackboxCurrent
->time
= currentTime
;
880 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
881 blackboxCurrent
->axisPID_P
[i
] = axisPID_P
[i
];
883 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
884 blackboxCurrent
->axisPID_I
[i
] = axisPID_I
[i
];
886 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
887 blackboxCurrent
->axisPID_D
[i
] = axisPID_D
[i
];
890 for (i
= 0; i
< 4; i
++) {
891 blackboxCurrent
->rcCommand
[i
] = rcCommand
[i
];
894 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
895 blackboxCurrent
->gyroADC
[i
] = gyroADC
[i
];
898 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
899 blackboxCurrent
->accSmooth
[i
] = accSmooth
[i
];
902 for (i
= 0; i
< motorCount
; i
++) {
903 blackboxCurrent
->motor
[i
] = motor
[i
];
906 blackboxCurrent
->vbatLatest
= vbatLatestADC
;
907 blackboxCurrent
->amperageLatest
= amperageLatestADC
;
910 for (i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
911 blackboxCurrent
->magADC
[i
] = magADC
[i
];
916 blackboxCurrent
->BaroAlt
= BaroAlt
;
920 // Store the raw sonar value without applying tilt correction
921 blackboxCurrent
->sonarRaw
= sonarRead();
924 blackboxCurrent
->rssi
= rssi
;
927 //Tail servo for tricopters
928 blackboxCurrent
->servo
[5] = servo
[5];
933 * Transmit the header information for the given field definitions. Transmitted header lines look like:
935 * H Field I name:a,b,c
936 * H Field I predictor:0,1,2
938 * For all header types, provide a "mainFrameChar" which is the name for the field and will be used to refer to it in the
939 * header (e.g. P, I etc). For blackboxDeltaField_t fields, also provide deltaFrameChar, otherwise set this to zero.
941 * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
942 * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
944 * Set xmitState.headerIndex to 0 and xmitState.u.fieldIndex to -1 before calling for the first time.
946 * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the
947 * fieldDefinition and secondCondition arrays.
949 * Returns true if there is still header left to transmit (so call again to continue transmission).
951 static bool sendFieldDefinition(char mainFrameChar
, char deltaFrameChar
, const void *fieldDefinitions
,
952 const void *secondFieldDefinition
, int fieldCount
, const uint8_t *conditions
, const uint8_t *secondCondition
)
954 const blackboxFieldDefinition_t
*def
;
956 unsigned int headerCount
;
957 static bool needComma
= false;
958 size_t definitionStride
= (char*) secondFieldDefinition
- (char*) fieldDefinitions
;
959 size_t conditionsStride
= (char*) secondCondition
- (char*) conditions
;
961 if (deltaFrameChar
) {
962 headerCount
= BLACKBOX_DELTA_FIELD_HEADER_COUNT
;
964 headerCount
= BLACKBOX_SIMPLE_FIELD_HEADER_COUNT
;
968 * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit
971 if (xmitState
.u
.fieldIndex
== -1) {
972 if (xmitState
.headerIndex
>= headerCount
) {
973 return false; //Someone probably called us again after we had already completed transmission
976 charsWritten
= blackboxPrintf("H Field %c %s:", xmitState
.headerIndex
>= BLACKBOX_SIMPLE_FIELD_HEADER_COUNT
? deltaFrameChar
: mainFrameChar
, blackboxFieldHeaderNames
[xmitState
.headerIndex
]);
978 xmitState
.u
.fieldIndex
++;
983 for (; xmitState
.u
.fieldIndex
< fieldCount
&& charsWritten
< blackboxWriteChunkSize
; xmitState
.u
.fieldIndex
++) {
984 def
= (const blackboxFieldDefinition_t
*) ((const char*)fieldDefinitions
+ definitionStride
* xmitState
.u
.fieldIndex
);
986 if (!conditions
|| testBlackboxCondition(conditions
[conditionsStride
* xmitState
.u
.fieldIndex
])) {
994 // The first header is a field name
995 if (xmitState
.headerIndex
== 0) {
996 charsWritten
+= blackboxPrint(def
->name
);
998 // Do we need to print an index in brackets after the name?
999 if (def
->fieldNameIndex
!= -1) {
1000 charsWritten
+= blackboxPrintf("[%d]", def
->fieldNameIndex
);
1003 //The other headers are integers
1004 charsWritten
+= blackboxPrintf("%d", def
->arr
[xmitState
.headerIndex
- 1]);
1009 // Did we complete this line?
1010 if (xmitState
.u
.fieldIndex
== fieldCount
) {
1011 blackboxWrite('\n');
1012 xmitState
.headerIndex
++;
1013 xmitState
.u
.fieldIndex
= -1;
1016 return xmitState
.headerIndex
< headerCount
;
1020 * Transmit a portion of the system information headers. Call the first time with xmitState.headerIndex == 0. Returns
1021 * true iff transmission is complete, otherwise call again later to continue transmission.
1023 static bool blackboxWriteSysinfo()
1025 if (xmitState
.headerIndex
== 0) {
1026 xmitState
.u
.serialBudget
= 0;
1027 xmitState
.headerIndex
= 1;
1030 // How many bytes can we afford to transmit this loop?
1031 xmitState
.u
.serialBudget
= MIN(xmitState
.u
.serialBudget
+ blackboxWriteChunkSize
, 64);
1033 // Most headers will consume at least 20 bytes so wait until we've built up that much link budget
1034 if (xmitState
.u
.serialBudget
< 20) {
1038 switch (xmitState
.headerIndex
) {
1040 //Shouldn't ever get here
1043 xmitState
.u
.serialBudget
-= blackboxPrint("H Firmware type:Cleanflight\n");
1046 xmitState
.u
.serialBudget
-= blackboxPrintf("H Firmware revision:%s\n", shortGitRevision
);
1049 xmitState
.u
.serialBudget
-= blackboxPrintf("H Firmware date:%s %s\n", buildDate
, buildTime
);
1052 xmitState
.u
.serialBudget
-= blackboxPrintf("H P interval:%d/%d\n", masterConfig
.blackbox_rate_num
, masterConfig
.blackbox_rate_denom
);
1055 xmitState
.u
.serialBudget
-= blackboxPrintf("H rcRate:%d\n", masterConfig
.controlRateProfiles
[masterConfig
.current_profile_index
].rcRate8
);
1058 xmitState
.u
.serialBudget
-= blackboxPrintf("H minthrottle:%d\n", masterConfig
.escAndServoConfig
.minthrottle
);
1061 xmitState
.u
.serialBudget
-= blackboxPrintf("H maxthrottle:%d\n", masterConfig
.escAndServoConfig
.maxthrottle
);
1064 xmitState
.u
.serialBudget
-= blackboxPrintf("H gyro.scale:0x%x\n", castFloatBytesToInt(gyro
.scale
));
1067 xmitState
.u
.serialBudget
-= blackboxPrintf("H acc_1G:%u\n", acc_1G
);
1070 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT
)) {
1071 xmitState
.u
.serialBudget
-= blackboxPrintf("H vbatscale:%u\n", masterConfig
.batteryConfig
.vbatscale
);
1073 xmitState
.headerIndex
+= 2; // Skip the next two vbat fields too
1077 xmitState
.u
.serialBudget
-= blackboxPrintf("H vbatcellvoltage:%u,%u,%u\n", masterConfig
.batteryConfig
.vbatmincellvoltage
,
1078 masterConfig
.batteryConfig
.vbatwarningcellvoltage
, masterConfig
.batteryConfig
.vbatmaxcellvoltage
);
1081 xmitState
.u
.serialBudget
-= blackboxPrintf("H vbatref:%u\n", vbatReference
);
1084 //Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too:
1085 if (feature(FEATURE_CURRENT_METER
)) {
1086 xmitState
.u
.serialBudget
-= blackboxPrintf("H currentMeter:%d,%d\n", masterConfig
.batteryConfig
.currentMeterOffset
, masterConfig
.batteryConfig
.currentMeterScale
);
1093 xmitState
.headerIndex
++;
1098 * Write the given event to the log immediately
1100 void blackboxLogEvent(FlightLogEvent event
, flightLogEventData_t
*data
)
1102 if (blackboxState
!= BLACKBOX_STATE_RUNNING
) {
1106 //Shared header for event frames
1108 blackboxWrite(event
);
1110 //Now serialize the data for this specific frame type
1112 case FLIGHT_LOG_EVENT_SYNC_BEEP
:
1113 blackboxWriteUnsignedVB(data
->syncBeep
.time
);
1115 case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START
:
1116 blackboxWrite(data
->autotuneCycleStart
.phase
);
1117 blackboxWrite(data
->autotuneCycleStart
.cycle
| (data
->autotuneCycleStart
.rising
? 0x80 : 0));
1118 blackboxWrite(data
->autotuneCycleStart
.p
);
1119 blackboxWrite(data
->autotuneCycleStart
.i
);
1120 blackboxWrite(data
->autotuneCycleStart
.d
);
1122 case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT
:
1123 blackboxWrite(data
->autotuneCycleResult
.flags
);
1124 blackboxWrite(data
->autotuneCycleStart
.p
);
1125 blackboxWrite(data
->autotuneCycleStart
.i
);
1126 blackboxWrite(data
->autotuneCycleStart
.d
);
1128 case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS
:
1129 blackboxWriteS16(data
->autotuneTargets
.currentAngle
);
1130 blackboxWrite((uint8_t) data
->autotuneTargets
.targetAngle
);
1131 blackboxWrite((uint8_t) data
->autotuneTargets
.targetAngleAtPeak
);
1132 blackboxWriteS16(data
->autotuneTargets
.firstPeakAngle
);
1133 blackboxWriteS16(data
->autotuneTargets
.secondPeakAngle
);
1135 case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT
:
1136 if (data
->inflightAdjustment
.floatFlag
) {
1137 blackboxWrite(data
->inflightAdjustment
.adjustmentFunction
+ FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG
);
1138 blackboxWriteFloat(data
->inflightAdjustment
.newFloatValue
);
1140 blackboxWrite(data
->inflightAdjustment
.adjustmentFunction
);
1141 blackboxWriteSignedVB(data
->inflightAdjustment
.newValue
);
1144 case FLIGHT_LOG_EVENT_LOG_END
:
1145 blackboxPrint("End of log");
1151 /* 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 */
1152 static void blackboxCheckAndLogArmingBeep()
1154 flightLogEvent_syncBeep_t eventData
;
1156 // Use != so that we can still detect a change if the counter wraps
1157 if (getArmingBeepTimeMicros() != blackboxLastArmingBeep
) {
1158 blackboxLastArmingBeep
= getArmingBeepTimeMicros();
1160 eventData
.time
= blackboxLastArmingBeep
;
1162 blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP
, (flightLogEventData_t
*) &eventData
);
1167 * 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
1168 * the portion of logged loop iterations.
1170 static bool blackboxShouldLogPFrame(uint32_t pFrameIndex
)
1172 /* Adding a magic shift of "masterConfig.blackbox_rate_num - 1" in here creates a better spread of
1173 * recorded / skipped frames when the I frame's position is considered:
1175 return (pFrameIndex
+ masterConfig
.blackbox_rate_num
- 1) % masterConfig
.blackbox_rate_denom
< masterConfig
.blackbox_rate_num
;
1178 // Called once every FC loop in order to log the current state
1179 static void blackboxLogIteration()
1181 // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
1182 if (blackboxPFrameIndex
== 0) {
1184 * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
1185 * an additional item to write at the same time)
1187 writeSlowFrameIfNeeded(false);
1192 blackboxCheckAndLogArmingBeep();
1194 if (blackboxShouldLogPFrame(blackboxPFrameIndex
)) {
1196 * We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
1197 * So only log slow frames during loop iterations where we log a main frame.
1199 writeSlowFrameIfNeeded(true);
1205 if (feature(FEATURE_GPS
)) {
1207 * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the
1208 * GPS home position.
1210 * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
1211 * still be interpreted correctly.
1213 if (GPS_home
[0] != gpsHistory
.GPS_home
[0] || GPS_home
[1] != gpsHistory
.GPS_home
[1]
1214 || (blackboxPFrameIndex
== BLACKBOX_I_INTERVAL
/ 2 && blackboxIFrameIndex
% 128 == 0)) {
1216 writeGPSHomeFrame();
1218 } else if (GPS_numSat
!= gpsHistory
.GPS_numSat
|| GPS_coord
[0] != gpsHistory
.GPS_coord
[0]
1219 || GPS_coord
[1] != gpsHistory
.GPS_coord
[1]) {
1220 //We could check for velocity changes as well but I doubt it changes independent of position
1227 //Flush every iteration so that our runtime variance is minimized
1228 blackboxDeviceFlush();
1230 blackboxSlowFrameIterationTimer
++;
1231 blackboxIteration
++;
1232 blackboxPFrameIndex
++;
1234 if (blackboxPFrameIndex
== BLACKBOX_I_INTERVAL
) {
1235 blackboxPFrameIndex
= 0;
1236 blackboxIFrameIndex
++;
1241 * Call each flight loop iteration to perform blackbox logging.
1243 void handleBlackbox(void)
1247 switch (blackboxState
) {
1248 case BLACKBOX_STATE_SEND_HEADER
:
1249 //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
1252 * Once the UART has had time to init, transmit the header in chunks so we don't overflow our transmit
1255 if (millis() > xmitState
.u
.startTime
+ 100) {
1256 for (i
= 0; i
< blackboxWriteChunkSize
&& blackboxHeader
[xmitState
.headerIndex
] != '\0'; i
++, xmitState
.headerIndex
++) {
1257 blackboxWrite(blackboxHeader
[xmitState
.headerIndex
]);
1260 if (blackboxHeader
[xmitState
.headerIndex
] == '\0') {
1261 blackboxSetState(BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER
);
1265 case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER
:
1266 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1267 if (!sendFieldDefinition('I', 'P', blackboxMainFields
, blackboxMainFields
+ 1, ARRAY_LENGTH(blackboxMainFields
),
1268 &blackboxMainFields
[0].condition
, &blackboxMainFields
[1].condition
)) {
1270 if (feature(FEATURE_GPS
)) {
1271 blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADER
);
1274 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER
);
1278 case BLACKBOX_STATE_SEND_GPS_H_HEADER
:
1279 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1280 if (!sendFieldDefinition('H', 0, blackboxGpsHFields
, blackboxGpsHFields
+ 1, ARRAY_LENGTH(blackboxGpsHFields
),
1282 blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADER
);
1285 case BLACKBOX_STATE_SEND_GPS_G_HEADER
:
1286 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1287 if (!sendFieldDefinition('G', 0, blackboxGpsGFields
, blackboxGpsGFields
+ 1, ARRAY_LENGTH(blackboxGpsGFields
),
1288 &blackboxGpsGFields
[0].condition
, &blackboxGpsGFields
[1].condition
)) {
1289 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER
);
1293 case BLACKBOX_STATE_SEND_SLOW_HEADER
:
1294 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1295 if (!sendFieldDefinition('S', 0, blackboxSlowFields
, blackboxSlowFields
+ 1, ARRAY_LENGTH(blackboxSlowFields
),
1297 blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO
);
1300 case BLACKBOX_STATE_SEND_SYSINFO
:
1301 //On entry of this state, xmitState.headerIndex is 0
1303 //Keep writing chunks of the system info headers until it returns true to signal completion
1304 if (blackboxWriteSysinfo()) {
1305 blackboxSetState(BLACKBOX_STATE_RUNNING
);
1308 case BLACKBOX_STATE_RUNNING
:
1309 // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
1311 blackboxLogIteration();
1313 case BLACKBOX_STATE_SHUTTING_DOWN
:
1314 //On entry of this state, startTime is set and a flush is performed
1317 * Wait for the log we've transmitted to make its way to the logger before we release the serial port,
1318 * since releasing the port clears the Tx buffer.
1320 * Don't wait longer than it could possibly take if something funky happens.
1322 if (millis() > xmitState
.u
.startTime
+ BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS
|| blackboxDeviceFlush()) {
1323 blackboxDeviceClose();
1324 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1331 // Did we run out of room on the device? Stop!
1332 if (isBlackboxDeviceFull()) {
1333 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1337 static bool canUseBlackboxWithCurrentConfiguration(void)
1339 return feature(FEATURE_BLACKBOX
);
1343 * Call during system startup to initialize the blackbox.
1345 void initBlackbox(void)
1347 if (canUseBlackboxWithCurrentConfiguration()) {
1348 blackboxSetState(BLACKBOX_STATE_STOPPED
);
1350 blackboxSetState(BLACKBOX_STATE_DISABLED
);