?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
[4.4.2] Remove 15 m/s limit on estimated vario (#12788)
[betaflight.git] / src / main / blackbox / blackbox.c
blob294dae98931e84ff6a07914a96b1428cf65363fa
1 /*
2 * This file is part of Cleanflight and Betaflight.
3 *
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
9 *
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
17 *
18 * If not, see <http://www.gnu.org/licenses/>.
19 */
20
21 #include <stdbool.h>
22 #include <stdint.h>
23 #include <string.h>
24 #include <math.h>
25
26 #include "platform.h"
27
28 #ifdef USE_BLACKBOX
29
30 #include "blackbox.h"
31 #include "blackbox_encoding.h"
32 #include "blackbox_fielddefs.h"
33 #include "blackbox_io.h"
34
35 #include "build/build_config.h"
36 #include "build/debug.h"
37 #include "build/version.h"
38
39 #include "common/axis.h"
40 #include "common/encoding.h"
41 #include "common/maths.h"
42 #include "common/time.h"
43 #include "common/utils.h"
44
45 #include "config/config.h"
46 #include "config/feature.h"
47
48 #include "drivers/compass/compass.h"
49 #include "drivers/sensor.h"
50 #include "drivers/time.h"
51
52 #include "fc/board_info.h"
53 #include "fc/controlrate_profile.h"
54 #include "fc/parameter_names.h"
55 #include "fc/rc.h"
56 #include "fc/rc_controls.h"
57 #include "fc/rc_modes.h"
58 #include "fc/runtime_config.h"
59
60 #include "flight/failsafe.h"
61 #include "flight/mixer.h"
62 #include "flight/pid.h"
63 #include "flight/rpm_filter.h"
64 #include "flight/servos.h"
65 #include "flight/gps_rescue.h"
66 #include "flight/position.h"
67
68 #include "io/beeper.h"
69 #include "io/gps.h"
70 #include "io/serial.h"
71
72 #include "pg/pg.h"
73 #include "pg/pg_ids.h"
74 #include "pg/motor.h"
75 #include "pg/rx.h"
76
77 #include "rx/rx.h"
78
79 #include "sensors/acceleration.h"
80 #include "sensors/barometer.h"
81 #include "sensors/battery.h"
82 #include "sensors/compass.h"
83 #include "sensors/gyro.h"
84 #include "sensors/rangefinder.h"
85
86 #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
87 #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH
88 #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT)
89 #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_SDCARD
90 #else
91 #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_SERIAL
92 #endif
93
94 PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 3);
95
96 PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
97 .fields_disabled_mask = 0, // default log all fields
98 .sample_rate = BLACKBOX_RATE_QUARTER,
99 .device = DEFAULT_BLACKBOX_DEVICE,
100 .mode = BLACKBOX_MODE_NORMAL,
101 .high_resolution = false
102 );
103
104 STATIC_ASSERT((sizeof(blackboxConfig()->fields_disabled_mask) * 8) >= FLIGHT_LOG_FIELD_SELECT_COUNT, too_many_flight_log_fields_selections);
105
106 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
107
108 // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
109
110 #define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
111 #define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)
112 #define CONDITION(x) CONCAT(FLIGHT_LOG_FIELD_CONDITION_, x)
113 #define FIELD_SELECT(x) CONCAT(FLIGHT_LOG_FIELD_SELECT_, x)
114 #define UNSIGNED FLIGHT_LOG_FIELD_UNSIGNED
115 #define SIGNED FLIGHT_LOG_FIELD_SIGNED
116
117 static const char blackboxHeader[] =
118 "H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
119 "H Data version:2\n";
120
121 static const char* const blackboxFieldHeaderNames[] = {
122 "name",
123 "signed",
124 "predictor",
125 "encoding",
126 "predictor",
127 "encoding"
128 };
129
130 /* All field definition structs should look like this (but with longer arrs): */
131 typedef struct blackboxFieldDefinition_s {
132 const char *name;
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;
135
136 // Each member of this array will be the value to print for this field for the given header index
137 uint8_t arr[1];
138 } blackboxFieldDefinition_t;
139
140 #define BLACKBOX_DELTA_FIELD_HEADER_COUNT ARRAYLEN(blackboxFieldHeaderNames)
141 #define BLACKBOX_SIMPLE_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
142 #define BLACKBOX_CONDITIONAL_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
143
144 typedef struct blackboxSimpleFieldDefinition_s {
145 const char *name;
146 int8_t fieldNameIndex;
147
148 uint8_t isSigned;
149 uint8_t predict;
150 uint8_t encode;
151 } blackboxSimpleFieldDefinition_t;
152
153 typedef struct blackboxConditionalFieldDefinition_s {
154 const char *name;
155 int8_t fieldNameIndex;
156
157 uint8_t isSigned;
158 uint8_t predict;
159 uint8_t encode;
160 uint8_t condition; // Decide whether this field should appear in the log
161 } blackboxConditionalFieldDefinition_t;
162
163 typedef struct blackboxDeltaFieldDefinition_s {
164 const char *name;
165 int8_t fieldNameIndex;
166
167 uint8_t isSigned;
168 uint8_t Ipredict;
169 uint8_t Iencode;
170 uint8_t Ppredict;
171 uint8_t Pencode;
172 uint8_t condition; // Decide whether this field should appear in the log
173 } blackboxDeltaFieldDefinition_t;
174
175 /**
176 * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
177 * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
178 * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
179 * the encoding we've promised here).
180 */
181 static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
182 /* loopIteration doesn't appear in P frames since it always increments */
183 {"loopIteration",-1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC), .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)},
184 /* Time advances pretty steadily so the P-frame prediction is a straight line */
185 {"time", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(STRAIGHT_LINE), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
186 {"axisP", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
187 {"axisP", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
188 {"axisP", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
189 /* I terms get special packed encoding in P frames: */
190 {"axisI", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(PID)},
191 {"axisI", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(PID)},
192 {"axisI", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(PID)},
193 {"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
194 {"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
195 {"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
196 {"axisF", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
197 {"axisF", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
198 {"axisF", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
199 /* rcCommands are encoded together as a group in P-frames: */
200 {"rcCommand", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
201 {"rcCommand", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
202 {"rcCommand", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
203 {"rcCommand", 3, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
204
205 // setpoint - define 4 fields like rcCommand to use the same encoding. setpoint[4] contains the mixer throttle
206 {"setpoint", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
207 {"setpoint", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
208 {"setpoint", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
209 {"setpoint", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
210
211 {"vbatLatest", -1, UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(VBAT)},
212 {"amperageLatest",-1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(AMPERAGE_ADC)},
213
214 #ifdef USE_MAG
215 {"magADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)},
216 {"magADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)},
217 {"magADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)},
218 #endif
219 #ifdef USE_BARO
220 {"baroAlt", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(BARO)},
221 #endif
222 #ifdef USE_RANGEFINDER
223 {"surfaceRaw", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(RANGEFINDER)},
224 #endif
225 {"rssi", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(RSSI)},
226
227 /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
228 {"gyroADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(GYRO)},
229 {"gyroADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(GYRO)},
230 {"gyroADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(GYRO)},
231 {"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
232 {"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
233 {"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
234 {"debug", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
235 {"debug", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
236 {"debug", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
237 {"debug", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
238 /* 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): */
239 {"motor", 0, UNSIGNED, .Ipredict = PREDICT(MINMOTOR), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
240 /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
241 {"motor", 1, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_2)},
242 {"motor", 2, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_3)},
243 {"motor", 3, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_4)},
244 {"motor", 4, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_5)},
245 {"motor", 5, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_6)},
246 {"motor", 6, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)},
247 {"motor", 7, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)},
248
249 /* Tricopter tail servo */
250 {"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)}
251 };
252
253 #ifdef USE_GPS
254 // GPS position/vel frame
255 static const blackboxConditionalFieldDefinition_t blackboxGpsGFields[] = {
256 {"time", -1, UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)},
257 {"GPS_numSat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
258 {"GPS_coord", 0, SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
259 {"GPS_coord", 1, SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
260 {"GPS_altitude", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
261 {"GPS_speed", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
262 {"GPS_ground_course", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}
263 };
264
265 // GPS home frame
266 static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = {
267 {"GPS_home", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
268 {"GPS_home", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}
269 };
270 #endif
271
272 // Rarely-updated fields
273 static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
274 {"flightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
275 {"stateFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
276
277 {"failsafePhase", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)},
278 {"rxSignalReceived", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)},
279 {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)}
280 };
281
282 typedef enum BlackboxState {
283 BLACKBOX_STATE_DISABLED = 0,
284 BLACKBOX_STATE_STOPPED,
285 BLACKBOX_STATE_PREPARE_LOG_FILE,
286 BLACKBOX_STATE_SEND_HEADER,
287 BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER,
288 BLACKBOX_STATE_SEND_GPS_H_HEADER,
289 BLACKBOX_STATE_SEND_GPS_G_HEADER,
290 BLACKBOX_STATE_SEND_SLOW_HEADER,
291 BLACKBOX_STATE_SEND_SYSINFO,
292 BLACKBOX_STATE_CACHE_FLUSH,
293 BLACKBOX_STATE_PAUSED,
294 BLACKBOX_STATE_RUNNING,
295 BLACKBOX_STATE_SHUTTING_DOWN,
296 BLACKBOX_STATE_START_ERASE,
297 BLACKBOX_STATE_ERASING,
298 BLACKBOX_STATE_ERASED
299 } BlackboxState;
300
301
302 typedef struct blackboxMainState_s {
303 uint32_t time;
304
305 int32_t axisPID_P[XYZ_AXIS_COUNT];
306 int32_t axisPID_I[XYZ_AXIS_COUNT];
307 int32_t axisPID_D[XYZ_AXIS_COUNT];
308 int32_t axisPID_F[XYZ_AXIS_COUNT];
309
310 int16_t rcCommand[4];
311 int16_t setpoint[4];
312 int16_t gyroADC[XYZ_AXIS_COUNT];
313 int16_t accADC[XYZ_AXIS_COUNT];
314 int16_t debug[DEBUG16_VALUE_COUNT];
315 int16_t motor[MAX_SUPPORTED_MOTORS];
316 int16_t servo[MAX_SUPPORTED_SERVOS];
317
318 uint16_t vbatLatest;
319 int32_t amperageLatest;
320
321 #ifdef USE_BARO
322 int32_t baroAlt;
323 #endif
324 #ifdef USE_MAG
325 int16_t magADC[XYZ_AXIS_COUNT];
326 #endif
327 #ifdef USE_RANGEFINDER
328 int32_t surfaceRaw;
329 #endif
330 uint16_t rssi;
331 } blackboxMainState_t;
332
333 typedef struct blackboxGpsState_s {
334 int32_t GPS_home[2];
335 int32_t GPS_coord[2];
336 uint8_t GPS_numSat;
337 } blackboxGpsState_t;
338
339 // This data is updated really infrequently:
340 typedef struct blackboxSlowState_s {
341 uint32_t flightModeFlags; // extend this data size (from uint16_t)
342 uint8_t stateFlags;
343 uint8_t failsafePhase;
344 bool rxSignalReceived;
345 bool rxFlightChannelsValid;
346 } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
347
348 //From rc_controls.c
349 extern boxBitmask_t rcModeActivationMask;
350
351 static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
352
353 static uint32_t blackboxLastArmingBeep = 0;
354 static uint32_t blackboxLastFlightModeFlags = 0; // New event tracking of flight modes
355
356 static struct {
357 uint32_t headerIndex;
358
359 /* Since these fields are used during different blackbox states (never simultaneously) we can
360 * overlap them to save on RAM
361 */
362 union {
363 int fieldIndex;
364 uint32_t startTime;
365 } u;
366 } xmitState;
367
368 // Cache for FLIGHT_LOG_FIELD_CONDITION_* test results:
369 static uint32_t blackboxConditionCache;
370
371 STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions);
372
373 static uint32_t blackboxIteration;
374 static uint16_t blackboxLoopIndex;
375 static uint16_t blackboxPFrameIndex;
376 static uint16_t blackboxIFrameIndex;
377 // number of flight loop iterations before logging I-frame
378 // typically 32 for 1kHz loop, 64 for 2kHz loop etc
379 STATIC_UNIT_TESTED int16_t blackboxIInterval = 0;
380 // number of flight loop iterations before logging P-frame
381 STATIC_UNIT_TESTED int8_t blackboxPInterval = 0;
382 STATIC_UNIT_TESTED int32_t blackboxSInterval = 0;
383 STATIC_UNIT_TESTED int32_t blackboxSlowFrameIterationTimer;
384 static bool blackboxLoggedAnyFrames;
385 static float blackboxHighResolutionScale;
386
387 /*
388 * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
389 * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs
390 * to encode:
391 */
392 static uint16_t vbatReference;
393
394 static blackboxGpsState_t gpsHistory;
395 static blackboxSlowState_t slowHistory;
396
397 // Keep a history of length 2, plus a buffer for MW to store the new values into
398 static blackboxMainState_t blackboxHistoryRing[3];
399
400 // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
401 static blackboxMainState_t* blackboxHistory[3];
402
403 static bool blackboxModeActivationConditionPresent = false;
404
405 /**
406 * Return true if it is safe to edit the Blackbox configuration.
407 */
408 bool blackboxMayEditConfig(void)
409 {
410 return blackboxState <= BLACKBOX_STATE_STOPPED;
411 }
412
413 static bool blackboxIsOnlyLoggingIntraframes(void)
414 {
415 return blackboxPInterval == 0;
416 }
417
418 static bool isFieldEnabled(FlightLogFieldSelect_e field)
419 {
420 return (blackboxConfig()->fields_disabled_mask & (1 << field)) == 0;
421 }
422
423 static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
424 {
425 switch (condition) {
426 case CONDITION(ALWAYS):
427 return true;
428
429 case CONDITION(AT_LEAST_MOTORS_1):
430 case CONDITION(AT_LEAST_MOTORS_2):
431 case CONDITION(AT_LEAST_MOTORS_3):
432 case CONDITION(AT_LEAST_MOTORS_4):
433 case CONDITION(AT_LEAST_MOTORS_5):
434 case CONDITION(AT_LEAST_MOTORS_6):
435 case CONDITION(AT_LEAST_MOTORS_7):
436 case CONDITION(AT_LEAST_MOTORS_8):
437 return (getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1) && isFieldEnabled(FIELD_SELECT(MOTOR));
438
439 case CONDITION(TRICOPTER):
440 return (mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && isFieldEnabled(FIELD_SELECT(MOTOR));
441
442 case CONDITION(PID):
443 return isFieldEnabled(FIELD_SELECT(PID));
444
445 case CONDITION(NONZERO_PID_D_0):
446 case CONDITION(NONZERO_PID_D_1):
447 case CONDITION(NONZERO_PID_D_2):
448 return (currentPidProfile->pid[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0].D != 0) && isFieldEnabled(FIELD_SELECT(PID));
449
450 case CONDITION(RC_COMMANDS):
451 return isFieldEnabled(FIELD_SELECT(RC_COMMANDS));
452
453 case CONDITION(SETPOINT):
454 return isFieldEnabled(FIELD_SELECT(SETPOINT));
455
456 case CONDITION(MAG):
457 #ifdef USE_MAG
458 return sensors(SENSOR_MAG) && isFieldEnabled(FIELD_SELECT(MAG));
459 #else
460 return false;
461 #endif
462
463 case CONDITION(BARO):
464 #ifdef USE_BARO
465 return sensors(SENSOR_BARO) && isFieldEnabled(FIELD_SELECT(ALTITUDE));
466 #else
467 return false;
468 #endif
469
470 case CONDITION(VBAT):
471 return (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) && isFieldEnabled(FIELD_SELECT(BATTERY));
472
473 case CONDITION(AMPERAGE_ADC):
474 return (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) && (batteryConfig()->currentMeterSource != CURRENT_METER_VIRTUAL) && isFieldEnabled(FIELD_SELECT(BATTERY));
475
476 case CONDITION(RANGEFINDER):
477 #ifdef USE_RANGEFINDER
478 return sensors(SENSOR_RANGEFINDER) && isFieldEnabled(FIELD_SELECT(ALTITUDE));
479 #else
480 return false;
481 #endif
482
483 case CONDITION(RSSI):
484 return isRssiConfigured() && isFieldEnabled(FIELD_SELECT(RSSI));
485
486 case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
487 return blackboxPInterval != blackboxIInterval;
488
489 case CONDITION(GYRO):
490 return isFieldEnabled(FIELD_SELECT(GYRO));
491
492 case CONDITION(ACC):
493 return sensors(SENSOR_ACC) && isFieldEnabled(FIELD_SELECT(ACC));
494
495 case CONDITION(DEBUG_LOG):
496 return (debugMode != DEBUG_NONE) && isFieldEnabled(FIELD_SELECT(DEBUG_LOG));
497
498 case CONDITION(NEVER):
499 return false;
500
501 default:
502 return false;
503 }
504 }
505
506 static void blackboxBuildConditionCache(void)
507 {
508 blackboxConditionCache = 0;
509 for (FlightLogFieldCondition cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
510 if (testBlackboxConditionUncached(cond)) {
511 blackboxConditionCache |= 1 << cond;
512 }
513 }
514 }
515
516 static bool testBlackboxCondition(FlightLogFieldCondition condition)
517 {
518 return (blackboxConditionCache & (1 << condition)) != 0;
519 }
520
521 static void blackboxSetState(BlackboxState newState)
522 {
523 //Perform initial setup required for the new state
524 switch (newState) {
525 case BLACKBOX_STATE_PREPARE_LOG_FILE:
526 blackboxLoggedAnyFrames = false;
527 break;
528 case BLACKBOX_STATE_SEND_HEADER:
529 blackboxHeaderBudget = 0;
530 xmitState.headerIndex = 0;
531 xmitState.u.startTime = millis();
532 break;
533 case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
534 case BLACKBOX_STATE_SEND_GPS_G_HEADER:
535 case BLACKBOX_STATE_SEND_GPS_H_HEADER:
536 case BLACKBOX_STATE_SEND_SLOW_HEADER:
537 xmitState.headerIndex = 0;
538 xmitState.u.fieldIndex = -1;
539 break;
540 case BLACKBOX_STATE_SEND_SYSINFO:
541 xmitState.headerIndex = 0;
542 break;
543 case BLACKBOX_STATE_RUNNING:
544 blackboxSlowFrameIterationTimer = blackboxSInterval; //Force a slow frame to be written on the first iteration
545 break;
546 case BLACKBOX_STATE_SHUTTING_DOWN:
547 xmitState.u.startTime = millis();
548 break;
549 default:
550 ;
551 }
552 blackboxState = newState;
553 }
554
555 static void writeIntraframe(void)
556 {
557 blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
558
559 blackboxWrite('I');
560
561 blackboxWriteUnsignedVB(blackboxIteration);
562 blackboxWriteUnsignedVB(blackboxCurrent->time);
563
564 if (testBlackboxCondition(CONDITION(PID))) {
565 blackboxWriteSignedVBArray(blackboxCurrent->axisPID_P, XYZ_AXIS_COUNT);
566 blackboxWriteSignedVBArray(blackboxCurrent->axisPID_I, XYZ_AXIS_COUNT);
567
568 // Don't bother writing the current D term if the corresponding PID setting is zero
569 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
570 if (testBlackboxCondition(CONDITION(NONZERO_PID_D_0) + x)) {
571 blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]);
572 }
573 }
574
575 blackboxWriteSignedVBArray(blackboxCurrent->axisPID_F, XYZ_AXIS_COUNT);
576 }
577
578 if (testBlackboxCondition(CONDITION(RC_COMMANDS))) {
579 // Write roll, pitch and yaw first:
580 blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3);
581
582 /*
583 * Write the throttle separately from the rest of the RC data as it's unsigned.
584 * Throttle lies in range [PWM_RANGE_MIN..PWM_RANGE_MAX]:
585 */
586 blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE]);
587 }
588
589 if (testBlackboxCondition(CONDITION(SETPOINT))) {
590 // Write setpoint roll, pitch, yaw, and throttle
591 blackboxWriteSigned16VBArray(blackboxCurrent->setpoint, 4);
592 }
593
594 if (testBlackboxCondition(CONDITION(VBAT))) {
595 /*
596 * Our voltage is expected to decrease over the course of the flight, so store our difference from
597 * the reference:
598 *
599 * Write 14 bits even if the number is negative (which would otherwise result in 32 bits)
600 */
601 blackboxWriteUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF);
602 }
603
604 if (testBlackboxCondition(CONDITION(AMPERAGE_ADC))) {
605 // 12bit value directly from ADC
606 blackboxWriteSignedVB(blackboxCurrent->amperageLatest);
607 }
608
609 #ifdef USE_MAG
610 if (testBlackboxCondition(CONDITION(MAG))) {
611 blackboxWriteSigned16VBArray(blackboxCurrent->magADC, XYZ_AXIS_COUNT);
612 }
613 #endif
614
615 #ifdef USE_BARO
616 if (testBlackboxCondition(CONDITION(BARO))) {
617 blackboxWriteSignedVB(blackboxCurrent->baroAlt);
618 }
619 #endif
620
621 #ifdef USE_RANGEFINDER
622 if (testBlackboxCondition(CONDITION(RANGEFINDER))) {
623 blackboxWriteSignedVB(blackboxCurrent->surfaceRaw);
624 }
625 #endif
626
627 if (testBlackboxCondition(CONDITION(RSSI))) {
628 blackboxWriteUnsignedVB(blackboxCurrent->rssi);
629 }
630
631 if (testBlackboxCondition(CONDITION(GYRO))) {
632 blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
633 }
634
635 if (testBlackboxCondition(CONDITION(ACC))) {
636 blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
637 }
638
639 if (testBlackboxCondition(CONDITION(DEBUG_LOG))) {
640 blackboxWriteSigned16VBArray(blackboxCurrent->debug, DEBUG16_VALUE_COUNT);
641 }
642
643 if (isFieldEnabled(FIELD_SELECT(MOTOR))) {
644 //Motors can be below minimum output when disarmed, but that doesn't happen much
645 blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - getMotorOutputLow());
646
647 //Motors tend to be similar to each other so use the first motor's value as a predictor of the others
648 const int motorCount = getMotorCount();
649 for (int x = 1; x < motorCount; x++) {
650 blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
651 }
652
653 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) {
654 //Assume the tail spends most of its time around the center
655 blackboxWriteSignedVB(blackboxCurrent->servo[5] - 1500);
656 }
657 }
658
659 //Rotate our history buffers:
660
661 //The current state becomes the new "before" state
662 blackboxHistory[1] = blackboxHistory[0];
663 //And since we have no other history, we also use it for the "before, before" state
664 blackboxHistory[2] = blackboxHistory[0];
665 //And advance the current state over to a blank space ready to be filled
666 blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
667
668 blackboxLoggedAnyFrames = true;
669 }
670
671 static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHistory, int count)
672 {
673 int16_t *curr = (int16_t*) ((char*) (blackboxHistory[0]) + arrOffsetInHistory);
674 int16_t *prev1 = (int16_t*) ((char*) (blackboxHistory[1]) + arrOffsetInHistory);
675 int16_t *prev2 = (int16_t*) ((char*) (blackboxHistory[2]) + arrOffsetInHistory);
676
677 for (int i = 0; i < count; i++) {
678 // Predictor is the average of the previous two history states
679 int32_t predictor = (prev1[i] + prev2[i]) / 2;
680
681 blackboxWriteSignedVB(curr[i] - predictor);
682 }
683 }
684
685 static void writeInterframe(void)
686 {
687 blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
688 blackboxMainState_t *blackboxLast = blackboxHistory[1];
689
690 blackboxWrite('P');
691
692 //No need to store iteration count since its delta is always 1
693
694 /*
695 * Since the difference between the difference between successive times will be nearly zero (due to consistent
696 * looptime spacing), use second-order differences.
697 */
698 blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
699
700 int32_t deltas[8];
701 int32_t setpointDeltas[4];
702
703 if (testBlackboxCondition(CONDITION(PID))) {
704 arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
705 blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
706
707 /*
708 * The PID I field changes very slowly, most of the time +-2, so use an encoding
709 * that can pack all three fields into one byte in that situation.
710 */
711 arraySubInt32(deltas, blackboxCurrent->axisPID_I, blackboxLast->axisPID_I, XYZ_AXIS_COUNT);
712 blackboxWriteTag2_3S32(deltas);
713
714 /*
715 * The PID D term is frequently set to zero for yaw, which makes the result from the calculation
716 * always zero. So don't bother recording D results when PID D terms are zero.
717 */
718 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
719 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) {
720 blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]);
721 }
722 }
723
724 arraySubInt32(deltas, blackboxCurrent->axisPID_F, blackboxLast->axisPID_F, XYZ_AXIS_COUNT);
725 blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
726 }
727
728 /*
729 * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
730 * can pack multiple values per byte:
731 */
732 for (int x = 0; x < 4; x++) {
733 deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x];
734 setpointDeltas[x] = blackboxCurrent->setpoint[x] - blackboxLast->setpoint[x];
735 }
736
737 if (testBlackboxCondition(CONDITION(RC_COMMANDS))) {
738 blackboxWriteTag8_4S16(deltas);
739 }
740 if (testBlackboxCondition(CONDITION(SETPOINT))) {
741 blackboxWriteTag8_4S16(setpointDeltas);
742 }
743
744 //Check for sensors that are updated periodically (so deltas are normally zero)
745 int optionalFieldCount = 0;
746
747 if (testBlackboxCondition(CONDITION(VBAT))) {
748 deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->vbatLatest - blackboxLast->vbatLatest;
749 }
750
751 if (testBlackboxCondition(CONDITION(AMPERAGE_ADC))) {
752 deltas[optionalFieldCount++] = blackboxCurrent->amperageLatest - blackboxLast->amperageLatest;
753 }
754
755 #ifdef USE_MAG
756 if (testBlackboxCondition(CONDITION(MAG))) {
757 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
758 deltas[optionalFieldCount++] = blackboxCurrent->magADC[x] - blackboxLast->magADC[x];
759 }
760 }
761 #endif
762
763 #ifdef USE_BARO
764 if (testBlackboxCondition(CONDITION(BARO))) {
765 deltas[optionalFieldCount++] = blackboxCurrent->baroAlt - blackboxLast->baroAlt;
766 }
767 #endif
768
769 #ifdef USE_RANGEFINDER
770 if (testBlackboxCondition(CONDITION(RANGEFINDER))) {
771 deltas[optionalFieldCount++] = blackboxCurrent->surfaceRaw - blackboxLast->surfaceRaw;
772 }
773 #endif
774
775 if (testBlackboxCondition(CONDITION(RSSI))) {
776 deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->rssi - blackboxLast->rssi;
777 }
778
779 blackboxWriteTag8_8SVB(deltas, optionalFieldCount);
780
781 //Since gyros, accs and motors are noisy, base their predictions on the average of the history:
782 if (testBlackboxCondition(CONDITION(GYRO))) {
783 blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT);
784 }
785 if (testBlackboxCondition(CONDITION(ACC))) {
786 blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
787 }
788 if (testBlackboxCondition(CONDITION(DEBUG_LOG))) {
789 blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), DEBUG16_VALUE_COUNT);
790 }
791
792 if (isFieldEnabled(FIELD_SELECT(MOTOR))) {
793 blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor), getMotorCount());
794
795 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) {
796 blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]);
797 }
798 }
799
800 //Rotate our history buffers
801 blackboxHistory[2] = blackboxHistory[1];
802 blackboxHistory[1] = blackboxHistory[0];
803 blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
804
805 blackboxLoggedAnyFrames = true;
806 }
807
808 /* Write the contents of the global "slowHistory" to the log as an "S" frame. Because this data is logged so
809 * infrequently, delta updates are not reasonable, so we log independent frames. */
810 static void writeSlowFrame(void)
811 {
812 int32_t values[3];
813
814 blackboxWrite('S');
815
816 blackboxWriteUnsignedVB(slowHistory.flightModeFlags);
817 blackboxWriteUnsignedVB(slowHistory.stateFlags);
818
819 /*
820 * Most of the time these three values will be able to pack into one byte for us:
821 */
822 values[0] = slowHistory.failsafePhase;
823 values[1] = slowHistory.rxSignalReceived ? 1 : 0;
824 values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0;
825 blackboxWriteTag2_3S32(values);
826
827 blackboxSlowFrameIterationTimer = 0;
828 }
829
830 /**
831 * Load rarely-changing values from the FC into the given structure
832 */
833 static void loadSlowState(blackboxSlowState_t *slow)
834 {
835 memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
836 slow->stateFlags = stateFlags;
837 slow->failsafePhase = failsafePhase();
838 slow->rxSignalReceived = rxIsReceivingSignal();
839 slow->rxFlightChannelsValid = rxAreFlightChannelsValid();
840 }
841
842 /**
843 * If the data in the slow frame has changed, log a slow frame.
844 *
845 * If allowPeriodicWrite is true, the frame is also logged if it has been more than blackboxSInterval logging iterations
846 * since the field was last logged.
847 */
848 STATIC_UNIT_TESTED bool writeSlowFrameIfNeeded(void)
849 {
850 // Write the slow frame peridocially so it can be recovered if we ever lose sync
851 bool shouldWrite = blackboxSlowFrameIterationTimer >= blackboxSInterval;
852
853 if (shouldWrite) {
854 loadSlowState(&slowHistory);
855 } else {
856 blackboxSlowState_t newSlowState;
857
858 loadSlowState(&newSlowState);
859
860 // Only write a slow frame if it was different from the previous state
861 if (memcmp(&newSlowState, &slowHistory, sizeof(slowHistory)) != 0) {
862 // Use the new state as our new history
863 memcpy(&slowHistory, &newSlowState, sizeof(slowHistory));
864 shouldWrite = true;
865 }
866 }
867
868 if (shouldWrite) {
869 writeSlowFrame();
870 }
871 return shouldWrite;
872 }
873
874 void blackboxValidateConfig(void)
875 {
876 // If we've chosen an unsupported device, change the device to serial
877 switch (blackboxConfig()->device) {
878 #ifdef USE_FLASHFS
879 case BLACKBOX_DEVICE_FLASH:
880 #endif
881 #ifdef USE_SDCARD
882 case BLACKBOX_DEVICE_SDCARD:
883 #endif
884 case BLACKBOX_DEVICE_SERIAL:
885 // Device supported, leave the setting alone
886 break;
887
888 default:
889 blackboxConfigMutable()->device = BLACKBOX_DEVICE_SERIAL;
890 }
891 }
892
893 static void blackboxResetIterationTimers(void)
894 {
895 blackboxIteration = 0;
896 blackboxLoopIndex = 0;
897 blackboxIFrameIndex = 0;
898 blackboxPFrameIndex = 0;
899 blackboxSlowFrameIterationTimer = 0;
900 }
901
902 /**
903 * Start Blackbox logging if it is not already running. Intended to be called upon arming.
904 */
905 static void blackboxStart(void)
906 {
907 blackboxValidateConfig();
908
909 if (!blackboxDeviceOpen()) {
910 blackboxSetState(BLACKBOX_STATE_DISABLED);
911 return;
912 }
913
914 memset(&gpsHistory, 0, sizeof(gpsHistory));
915
916 blackboxHistory[0] = &blackboxHistoryRing[0];
917 blackboxHistory[1] = &blackboxHistoryRing[1];
918 blackboxHistory[2] = &blackboxHistoryRing[2];
919
920 vbatReference = getBatteryVoltageLatest();
921
922 //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
923
924 /*
925 * We use conditional tests to decide whether or not certain fields should be logged. Since our headers
926 * must always agree with the logged data, the results of these tests must not change during logging. So
927 * cache those now.
928 */
929 blackboxBuildConditionCache();
930
931 blackboxModeActivationConditionPresent = isModeActivationConditionPresent(BOXBLACKBOX);
932
933 blackboxResetIterationTimers();
934
935 /*
936 * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
937 * it finally plays the beep for this arming event.
938 */
939 blackboxLastArmingBeep = getArmingBeepTimeMicros();
940 memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); // record startup status
941
942 blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
943 }
944
945 /**
946 * Begin Blackbox shutdown.
947 */
948 void blackboxFinish(void)
949 {
950 switch (blackboxState) {
951 case BLACKBOX_STATE_DISABLED:
952 case BLACKBOX_STATE_STOPPED:
953 case BLACKBOX_STATE_SHUTTING_DOWN:
954 // We're already stopped/shutting down
955 break;
956 case BLACKBOX_STATE_RUNNING:
957 case BLACKBOX_STATE_PAUSED:
958 blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END, NULL);
959 FALLTHROUGH;
960 default:
961 blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN);
962 }
963 }
964
965 /**
966 * Test Motors Blackbox Logging
967 */
968 static bool startedLoggingInTestMode = false;
969
970 static void startInTestMode(void)
971 {
972 if (!startedLoggingInTestMode) {
973 if (blackboxConfig()->device == BLACKBOX_DEVICE_SERIAL) {
974 serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
975 if (sharedBlackboxAndMspPort) {
976 return; // When in test mode, we cannot share the MSP and serial logger port!
977 }
978 }
979 blackboxStart();
980 startedLoggingInTestMode = true;
981 }
982 }
983
984 static void stopInTestMode(void)
985 {
986 if (startedLoggingInTestMode) {
987 blackboxFinish();
988 startedLoggingInTestMode = false;
989 }
990 }
991 /**
992 * We are going to monitor the MSP_SET_MOTOR target variables motor_disarmed[] for values other than minthrottle
993 * on reading a value (i.e. the user is testing the motors), then we enable test mode logging;
994 * we monitor when the values return to minthrottle and start a delay timer (5 seconds); if
995 * the test motors are left at minimum throttle for this delay timer, then we assume we are done testing and
996 * shutdown the logger.
997 *
998 * Of course, after the 5 seconds and shutdown of the logger, the system will be re-enabled to allow the
999 * test mode to trigger again; its just that the data will be in a second, third, fourth etc log file.
1000 */
1001 static bool inMotorTestMode(void)
1002 {
1003 static uint32_t resetTime = 0;
1004
1005 if (!ARMING_FLAG(ARMED) && areMotorsRunning()) {
1006 resetTime = millis() + 5000; // add 5 seconds
1007 return true;
1008 } else {
1009 // Monitor the duration at minimum
1010 return (millis() < resetTime);
1011 }
1012 return false;
1013 }
1014
1015 #ifdef USE_GPS
1016 static void writeGPSHomeFrame(void)
1017 {
1018 blackboxWrite('H');
1019
1020 blackboxWriteSignedVB(GPS_home[0]);
1021 blackboxWriteSignedVB(GPS_home[1]);
1022 //TODO it'd be great if we could grab the GPS current time and write that too
1023
1024 gpsHistory.GPS_home[0] = GPS_home[0];
1025 gpsHistory.GPS_home[1] = GPS_home[1];
1026 }
1027
1028 static void writeGPSFrame(timeUs_t currentTimeUs)
1029 {
1030 blackboxWrite('G');
1031
1032 /*
1033 * If we're logging every frame, then a GPS frame always appears just after a frame with the
1034 * currentTime timestamp in the log, so the reader can just use that timestamp for the GPS frame.
1035 *
1036 * If we're not logging every frame, we need to store the time of this GPS frame.
1037 */
1038 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) {
1039 // Predict the time of the last frame in the main log
1040 blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time);
1041 }
1042
1043 blackboxWriteUnsignedVB(gpsSol.numSat);
1044 blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[GPS_LATITUDE]);
1045 blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[GPS_LONGITUDE]);
1046 blackboxWriteUnsignedVB(gpsSol.llh.altCm / 10); // was originally designed to transport meters in int16, but +-3276.7m is a good compromise
1047 blackboxWriteUnsignedVB(gpsSol.groundSpeed);
1048 blackboxWriteUnsignedVB(gpsSol.groundCourse);
1049
1050 gpsHistory.GPS_numSat = gpsSol.numSat;
1051 gpsHistory.GPS_coord[GPS_LATITUDE] = gpsSol.llh.lat;
1052 gpsHistory.GPS_coord[GPS_LONGITUDE] = gpsSol.llh.lon;
1053 }
1054 #endif
1055
1056 /**
1057 * Fill the current state of the blackbox using values read from the flight controller
1058 */
1059 static void loadMainState(timeUs_t currentTimeUs)
1060 {
1061 #ifndef UNIT_TEST
1062 blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
1063
1064 blackboxCurrent->time = currentTimeUs;
1065
1066 for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
1067 blackboxCurrent->axisPID_P[i] = lrintf(pidData[i].P);
1068 blackboxCurrent->axisPID_I[i] = lrintf(pidData[i].I);
1069 blackboxCurrent->axisPID_D[i] = lrintf(pidData[i].D);
1070 blackboxCurrent->axisPID_F[i] = lrintf(pidData[i].F);
1071 blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i] * blackboxHighResolutionScale);
1072 #if defined(USE_ACC)
1073 blackboxCurrent->accADC[i] = lrintf(acc.accADC[i]);
1074 #endif
1075 #ifdef USE_MAG
1076 blackboxCurrent->magADC[i] = lrintf(mag.magADC[i]);
1077 #endif
1078 }
1079
1080 for (int i = 0; i < 4; i++) {
1081 blackboxCurrent->rcCommand[i] = lrintf(rcCommand[i] * blackboxHighResolutionScale);
1082 }
1083
1084 // log the currentPidSetpoint values applied to the PID controller
1085 for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
1086 blackboxCurrent->setpoint[i] = lrintf(pidGetPreviousSetpoint(i) * blackboxHighResolutionScale);
1087 }
1088 // log the final throttle value used in the mixer
1089 blackboxCurrent->setpoint[3] = lrintf(mixerGetThrottle() * 1000);
1090
1091 for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
1092 blackboxCurrent->debug[i] = debug[i];
1093 }
1094
1095 const int motorCount = getMotorCount();
1096 for (int i = 0; i < motorCount; i++) {
1097 blackboxCurrent->motor[i] = lrintf(motor[i]);
1098 }
1099
1100 blackboxCurrent->vbatLatest = getBatteryVoltageLatest();
1101 blackboxCurrent->amperageLatest = getAmperageLatest();
1102
1103 #ifdef USE_BARO
1104 blackboxCurrent->baroAlt = baro.altitude;
1105 #endif
1106
1107 #ifdef USE_RANGEFINDER
1108 // Store the raw sonar value without applying tilt correction
1109 blackboxCurrent->surfaceRaw = rangefinderGetLatestAltitude();
1110 #endif
1111
1112 blackboxCurrent->rssi = getRssi();
1113
1114 #ifdef USE_SERVOS
1115 //Tail servo for tricopters
1116 blackboxCurrent->servo[5] = servo[5];
1117 #endif
1118 #else
1119 UNUSED(currentTimeUs);
1120 #endif // UNIT_TEST
1121 }
1122
1123 /**
1124 * Transmit the header information for the given field definitions. Transmitted header lines look like:
1125 *
1126 * H Field I name:a,b,c
1127 * H Field I predictor:0,1,2
1128 *
1129 * For all header types, provide a "mainFrameChar" which is the name for the field and will be used to refer to it in the
1130 * header (e.g. P, I etc). For blackboxDeltaField_t fields, also provide deltaFrameChar, otherwise set this to zero.
1131 *
1132 * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
1133 * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
1134 *
1135 * Set xmitState.headerIndex to 0 and xmitState.u.fieldIndex to -1 before calling for the first time.
1136 *
1137 * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the
1138 * fieldDefinition and secondCondition arrays.
1139 *
1140 * Returns true if there is still header left to transmit (so call again to continue transmission).
1141 */
1142 static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const void *fieldDefinitions,
1143 const void *secondFieldDefinition, int fieldCount, const uint8_t *conditions, const uint8_t *secondCondition)
1144 {
1145 const blackboxFieldDefinition_t *def;
1146 unsigned int headerCount;
1147 static bool needComma = false;
1148 size_t definitionStride = (char*) secondFieldDefinition - (char*) fieldDefinitions;
1149 size_t conditionsStride = (char*) secondCondition - (char*) conditions;
1150
1151 if (deltaFrameChar) {
1152 headerCount = BLACKBOX_DELTA_FIELD_HEADER_COUNT;
1153 } else {
1154 headerCount = BLACKBOX_SIMPLE_FIELD_HEADER_COUNT;
1155 }
1156
1157 /*
1158 * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit
1159 * the whole header.
1160 */
1161
1162 // On our first call we need to print the name of the header and a colon
1163 if (xmitState.u.fieldIndex == -1) {
1164 if (xmitState.headerIndex >= headerCount) {
1165 return false; //Someone probably called us again after we had already completed transmission
1166 }
1167
1168 uint32_t charsToBeWritten = strlen("H Field x :") + strlen(blackboxFieldHeaderNames[xmitState.headerIndex]);
1169
1170 if (blackboxDeviceReserveBufferSpace(charsToBeWritten) != BLACKBOX_RESERVE_SUCCESS) {
1171 return true; // Try again later
1172 }
1173
1174 blackboxHeaderBudget -= blackboxPrintf("H Field %c %s:", xmitState.headerIndex >= BLACKBOX_SIMPLE_FIELD_HEADER_COUNT ? deltaFrameChar : mainFrameChar, blackboxFieldHeaderNames[xmitState.headerIndex]);
1175
1176 xmitState.u.fieldIndex++;
1177 needComma = false;
1178 }
1179
1180 // The longest we expect an integer to be as a string:
1181 const uint32_t LONGEST_INTEGER_STRLEN = 2;
1182
1183 for (; xmitState.u.fieldIndex < fieldCount; xmitState.u.fieldIndex++) {
1184 def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * xmitState.u.fieldIndex);
1185
1186 if (!conditions || testBlackboxCondition(conditions[conditionsStride * xmitState.u.fieldIndex])) {
1187 // First (over)estimate the length of the string we want to print
1188
1189 int32_t bytesToWrite = 1; // Leading comma
1190
1191 // The first header is a field name
1192 if (xmitState.headerIndex == 0) {
1193 bytesToWrite += strlen(def->name) + strlen("[]") + LONGEST_INTEGER_STRLEN;
1194 } else {
1195 //The other headers are integers
1196 bytesToWrite += LONGEST_INTEGER_STRLEN;
1197 }
1198
1199 // Now perform the write if the buffer is large enough
1200 if (blackboxDeviceReserveBufferSpace(bytesToWrite) != BLACKBOX_RESERVE_SUCCESS) {
1201 // Ran out of space!
1202 return true;
1203 }
1204
1205 blackboxHeaderBudget -= bytesToWrite;
1206
1207 if (needComma) {
1208 blackboxWrite(',');
1209 } else {
1210 needComma = true;
1211 }
1212
1213 // The first header is a field name
1214 if (xmitState.headerIndex == 0) {
1215 blackboxWriteString(def->name);
1216
1217 // Do we need to print an index in brackets after the name?
1218 if (def->fieldNameIndex != -1) {
1219 blackboxPrintf("[%d]", def->fieldNameIndex);
1220 }
1221 } else {
1222 //The other headers are integers
1223 blackboxPrintf("%d", def->arr[xmitState.headerIndex - 1]);
1224 }
1225 }
1226 }
1227
1228 // Did we complete this line?
1229 if (xmitState.u.fieldIndex == fieldCount && blackboxDeviceReserveBufferSpace(1) == BLACKBOX_RESERVE_SUCCESS) {
1230 blackboxHeaderBudget--;
1231 blackboxWrite('\n');
1232 xmitState.headerIndex++;
1233 xmitState.u.fieldIndex = -1;
1234 }
1235
1236 return xmitState.headerIndex < headerCount;
1237 }
1238
1239 // Buf must be at least FORMATTED_DATE_TIME_BUFSIZE
1240 STATIC_UNIT_TESTED char *blackboxGetStartDateTime(char *buf)
1241 {
1242 #ifdef USE_RTC_TIME
1243 dateTime_t dt;
1244 // rtcGetDateTime will fill dt with 0000-01-01T00:00:00
1245 // when time is not known.
1246 rtcGetDateTime(&dt);
1247 dateTimeFormatLocal(buf, &dt);
1248 #else
1249 buf = "0000-01-01T00:00:00.000";
1250 #endif
1251
1252 return buf;
1253 }
1254
1255 #ifndef BLACKBOX_PRINT_HEADER_LINE
1256 #define BLACKBOX_PRINT_HEADER_LINE(name, format, ...) case __COUNTER__: \
1257 blackboxPrintfHeaderLine(name, format, __VA_ARGS__); \
1258 break;
1259 #define BLACKBOX_PRINT_HEADER_LINE_CUSTOM(...) case __COUNTER__: \
1260 {__VA_ARGS__}; \
1261 break;
1262 #endif
1263
1264 /**
1265 * Transmit a portion of the system information headers. Call the first time with xmitState.headerIndex == 0. Returns
1266 * true iff transmission is complete, otherwise call again later to continue transmission.
1267 */
1268 static bool blackboxWriteSysinfo(void)
1269 {
1270 #ifndef UNIT_TEST
1271 const uint16_t motorOutputLowInt = lrintf(getMotorOutputLow());
1272 const uint16_t motorOutputHighInt = lrintf(getMotorOutputHigh());
1273
1274 // Make sure we have enough room in the buffer for our longest line (as of this writing, the "Firmware date" line)
1275 if (blackboxDeviceReserveBufferSpace(64) != BLACKBOX_RESERVE_SUCCESS) {
1276 return false;
1277 }
1278
1279 char buf[FORMATTED_DATE_TIME_BUFSIZE];
1280
1281 #ifdef USE_RC_SMOOTHING_FILTER
1282 rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData();
1283 #endif
1284
1285 const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile);
1286 switch (xmitState.headerIndex) {
1287 BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight");
1288 BLACKBOX_PRINT_HEADER_LINE("Firmware revision", "%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName);
1289 BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s", buildDate, buildTime);
1290 #ifdef USE_BOARD_INFO
1291 BLACKBOX_PRINT_HEADER_LINE("Board information", "%s %s", getManufacturerId(), getBoardName());
1292 #endif
1293 BLACKBOX_PRINT_HEADER_LINE("Log start datetime", "%s", blackboxGetStartDateTime(buf));
1294 BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", pilotConfig()->craftName);
1295 BLACKBOX_PRINT_HEADER_LINE("I interval", "%d", blackboxIInterval);
1296 BLACKBOX_PRINT_HEADER_LINE("P interval", "%d", blackboxPInterval);
1297 BLACKBOX_PRINT_HEADER_LINE("P ratio", "%d", (uint16_t)(blackboxIInterval / blackboxPInterval));
1298 BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle);
1299 BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
1300 BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x", castFloatBytesToInt(1.0f));
1301 BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorOutputLowInt, motorOutputHighInt);
1302 #if defined(USE_ACC)
1303 BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
1304 #endif
1305
1306 BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
1307 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
1308 blackboxPrintfHeaderLine("vbat_scale", "%u", voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale);
1309 } else {
1310 xmitState.headerIndex += 2; // Skip the next two vbat fields too
1311 }
1312 );
1313
1314 BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", batteryConfig()->vbatmincellvoltage,
1315 batteryConfig()->vbatwarningcellvoltage,
1316 batteryConfig()->vbatmaxcellvoltage);
1317 BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference);
1318
1319 BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
1320 if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
1321 blackboxPrintfHeaderLine("currentSensor", "%d,%d",currentSensorADCConfig()->offset, currentSensorADCConfig()->scale);
1322 }
1323 );
1324
1325 BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.sampleLooptime);
1326 BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", 1);
1327 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PID_PROCESS_DENOM, "%d", activePidLoopDenom);
1328 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_MID, "%d", currentControlRateProfile->thrMid8);
1329 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_EXPO, "%d", currentControlRateProfile->thrExpo8);
1330 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_MODE, "%d", currentPidProfile->tpa_mode);
1331 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE, "%d", currentPidProfile->tpa_rate);
1332 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT, "%d", currentPidProfile->tpa_breakpoint);
1333 BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d", currentControlRateProfile->rcRates[ROLL],
1334 currentControlRateProfile->rcRates[PITCH],
1335 currentControlRateProfile->rcRates[YAW]);
1336 BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d,%d,%d", currentControlRateProfile->rcExpo[ROLL],
1337 currentControlRateProfile->rcExpo[PITCH],
1338 currentControlRateProfile->rcExpo[YAW]);
1339 BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->rates[ROLL],
1340 currentControlRateProfile->rates[PITCH],
1341 currentControlRateProfile->rates[YAW]);
1342 BLACKBOX_PRINT_HEADER_LINE("rate_limits", "%d,%d,%d", currentControlRateProfile->rate_limit[ROLL],
1343 currentControlRateProfile->rate_limit[PITCH],
1344 currentControlRateProfile->rate_limit[YAW]);
1345 BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].P,
1346 currentPidProfile->pid[PID_ROLL].I,
1347 currentPidProfile->pid[PID_ROLL].D);
1348 BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d", currentPidProfile->pid[PID_PITCH].P,
1349 currentPidProfile->pid[PID_PITCH].I,
1350 currentPidProfile->pid[PID_PITCH].D);
1351 BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d", currentPidProfile->pid[PID_YAW].P,
1352 currentPidProfile->pid[PID_YAW].I,
1353 currentPidProfile->pid[PID_YAW].D);
1354 BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", currentPidProfile->pid[PID_LEVEL].P,
1355 currentPidProfile->pid[PID_LEVEL].I,
1356 currentPidProfile->pid[PID_LEVEL].D);
1357 BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", currentPidProfile->pid[PID_MAG].P);
1358 #ifdef USE_D_MIN
1359 BLACKBOX_PRINT_HEADER_LINE("d_min", "%d,%d,%d", currentPidProfile->d_min[ROLL],
1360 currentPidProfile->d_min[PITCH],
1361 currentPidProfile->d_min[YAW]);
1362 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_D_MAX_GAIN, "%d", currentPidProfile->d_min_gain);
1363 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_D_MAX_ADVANCE, "%d", currentPidProfile->d_min_advance);
1364 #endif
1365 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_LPF1_TYPE, "%d", currentPidProfile->dterm_lpf1_type);
1366 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_LPF1_STATIC_HZ, "%d", currentPidProfile->dterm_lpf1_static_hz);
1367 #ifdef USE_DYN_LPF
1368 BLACKBOX_PRINT_HEADER_LINE("dterm_lpf1_dyn_hz", "%d,%d", currentPidProfile->dterm_lpf1_dyn_min_hz,
1369 currentPidProfile->dterm_lpf1_dyn_max_hz);
1370 #endif
1371 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_LPF2_TYPE, "%d", currentPidProfile->dterm_lpf2_type);
1372 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_LPF2_STATIC_HZ, "%d", currentPidProfile->dterm_lpf2_static_hz);
1373 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_YAW_LOWPASS_HZ, "%d", currentPidProfile->yaw_lowpass_hz);
1374 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_NOTCH_HZ, "%d", currentPidProfile->dterm_notch_hz);
1375 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_NOTCH_CUTOFF, "%d", currentPidProfile->dterm_notch_cutoff);
1376 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_WINDUP, "%d", currentPidProfile->itermWindupPointPercent);
1377 #if defined(USE_ITERM_RELAX)
1378 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_RELAX, "%d", currentPidProfile->iterm_relax);
1379 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_RELAX_TYPE, "%d", currentPidProfile->iterm_relax_type);
1380 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_RELAX_CUTOFF, "%d", currentPidProfile->iterm_relax_cutoff);
1381 #endif
1382 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PID_AT_MIN_THROTTLE, "%d", currentPidProfile->pidAtMinThrottle);
1383
1384 // Betaflight PID controller parameters
1385 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_GAIN, "%d", currentPidProfile->anti_gravity_gain);
1386 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_CUTOFF_HZ, "%d", currentPidProfile->anti_gravity_cutoff_hz);
1387 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_P_GAIN, "%d", currentPidProfile->anti_gravity_p_gain);
1388
1389 #ifdef USE_ABSOLUTE_CONTROL
1390 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ABS_CONTROL_GAIN, "%d", currentPidProfile->abs_control_gain);
1391 #endif
1392 #ifdef USE_INTEGRATED_YAW_CONTROL
1393 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_USE_INTEGRATED_YAW, "%d", currentPidProfile->use_integrated_yaw);
1394 #endif
1395 BLACKBOX_PRINT_HEADER_LINE("ff_weight", "%d,%d,%d", currentPidProfile->pid[PID_ROLL].F,
1396 currentPidProfile->pid[PID_PITCH].F,
1397 currentPidProfile->pid[PID_YAW].F);
1398 #ifdef USE_FEEDFORWARD
1399 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_TRANSITION, "%d", currentPidProfile->feedforward_transition);
1400 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_AVERAGING, "%d", currentPidProfile->feedforward_averaging);
1401 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_SMOOTH_FACTOR, "%d", currentPidProfile->feedforward_smooth_factor);
1402 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_JITTER_FACTOR, "%d", currentPidProfile->feedforward_jitter_factor);
1403 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_BOOST, "%d", currentPidProfile->feedforward_boost);
1404 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT, "%d", currentPidProfile->feedforward_max_rate_limit);
1405 #endif
1406
1407 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT_YAW, "%d", currentPidProfile->yawRateAccelLimit);
1408 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT, "%d", currentPidProfile->rateAccelLimit);
1409 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PIDSUM_LIMIT, "%d", currentPidProfile->pidSumLimit);
1410 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PIDSUM_LIMIT_YAW, "%d", currentPidProfile->pidSumLimitYaw);
1411 // End of Betaflight controller parameters
1412
1413 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEADBAND, "%d", rcControlsConfig()->deadband);
1414 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_YAW_DEADBAND, "%d", rcControlsConfig()->yaw_deadband);
1415
1416 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_HARDWARE_LPF, "%d", gyroConfig()->gyro_hardware_lpf);
1417 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF1_TYPE, "%d", gyroConfig()->gyro_lpf1_type);
1418 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF1_STATIC_HZ, "%d", gyroConfig()->gyro_lpf1_static_hz);
1419 #ifdef USE_DYN_LPF
1420 BLACKBOX_PRINT_HEADER_LINE("gyro_lpf1_dyn_hz", "%d,%d", gyroConfig()->gyro_lpf1_dyn_min_hz,
1421 gyroConfig()->gyro_lpf1_dyn_max_hz);
1422 #endif
1423 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF2_TYPE, "%d", gyroConfig()->gyro_lpf2_type);
1424 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF2_STATIC_HZ, "%d", gyroConfig()->gyro_lpf2_static_hz);
1425 BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
1426 gyroConfig()->gyro_soft_notch_hz_2);
1427 BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
1428 gyroConfig()->gyro_soft_notch_cutoff_2);
1429 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_TO_USE, "%d", gyroConfig()->gyro_to_use);
1430 #ifdef USE_DYN_NOTCH_FILTER
1431 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_MAX_HZ, "%d", dynNotchConfig()->dyn_notch_max_hz);
1432 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_COUNT, "%d", dynNotchConfig()->dyn_notch_count);
1433 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_Q, "%d", dynNotchConfig()->dyn_notch_q);
1434 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_MIN_HZ, "%d", dynNotchConfig()->dyn_notch_min_hz);
1435 #endif
1436 #ifdef USE_DSHOT_TELEMETRY
1437 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DSHOT_BIDIR, "%d", motorConfig()->dev.useDshotTelemetry);
1438 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_POLES, "%d", motorConfig()->motorPoleCount);
1439 #endif
1440 #ifdef USE_RPM_FILTER
1441 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_HARMONICS, "%d", rpmFilterConfig()->rpm_filter_harmonics);
1442 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_Q, "%d", rpmFilterConfig()->rpm_filter_q);
1443 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_MIN_HZ, "%d", rpmFilterConfig()->rpm_filter_min_hz);
1444 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, "%d", rpmFilterConfig()->rpm_filter_fade_range_hz);
1445 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_LPF_HZ, "%d", rpmFilterConfig()->rpm_filter_lpf_hz);
1446 #endif
1447 #if defined(USE_ACC)
1448 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LPF_HZ, "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
1449 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_HARDWARE, "%d", accelerometerConfig()->acc_hardware);
1450 #endif
1451 #ifdef USE_BARO
1452 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d", barometerConfig()->baro_hardware);
1453 #endif
1454 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_SOURCE, "%d", positionConfig()->altitude_source);
1455 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_PREFER_BARO, "%d", positionConfig()->altitude_prefer_baro);
1456 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_LPF, "%d", positionConfig()->altitude_lpf);
1457 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf);
1458
1459 #ifdef USE_MAG
1460 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware);
1461 #endif
1462 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_CAL_ON_FIRST_ARM, "%d", armingConfig()->gyro_cal_on_first_arm);
1463 BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold);
1464 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SERIAL_RX_PROVIDER, "%d", rxConfig()->serialrx_provider);
1465 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_USE_UNSYNCED_PWM, "%d", motorConfig()->dev.useUnsyncedPwm);
1466 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_PROTOCOL, "%d", motorConfig()->dev.motorPwmProtocol);
1467 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_RATE, "%d", motorConfig()->dev.motorPwmRate);
1468 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DSHOT_IDLE_VALUE, "%d", motorConfig()->digitalIdleOffsetValue);
1469 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEBUG_MODE, "%d", debugMode);
1470 BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
1471
1472 #ifdef USE_RC_SMOOTHING_FILTER
1473 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING, "%d", rxConfig()->rc_smoothing_mode);
1474 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR, "%d", rxConfig()->rc_smoothing_auto_factor_rpy);
1475 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE, "%d", rxConfig()->rc_smoothing_auto_factor_throttle);
1476
1477 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_FEEDFORWARD_CUTOFF, "%d", rcSmoothingData->ffCutoffSetting);
1478 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF, "%d", rcSmoothingData->setpointCutoffSetting);
1479 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF, "%d", rcSmoothingData->throttleCutoffSetting);
1480 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_DEBUG_AXIS, "%d", rcSmoothingData->debugAxis);
1481 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS, "%d,%d,%d", rcSmoothingData->feedforwardCutoffFrequency,
1482 rcSmoothingData->setpointCutoffFrequency,
1483 rcSmoothingData->throttleCutoffFrequency);
1484 BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs);
1485 #endif // USE_RC_SMOOTHING_FILTER
1486 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RATES_TYPE, "%d", currentControlRateProfile->rates_type);
1487
1488 BLACKBOX_PRINT_HEADER_LINE("fields_disabled_mask", "%d", blackboxConfig()->fields_disabled_mask);
1489 BLACKBOX_PRINT_HEADER_LINE("blackbox_high_resolution", "%d", blackboxConfig()->high_resolution);
1490
1491 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
1492 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_VBAT_SAG_COMPENSATION, "%d", currentPidProfile->vbat_sag_compensation);
1493 #endif
1494
1495 #if defined(USE_DYN_IDLE)
1496 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_MIN_RPM, "%d", currentPidProfile->dyn_idle_min_rpm);
1497 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_P_GAIN, "%d", currentPidProfile->dyn_idle_p_gain);
1498 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_I_GAIN, "%d", currentPidProfile->dyn_idle_i_gain);
1499 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_D_GAIN, "%d", currentPidProfile->dyn_idle_d_gain);
1500 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_MAX_INCREASE, "%d", currentPidProfile->dyn_idle_max_increase);
1501 #endif
1502
1503 #ifdef USE_SIMPLIFIED_TUNING
1504 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PIDS_MODE, "%d", currentPidProfile->simplified_pids_mode);
1505 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_MASTER_MULTIPLIER, "%d", currentPidProfile->simplified_master_multiplier);
1506 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_I_GAIN, "%d", currentPidProfile->simplified_i_gain);
1507 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_D_GAIN, "%d", currentPidProfile->simplified_d_gain);
1508 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PI_GAIN, "%d", currentPidProfile->simplified_pi_gain);
1509 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_DMAX_GAIN, "%d", currentPidProfile->simplified_dmin_ratio);
1510 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_FEEDFORWARD_GAIN, "%d", currentPidProfile->simplified_feedforward_gain);
1511 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PITCH_D_GAIN, "%d", currentPidProfile->simplified_roll_pitch_ratio);
1512 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PITCH_PI_GAIN, "%d", currentPidProfile->simplified_pitch_pi_gain);
1513 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_DTERM_FILTER, "%d", currentPidProfile->simplified_dterm_filter);
1514 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_DTERM_FILTER_MULTIPLIER, "%d", currentPidProfile->simplified_dterm_filter_multiplier);
1515 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_GYRO_FILTER, "%d", gyroConfig()->simplified_gyro_filter);
1516 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_GYRO_FILTER_MULTIPLIER, "%d", gyroConfig()->simplified_gyro_filter_multiplier);
1517 #endif
1518
1519 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_OUTPUT_LIMIT, "%d", currentPidProfile->motor_output_limit);
1520 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_LIMIT_TYPE, "%d", currentControlRateProfile->throttle_limit_type);
1521 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_LIMIT_PERCENT, "%d", currentControlRateProfile->throttle_limit_percent);
1522 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_BOOST, "%d", currentPidProfile->throttle_boost);
1523 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_BOOST_CUTOFF, "%d", currentPidProfile->throttle_boost_cutoff)
1524
1525 #ifdef USE_GPS
1526 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_PROVIDER, "%d", gpsConfig()->provider)
1527 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_SET_HOME_POINT_ONCE, "%d", gpsConfig()->gps_set_home_point_once)
1528 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed)
1529
1530 #ifdef USE_GPS_RESCUE
1531 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minRescueDth)
1532 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode)
1533 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->rescueAltitudeBufferM)
1534 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ASCEND_RATE, "%d", gpsRescueConfig()->ascendRate)
1535
1536 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->initialAltitudeM)
1537 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_SPEED, "%d", gpsRescueConfig()->rescueGroundspeed)
1538 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle)
1539 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix)
1540 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d", gpsRescueConfig()->pitchCutoffHz)
1541
1542 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d", gpsRescueConfig()->descentDistanceM)
1543 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate)
1544 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_LANDING_ALT, "%d", gpsRescueConfig()->targetLandingAltitudeM)
1545 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold)
1546
1547 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, "%d", gpsRescueConfig()->throttleMin)
1548 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, "%d", gpsRescueConfig()->throttleMax)
1549 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, "%d", gpsRescueConfig()->throttleHover)
1550
1551 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks)
1552 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d", gpsRescueConfig()->minSats)
1553 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix)
1554
1555 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_P, "%d", gpsRescueConfig()->throttleP)
1556 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_I, "%d", gpsRescueConfig()->throttleI)
1557 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_D, "%d", gpsRescueConfig()->throttleD)
1558 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_P, "%d", gpsRescueConfig()->velP)
1559 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI)
1560 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD)
1561 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_YAW_P, "%d", gpsRescueConfig()->yawP)
1562
1563
1564 #ifdef USE_MAG
1565 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d", gpsRescueConfig()->useMag)
1566 #endif
1567 #endif
1568 #endif
1569
1570 default:
1571 return true;
1572 }
1573
1574 xmitState.headerIndex++;
1575 #endif // UNIT_TEST
1576 return false;
1577 }
1578
1579 /**
1580 * Write the given event to the log immediately
1581 */
1582 void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
1583 {
1584 // Only allow events to be logged after headers have been written
1585 if (!(blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_PAUSED)) {
1586 return;
1587 }
1588
1589 //Shared header for event frames
1590 blackboxWrite('E');
1591 blackboxWrite(event);
1592
1593 //Now serialize the data for this specific frame type
1594 switch (event) {
1595 case FLIGHT_LOG_EVENT_SYNC_BEEP:
1596 blackboxWriteUnsignedVB(data->syncBeep.time);
1597 break;
1598 case FLIGHT_LOG_EVENT_FLIGHTMODE: // New flightmode flags write
1599 blackboxWriteUnsignedVB(data->flightMode.flags);
1600 blackboxWriteUnsignedVB(data->flightMode.lastFlags);
1601 break;
1602 case FLIGHT_LOG_EVENT_DISARM:
1603 blackboxWriteUnsignedVB(data->disarm.reason);
1604 break;
1605 case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT:
1606 if (data->inflightAdjustment.floatFlag) {
1607 blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG);
1608 blackboxWriteFloat(data->inflightAdjustment.newFloatValue);
1609 } else {
1610 blackboxWrite(data->inflightAdjustment.adjustmentFunction);
1611 blackboxWriteSignedVB(data->inflightAdjustment.newValue);
1612 }
1613 break;
1614 case FLIGHT_LOG_EVENT_LOGGING_RESUME:
1615 blackboxWriteUnsignedVB(data->loggingResume.logIteration);
1616 blackboxWriteUnsignedVB(data->loggingResume.currentTime);
1617 break;
1618 case FLIGHT_LOG_EVENT_LOG_END:
1619 blackboxWriteString("End of log");
1620 blackboxWrite(0);
1621 break;
1622 default:
1623 break;
1624 }
1625 }
1626
1627 /* 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 */
1628 static void blackboxCheckAndLogArmingBeep(void)
1629 {
1630 // Use != so that we can still detect a change if the counter wraps
1631 if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) {
1632 blackboxLastArmingBeep = getArmingBeepTimeMicros();
1633 flightLogEvent_syncBeep_t eventData;
1634 eventData.time = blackboxLastArmingBeep;
1635 blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *)&eventData);
1636 }
1637 }
1638
1639 /* monitor the flight mode event status and trigger an event record if the state changes */
1640 static void blackboxCheckAndLogFlightMode(void)
1641 {
1642 // Use != so that we can still detect a change if the counter wraps
1643 if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
1644 flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
1645 eventData.lastFlags = blackboxLastFlightModeFlags;
1646 memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
1647 memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
1648 blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
1649 }
1650 }
1651
1652 STATIC_UNIT_TESTED bool blackboxShouldLogPFrame(void)
1653 {
1654 return blackboxPFrameIndex == 0 && blackboxPInterval != 0;
1655 }
1656
1657 STATIC_UNIT_TESTED bool blackboxShouldLogIFrame(void)
1658 {
1659 return blackboxLoopIndex == 0;
1660 }
1661
1662 /*
1663 * If the GPS home point has been updated, or every 128 I-frames (~10 seconds), write the
1664 * GPS home position.
1665 *
1666 * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
1667 * still be interpreted correctly.
1668 */
1669 #ifdef USE_GPS
1670 STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void)
1671 {
1672 if ((GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
1673 || (blackboxPFrameIndex == blackboxIInterval / 2 && blackboxIFrameIndex % 128 == 0)) && isFieldEnabled(FIELD_SELECT(GPS))) {
1674 return true;
1675 }
1676 return false;
1677 }
1678 #endif // GPS
1679
1680 // Called once every FC loop in order to keep track of how many FC loop iterations have passed
1681 STATIC_UNIT_TESTED void blackboxAdvanceIterationTimers(void)
1682 {
1683 ++blackboxSlowFrameIterationTimer;
1684 ++blackboxIteration;
1685
1686 if (++blackboxLoopIndex >= blackboxIInterval) {
1687 blackboxLoopIndex = 0;
1688 blackboxIFrameIndex++;
1689 blackboxPFrameIndex = 0;
1690 } else if (++blackboxPFrameIndex >= blackboxPInterval) {
1691 blackboxPFrameIndex = 0;
1692 }
1693 }
1694
1695 // Called once every FC loop in order to log the current state
1696 STATIC_UNIT_TESTED void blackboxLogIteration(timeUs_t currentTimeUs)
1697 {
1698 // Write a keyframe every blackboxIInterval frames so we can resynchronise upon missing frames
1699 if (blackboxShouldLogIFrame()) {
1700 /*
1701 * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
1702 * an additional item to write at the same time). Unless we're *only* logging "I" frames, then we have no choice.
1703 */
1704 if (blackboxIsOnlyLoggingIntraframes()) {
1705 writeSlowFrameIfNeeded();
1706 }
1707
1708 loadMainState(currentTimeUs);
1709 writeIntraframe();
1710 } else {
1711 blackboxCheckAndLogArmingBeep();
1712 blackboxCheckAndLogFlightMode(); // Check for FlightMode status change event
1713
1714 if (blackboxShouldLogPFrame()) {
1715 /*
1716 * We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
1717 * So only log slow frames during loop iterations where we log a main frame.
1718 */
1719 writeSlowFrameIfNeeded();
1720
1721 loadMainState(currentTimeUs);
1722 writeInterframe();
1723 }
1724 #ifdef USE_GPS
1725 if (featureIsEnabled(FEATURE_GPS) && isFieldEnabled(FIELD_SELECT(GPS))) {
1726 if (blackboxShouldLogGpsHomeFrame()) {
1727 writeGPSHomeFrame();
1728 writeGPSFrame(currentTimeUs);
1729 } else if (gpsSol.numSat != gpsHistory.GPS_numSat
1730 || gpsSol.llh.lat != gpsHistory.GPS_coord[GPS_LATITUDE]
1731 || gpsSol.llh.lon != gpsHistory.GPS_coord[GPS_LONGITUDE]) {
1732 //We could check for velocity changes as well but I doubt it changes independent of position
1733 writeGPSFrame(currentTimeUs);
1734 }
1735 }
1736 #endif
1737 }
1738
1739 //Flush every iteration so that our runtime variance is minimized
1740 blackboxDeviceFlush();
1741 }
1742
1743 /**
1744 * Call each flight loop iteration to perform blackbox logging.
1745 */
1746 void blackboxUpdate(timeUs_t currentTimeUs)
1747 {
1748 static BlackboxState cacheFlushNextState;
1749
1750 switch (blackboxState) {
1751 case BLACKBOX_STATE_STOPPED:
1752 if (ARMING_FLAG(ARMED)) {
1753 blackboxOpen();
1754 blackboxStart();
1755 }
1756 #ifdef USE_FLASHFS
1757 if (IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) {
1758 blackboxSetState(BLACKBOX_STATE_START_ERASE);
1759 }
1760 #endif
1761 break;
1762 case BLACKBOX_STATE_PREPARE_LOG_FILE:
1763 if (blackboxDeviceBeginLog()) {
1764 blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
1765 }
1766 break;
1767 case BLACKBOX_STATE_SEND_HEADER:
1768 blackboxReplenishHeaderBudget();
1769 //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
1770
1771 /*
1772 * Once the UART has had time to init, transmit the header in chunks so we don't overflow its transmit
1773 * buffer, overflow the OpenLog's buffer, or keep the main loop busy for too long.
1774 */
1775 if (millis() > xmitState.u.startTime + 100) {
1776 if (blackboxDeviceReserveBufferSpace(BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION) == BLACKBOX_RESERVE_SUCCESS) {
1777 for (int i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
1778 blackboxWrite(blackboxHeader[xmitState.headerIndex]);
1779 blackboxHeaderBudget--;
1780 }
1781 if (blackboxHeader[xmitState.headerIndex] == '\0') {
1782 blackboxSetState(BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER);
1783 }
1784 }
1785 }
1786 break;
1787 case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
1788 blackboxReplenishHeaderBudget();
1789 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1790 if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAYLEN(blackboxMainFields),
1791 &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
1792 #ifdef USE_GPS
1793 if (featureIsEnabled(FEATURE_GPS) && isFieldEnabled(FIELD_SELECT(GPS))) {
1794 blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADER);
1795 } else
1796 #endif
1797 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
1798 }
1799 break;
1800 #ifdef USE_GPS
1801 case BLACKBOX_STATE_SEND_GPS_H_HEADER:
1802 blackboxReplenishHeaderBudget();
1803 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1804 if (!sendFieldDefinition('H', 0, blackboxGpsHFields, blackboxGpsHFields + 1, ARRAYLEN(blackboxGpsHFields),
1805 NULL, NULL) && isFieldEnabled(FIELD_SELECT(GPS))) {
1806 blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADER);
1807 }
1808 break;
1809 case BLACKBOX_STATE_SEND_GPS_G_HEADER:
1810 blackboxReplenishHeaderBudget();
1811 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1812 if (!sendFieldDefinition('G', 0, blackboxGpsGFields, blackboxGpsGFields + 1, ARRAYLEN(blackboxGpsGFields),
1813 &blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition) && isFieldEnabled(FIELD_SELECT(GPS))) {
1814 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
1815 }
1816 break;
1817 #endif
1818 case BLACKBOX_STATE_SEND_SLOW_HEADER:
1819 blackboxReplenishHeaderBudget();
1820 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1821 if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAYLEN(blackboxSlowFields),
1822 NULL, NULL)) {
1823 cacheFlushNextState = BLACKBOX_STATE_SEND_SYSINFO;
1824 blackboxSetState(BLACKBOX_STATE_CACHE_FLUSH);
1825 }
1826 break;
1827 case BLACKBOX_STATE_SEND_SYSINFO:
1828 blackboxReplenishHeaderBudget();
1829 //On entry of this state, xmitState.headerIndex is 0
1830
1831 //Keep writing chunks of the system info headers until it returns true to signal completion
1832 if (blackboxWriteSysinfo()) {
1833 /*
1834 * Wait for header buffers to drain completely before data logging begins to ensure reliable header delivery
1835 * (overflowing circular buffers causes all data to be discarded, so the first few logged iterations
1836 * could wipe out the end of the header if we weren't careful)
1837 */
1838 cacheFlushNextState = BLACKBOX_STATE_RUNNING;
1839 blackboxSetState(BLACKBOX_STATE_CACHE_FLUSH);
1840 }
1841 break;
1842 case BLACKBOX_STATE_CACHE_FLUSH:
1843 // Flush the cache and wait until all possible entries have been written to the media
1844 if (blackboxDeviceFlushForceComplete()) {
1845 blackboxSetState(cacheFlushNextState);
1846 }
1847 break;
1848 case BLACKBOX_STATE_PAUSED:
1849 // Only allow resume to occur during an I-frame iteration, so that we have an "I" base to work from
1850 if (IS_RC_MODE_ACTIVE(BOXBLACKBOX) && blackboxShouldLogIFrame()) {
1851 // Write a log entry so the decoder is aware that our large time/iteration skip is intended
1852 flightLogEvent_loggingResume_t resume;
1853
1854 resume.logIteration = blackboxIteration;
1855 resume.currentTime = currentTimeUs;
1856
1857 blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
1858 blackboxSetState(BLACKBOX_STATE_RUNNING);
1859
1860 blackboxLogIteration(currentTimeUs);
1861 }
1862 // Keep the logging timers ticking so our log iteration continues to advance
1863 blackboxAdvanceIterationTimers();
1864 break;
1865 case BLACKBOX_STATE_RUNNING:
1866 // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
1867 // Prevent the Pausing of the log on the mode switch if in Motor Test Mode
1868 if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX) && !startedLoggingInTestMode) {
1869 blackboxSetState(BLACKBOX_STATE_PAUSED);
1870 } else {
1871 blackboxLogIteration(currentTimeUs);
1872 }
1873 blackboxAdvanceIterationTimers();
1874 break;
1875 case BLACKBOX_STATE_SHUTTING_DOWN:
1876 //On entry of this state, startTime is set
1877 /*
1878 * Wait for the log we've transmitted to make its way to the logger before we release the serial port,
1879 * since releasing the port clears the Tx buffer.
1880 *
1881 * Don't wait longer than it could possibly take if something funky happens.
1882 */
1883 if (blackboxDeviceEndLog(blackboxLoggedAnyFrames) && (millis() > xmitState.u.startTime + BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS || blackboxDeviceFlushForce())) {
1884 blackboxDeviceClose();
1885 blackboxSetState(BLACKBOX_STATE_STOPPED);
1886 }
1887 break;
1888 #ifdef USE_FLASHFS
1889 case BLACKBOX_STATE_START_ERASE:
1890 blackboxEraseAll();
1891 blackboxSetState(BLACKBOX_STATE_ERASING);
1892 beeper(BEEPER_BLACKBOX_ERASE);
1893 break;
1894 case BLACKBOX_STATE_ERASING:
1895 if (isBlackboxErased()) {
1896 //Done erasing
1897 blackboxSetState(BLACKBOX_STATE_ERASED);
1898 beeper(BEEPER_BLACKBOX_ERASE);
1899 }
1900 break;
1901 case BLACKBOX_STATE_ERASED:
1902 if (!IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) {
1903 blackboxSetState(BLACKBOX_STATE_STOPPED);
1904 }
1905 break;
1906 #endif
1907 default:
1908 break;
1909 }
1910
1911 // Did we run out of room on the device? Stop!
1912 if (isBlackboxDeviceFull()) {
1913 #ifdef USE_FLASHFS
1914 if (blackboxState != BLACKBOX_STATE_ERASING
1915 && blackboxState != BLACKBOX_STATE_START_ERASE
1916 && blackboxState != BLACKBOX_STATE_ERASED)
1917 #endif
1918 {
1919 blackboxSetState(BLACKBOX_STATE_STOPPED);
1920 // ensure we reset the test mode flag if we stop due to full memory card
1921 if (startedLoggingInTestMode) {
1922 startedLoggingInTestMode = false;
1923 }
1924 }
1925 } else { // Only log in test mode if there is room!
1926 switch (blackboxConfig()->mode) {
1927 case BLACKBOX_MODE_MOTOR_TEST:
1928 // Handle Motor Test Mode
1929 if (inMotorTestMode()) {
1930 if (blackboxState==BLACKBOX_STATE_STOPPED) {
1931 startInTestMode();
1932 }
1933 } else {
1934 if (blackboxState!=BLACKBOX_STATE_STOPPED) {
1935 stopInTestMode();
1936 }
1937 }
1938
1939 break;
1940 case BLACKBOX_MODE_ALWAYS_ON:
1941 if (blackboxState==BLACKBOX_STATE_STOPPED) {
1942 startInTestMode();
1943 }
1944
1945 break;
1946 case BLACKBOX_MODE_NORMAL:
1947 default:
1948
1949 break;
1950 }
1951 }
1952 }
1953
1954 int blackboxCalculatePDenom(int rateNum, int rateDenom)
1955 {
1956 return blackboxIInterval * rateNum / rateDenom;
1957 }
1958
1959 uint8_t blackboxGetRateDenom(void)
1960 {
1961 return blackboxPInterval;
1962
1963 }
1964
1965 uint16_t blackboxGetPRatio(void)
1966 {
1967 return blackboxIInterval / blackboxPInterval;
1968 }
1969
1970 uint8_t blackboxCalculateSampleRate(uint16_t pRatio)
1971 {
1972 return LOG2(32000 / (targetPidLooptime * pRatio));
1973 }
1974
1975 /**
1976 * Call during system startup to initialize the blackbox.
1977 */
1978 void blackboxInit(void)
1979 {
1980 blackboxResetIterationTimers();
1981
1982 // an I-frame is written every 32ms
1983 // blackboxUpdate() is run in synchronisation with the PID loop
1984 // targetPidLooptime is 1000 for 1kHz loop, 500 for 2kHz loop etc, targetPidLooptime is rounded for short looptimes
1985 blackboxIInterval = (uint16_t)(32 * 1000 / targetPidLooptime);
1986
1987 blackboxPInterval = 1 << blackboxConfig()->sample_rate;
1988 if (blackboxPInterval > blackboxIInterval) {
1989 blackboxPInterval = 0; // log only I frames if logging frequency is too low
1990 }
1991
1992 if (blackboxConfig()->device) {
1993 blackboxSetState(BLACKBOX_STATE_STOPPED);
1994 } else {
1995 blackboxSetState(BLACKBOX_STATE_DISABLED);
1996 }
1997 blackboxSInterval = blackboxIInterval * 256; // S-frame is written every 256*32 = 8192ms, approx every 8 seconds
1998
1999 blackboxHighResolutionScale = blackboxConfig()->high_resolution ? 10.0f : 1.0f;
2000 }
2001 #endif