?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Refactor Feedforward Angle and RC Smoothing - mashup of 12578 and 12594 (#12605)
[betaflight.git] / src / main / blackbox / blackbox.c
blobce4aba886d6c449ccdb75a3b8f0f79bc53879000
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(DEFAULT_BLACKBOX_DEVICE)
87 #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_NONE
88 #endif
89
90 PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 3);
91
92 PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
93 .fields_disabled_mask = 0, // default log all fields
94 .sample_rate = BLACKBOX_RATE_QUARTER,
95 .device = DEFAULT_BLACKBOX_DEVICE,
96 .mode = BLACKBOX_MODE_NORMAL,
97 .high_resolution = false
98 );
99
100 STATIC_ASSERT((sizeof(blackboxConfig()->fields_disabled_mask) * 8) >= FLIGHT_LOG_FIELD_SELECT_COUNT, too_many_flight_log_fields_selections);
101
102 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
103
104 // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
105
106 #define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
107 #define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)
108 #define CONDITION(x) CONCAT(FLIGHT_LOG_FIELD_CONDITION_, x)
109 #define FIELD_SELECT(x) CONCAT(FLIGHT_LOG_FIELD_SELECT_, x)
110 #define UNSIGNED FLIGHT_LOG_FIELD_UNSIGNED
111 #define SIGNED FLIGHT_LOG_FIELD_SIGNED
112
113 static const char blackboxHeader[] =
114 "H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
115 "H Data version:2\n";
116
117 static const char* const blackboxFieldHeaderNames[] = {
118 "name",
119 "signed",
120 "predictor",
121 "encoding",
122 "predictor",
123 "encoding"
124 };
125
126 /* All field definition structs should look like this (but with longer arrs): */
127 typedef struct blackboxFieldDefinition_s {
128 const char *name;
129 // If the field name has a number to be included in square brackets [1] afterwards, set it here, or -1 for no brackets:
130 int8_t fieldNameIndex;
131
132 // Each member of this array will be the value to print for this field for the given header index
133 uint8_t arr[1];
134 } blackboxFieldDefinition_t;
135
136 #define BLACKBOX_DELTA_FIELD_HEADER_COUNT ARRAYLEN(blackboxFieldHeaderNames)
137 #define BLACKBOX_SIMPLE_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
138 #define BLACKBOX_CONDITIONAL_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
139
140 typedef struct blackboxSimpleFieldDefinition_s {
141 const char *name;
142 int8_t fieldNameIndex;
143
144 uint8_t isSigned;
145 uint8_t predict;
146 uint8_t encode;
147 } blackboxSimpleFieldDefinition_t;
148
149 typedef struct blackboxConditionalFieldDefinition_s {
150 const char *name;
151 int8_t fieldNameIndex;
152
153 uint8_t isSigned;
154 uint8_t predict;
155 uint8_t encode;
156 uint8_t condition; // Decide whether this field should appear in the log
157 } blackboxConditionalFieldDefinition_t;
158
159 typedef struct blackboxDeltaFieldDefinition_s {
160 const char *name;
161 int8_t fieldNameIndex;
162
163 uint8_t isSigned;
164 uint8_t Ipredict;
165 uint8_t Iencode;
166 uint8_t Ppredict;
167 uint8_t Pencode;
168 uint8_t condition; // Decide whether this field should appear in the log
169 } blackboxDeltaFieldDefinition_t;
170
171 /**
172 * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
173 * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
174 * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
175 * the encoding we've promised here).
176 */
177 static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
178 /* loopIteration doesn't appear in P frames since it always increments */
179 {"loopIteration",-1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC), .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)},
180 /* Time advances pretty steadily so the P-frame prediction is a straight line */
181 {"time", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(STRAIGHT_LINE), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
182 {"axisP", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
183 {"axisP", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
184 {"axisP", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
185 /* I terms get special packed encoding in P frames: */
186 {"axisI", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(PID)},
187 {"axisI", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(PID)},
188 {"axisI", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(PID)},
189 {"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
190 {"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
191 {"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
192 {"axisF", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
193 {"axisF", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
194 {"axisF", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
195 /* rcCommands are encoded together as a group in P-frames: */
196 {"rcCommand", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
197 {"rcCommand", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
198 {"rcCommand", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
199 {"rcCommand", 3, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
200
201 // setpoint - define 4 fields like rcCommand to use the same encoding. setpoint[4] contains the mixer throttle
202 {"setpoint", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
203 {"setpoint", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
204 {"setpoint", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
205 {"setpoint", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
206
207 {"vbatLatest", -1, UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(VBAT)},
208 {"amperageLatest",-1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(AMPERAGE_ADC)},
209
210 #ifdef USE_MAG
211 {"magADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)},
212 {"magADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)},
213 {"magADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)},
214 #endif
215 #ifdef USE_BARO
216 {"baroAlt", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(BARO)},
217 #endif
218 #ifdef USE_RANGEFINDER
219 {"surfaceRaw", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(RANGEFINDER)},
220 #endif
221 {"rssi", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), CONDITION(RSSI)},
222
223 /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
224 {"gyroADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(GYRO)},
225 {"gyroADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(GYRO)},
226 {"gyroADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(GYRO)},
227 {"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
228 {"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
229 {"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
230 {"debug", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
231 {"debug", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
232 {"debug", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
233 {"debug", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
234 {"debug", 4, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
235 {"debug", 5, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
236 {"debug", 6, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
237 {"debug", 7, 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 NONE
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_NONE;
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 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FEEDFORWARD, "%d", currentPidProfile->pid[PID_LEVEL].F);
1406 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FF_SMOOTHING_MS, "%d", currentPidProfile->angle_feedforward_smoothing_ms);
1407 #endif
1408 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_LIMIT, "%d", currentPidProfile->angle_limit);
1409 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_EARTH_REF, "%d", currentPidProfile->angle_earth_ref);
1410 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HORIZON_LIMIT_DEGREES, "%d", currentPidProfile->horizon_limit_degrees);
1411 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HORIZON_DELAY_MS, "%d", currentPidProfile->horizon_delay_ms);
1412
1413 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT_YAW, "%d", currentPidProfile->yawRateAccelLimit);
1414 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT, "%d", currentPidProfile->rateAccelLimit);
1415 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PIDSUM_LIMIT, "%d", currentPidProfile->pidSumLimit);
1416 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PIDSUM_LIMIT_YAW, "%d", currentPidProfile->pidSumLimitYaw);
1417 // End of Betaflight controller parameters
1418
1419 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEADBAND, "%d", rcControlsConfig()->deadband);
1420 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_YAW_DEADBAND, "%d", rcControlsConfig()->yaw_deadband);
1421
1422 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_HARDWARE_LPF, "%d", gyroConfig()->gyro_hardware_lpf);
1423 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF1_TYPE, "%d", gyroConfig()->gyro_lpf1_type);
1424 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF1_STATIC_HZ, "%d", gyroConfig()->gyro_lpf1_static_hz);
1425 #ifdef USE_DYN_LPF
1426 BLACKBOX_PRINT_HEADER_LINE("gyro_lpf1_dyn_hz", "%d,%d", gyroConfig()->gyro_lpf1_dyn_min_hz,
1427 gyroConfig()->gyro_lpf1_dyn_max_hz);
1428 #endif
1429 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF2_TYPE, "%d", gyroConfig()->gyro_lpf2_type);
1430 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF2_STATIC_HZ, "%d", gyroConfig()->gyro_lpf2_static_hz);
1431 BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
1432 gyroConfig()->gyro_soft_notch_hz_2);
1433 BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
1434 gyroConfig()->gyro_soft_notch_cutoff_2);
1435 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_TO_USE, "%d", gyroConfig()->gyro_to_use);
1436 #ifdef USE_DYN_NOTCH_FILTER
1437 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_MAX_HZ, "%d", dynNotchConfig()->dyn_notch_max_hz);
1438 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_COUNT, "%d", dynNotchConfig()->dyn_notch_count);
1439 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_Q, "%d", dynNotchConfig()->dyn_notch_q);
1440 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_MIN_HZ, "%d", dynNotchConfig()->dyn_notch_min_hz);
1441 #endif
1442 #ifdef USE_DSHOT_TELEMETRY
1443 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DSHOT_BIDIR, "%d", motorConfig()->dev.useDshotTelemetry);
1444 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_POLES, "%d", motorConfig()->motorPoleCount);
1445 #endif
1446 #ifdef USE_RPM_FILTER
1447 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_HARMONICS, "%d", rpmFilterConfig()->rpm_filter_harmonics);
1448 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_Q, "%d", rpmFilterConfig()->rpm_filter_q);
1449 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_MIN_HZ, "%d", rpmFilterConfig()->rpm_filter_min_hz);
1450 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, "%d", rpmFilterConfig()->rpm_filter_fade_range_hz);
1451 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_LPF_HZ, "%d", rpmFilterConfig()->rpm_filter_lpf_hz);
1452 #endif
1453 #if defined(USE_ACC)
1454 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LPF_HZ, "%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
1455 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_HARDWARE, "%d", accelerometerConfig()->acc_hardware);
1456 #endif
1457 #ifdef USE_BARO
1458 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d", barometerConfig()->baro_hardware);
1459 #endif
1460 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_SOURCE, "%d", positionConfig()->altitude_source);
1461 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_PREFER_BARO, "%d", positionConfig()->altitude_prefer_baro);
1462 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_LPF, "%d", positionConfig()->altitude_lpf);
1463 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf);
1464
1465 #ifdef USE_MAG
1466 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware);
1467 #endif
1468 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_CAL_ON_FIRST_ARM, "%d", armingConfig()->gyro_cal_on_first_arm);
1469 BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold);
1470 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SERIAL_RX_PROVIDER, "%d", rxConfig()->serialrx_provider);
1471 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_USE_UNSYNCED_PWM, "%d", motorConfig()->dev.useUnsyncedPwm);
1472 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_PROTOCOL, "%d", motorConfig()->dev.motorPwmProtocol);
1473 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_RATE, "%d", motorConfig()->dev.motorPwmRate);
1474 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DSHOT_IDLE_VALUE, "%d", motorConfig()->digitalIdleOffsetValue);
1475 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEBUG_MODE, "%d", debugMode);
1476 BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
1477
1478 #ifdef USE_RC_SMOOTHING_FILTER
1479 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING, "%d", rxConfig()->rc_smoothing_mode);
1480 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR, "%d", rxConfig()->rc_smoothing_auto_factor_rpy);
1481 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE, "%d", rxConfig()->rc_smoothing_auto_factor_throttle);
1482
1483 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_FEEDFORWARD_CUTOFF, "%d", rcSmoothingData->feedforwardCutoffSetting);
1484 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF, "%d", rcSmoothingData->setpointCutoffSetting);
1485 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF, "%d", rcSmoothingData->throttleCutoffSetting);
1486
1487 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_DEBUG_AXIS, "%d", rcSmoothingData->debugAxis);
1488 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS, "%d,%d,%d", rcSmoothingData->feedforwardCutoffFrequency,
1489 rcSmoothingData->setpointCutoffFrequency,
1490 rcSmoothingData->throttleCutoffFrequency);
1491 BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_smoothed", "%d", lrintf(rcSmoothingData->smoothedRxRateHz));
1492 #endif // USE_RC_SMOOTHING_FILTER
1493 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RATES_TYPE, "%d", currentControlRateProfile->rates_type);
1494
1495 BLACKBOX_PRINT_HEADER_LINE("fields_disabled_mask", "%d", blackboxConfig()->fields_disabled_mask);
1496 BLACKBOX_PRINT_HEADER_LINE("blackbox_high_resolution", "%d", blackboxConfig()->high_resolution);
1497
1498 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
1499 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_VBAT_SAG_COMPENSATION, "%d", currentPidProfile->vbat_sag_compensation);
1500 #endif
1501
1502 #if defined(USE_DYN_IDLE)
1503 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_MIN_RPM, "%d", currentPidProfile->dyn_idle_min_rpm);
1504 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_P_GAIN, "%d", currentPidProfile->dyn_idle_p_gain);
1505 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_I_GAIN, "%d", currentPidProfile->dyn_idle_i_gain);
1506 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_D_GAIN, "%d", currentPidProfile->dyn_idle_d_gain);
1507 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_MAX_INCREASE, "%d", currentPidProfile->dyn_idle_max_increase);
1508 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_START_INCREASE, "%d", currentPidProfile->dyn_idle_start_increase);
1509 #endif
1510
1511 #ifdef USE_SIMPLIFIED_TUNING
1512 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PIDS_MODE, "%d", currentPidProfile->simplified_pids_mode);
1513 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_MASTER_MULTIPLIER, "%d", currentPidProfile->simplified_master_multiplier);
1514 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_I_GAIN, "%d", currentPidProfile->simplified_i_gain);
1515 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_D_GAIN, "%d", currentPidProfile->simplified_d_gain);
1516 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PI_GAIN, "%d", currentPidProfile->simplified_pi_gain);
1517 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_DMAX_GAIN, "%d", currentPidProfile->simplified_dmin_ratio);
1518 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_FEEDFORWARD_GAIN, "%d", currentPidProfile->simplified_feedforward_gain);
1519 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PITCH_D_GAIN, "%d", currentPidProfile->simplified_roll_pitch_ratio);
1520 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PITCH_PI_GAIN, "%d", currentPidProfile->simplified_pitch_pi_gain);
1521 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_DTERM_FILTER, "%d", currentPidProfile->simplified_dterm_filter);
1522 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_DTERM_FILTER_MULTIPLIER, "%d", currentPidProfile->simplified_dterm_filter_multiplier);
1523 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_GYRO_FILTER, "%d", gyroConfig()->simplified_gyro_filter);
1524 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_GYRO_FILTER_MULTIPLIER, "%d", gyroConfig()->simplified_gyro_filter_multiplier);
1525 #endif
1526
1527 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_OUTPUT_LIMIT, "%d", currentPidProfile->motor_output_limit);
1528 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_LIMIT_TYPE, "%d", currentControlRateProfile->throttle_limit_type);
1529 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_LIMIT_PERCENT, "%d", currentControlRateProfile->throttle_limit_percent);
1530 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_BOOST, "%d", currentPidProfile->throttle_boost);
1531 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_BOOST_CUTOFF, "%d", currentPidProfile->throttle_boost_cutoff);
1532 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THRUST_LINEARIZATION, "%d", currentPidProfile->thrustLinearization);
1533
1534 #ifdef USE_GPS
1535 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_PROVIDER, "%d", gpsConfig()->provider)
1536 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_SET_HOME_POINT_ONCE, "%d", gpsConfig()->gps_set_home_point_once)
1537 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed)
1538
1539 #ifdef USE_GPS_RESCUE
1540 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minRescueDth)
1541 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode)
1542 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->rescueAltitudeBufferM)
1543 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ASCEND_RATE, "%d", gpsRescueConfig()->ascendRate)
1544
1545 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->initialAltitudeM)
1546 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_SPEED, "%d", gpsRescueConfig()->rescueGroundspeed)
1547 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle)
1548 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix)
1549 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d", gpsRescueConfig()->pitchCutoffHz)
1550
1551 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d", gpsRescueConfig()->descentDistanceM)
1552 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate)
1553 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_LANDING_ALT, "%d", gpsRescueConfig()->targetLandingAltitudeM)
1554 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold)
1555
1556 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, "%d", gpsRescueConfig()->throttleMin)
1557 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, "%d", gpsRescueConfig()->throttleMax)
1558 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_HOVER, "%d", gpsRescueConfig()->throttleHover)
1559
1560 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d", gpsRescueConfig()->sanityChecks)
1561 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d", gpsRescueConfig()->minSats)
1562 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix)
1563
1564 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_P, "%d", gpsRescueConfig()->throttleP)
1565 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_I, "%d", gpsRescueConfig()->throttleI)
1566 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_D, "%d", gpsRescueConfig()->throttleD)
1567 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_P, "%d", gpsRescueConfig()->velP)
1568 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d", gpsRescueConfig()->velI)
1569 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d", gpsRescueConfig()->velD)
1570 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_YAW_P, "%d", gpsRescueConfig()->yawP)
1571
1572
1573 #ifdef USE_MAG
1574 BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d", gpsRescueConfig()->useMag)
1575 #endif
1576 #endif
1577 #endif
1578
1579 default:
1580 return true;
1581 }
1582
1583 xmitState.headerIndex++;
1584 #endif // UNIT_TEST
1585 return false;
1586 }
1587
1588 /**
1589 * Write the given event to the log immediately
1590 */
1591 void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
1592 {
1593 // Only allow events to be logged after headers have been written
1594 if (!(blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_PAUSED)) {
1595 return;
1596 }
1597
1598 //Shared header for event frames
1599 blackboxWrite('E');
1600 blackboxWrite(event);
1601
1602 //Now serialize the data for this specific frame type
1603 switch (event) {
1604 case FLIGHT_LOG_EVENT_SYNC_BEEP:
1605 blackboxWriteUnsignedVB(data->syncBeep.time);
1606 break;
1607 case FLIGHT_LOG_EVENT_FLIGHTMODE: // New flightmode flags write
1608 blackboxWriteUnsignedVB(data->flightMode.flags);
1609 blackboxWriteUnsignedVB(data->flightMode.lastFlags);
1610 break;
1611 case FLIGHT_LOG_EVENT_DISARM:
1612 blackboxWriteUnsignedVB(data->disarm.reason);
1613 break;
1614 case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT:
1615 if (data->inflightAdjustment.floatFlag) {
1616 blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG);
1617 blackboxWriteFloat(data->inflightAdjustment.newFloatValue);
1618 } else {
1619 blackboxWrite(data->inflightAdjustment.adjustmentFunction);
1620 blackboxWriteSignedVB(data->inflightAdjustment.newValue);
1621 }
1622 break;
1623 case FLIGHT_LOG_EVENT_LOGGING_RESUME:
1624 blackboxWriteUnsignedVB(data->loggingResume.logIteration);
1625 blackboxWriteUnsignedVB(data->loggingResume.currentTime);
1626 break;
1627 case FLIGHT_LOG_EVENT_LOG_END:
1628 blackboxWriteString("End of log");
1629 blackboxWrite(0);
1630 break;
1631 default:
1632 break;
1633 }
1634 }
1635
1636 /* 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 */
1637 static void blackboxCheckAndLogArmingBeep(void)
1638 {
1639 // Use != so that we can still detect a change if the counter wraps
1640 if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) {
1641 blackboxLastArmingBeep = getArmingBeepTimeMicros();
1642 flightLogEvent_syncBeep_t eventData;
1643 eventData.time = blackboxLastArmingBeep;
1644 blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *)&eventData);
1645 }
1646 }
1647
1648 /* monitor the flight mode event status and trigger an event record if the state changes */
1649 static void blackboxCheckAndLogFlightMode(void)
1650 {
1651 // Use != so that we can still detect a change if the counter wraps
1652 if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
1653 flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
1654 eventData.lastFlags = blackboxLastFlightModeFlags;
1655 memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
1656 memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
1657 blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
1658 }
1659 }
1660
1661 STATIC_UNIT_TESTED bool blackboxShouldLogPFrame(void)
1662 {
1663 return blackboxPFrameIndex == 0 && blackboxPInterval != 0;
1664 }
1665
1666 STATIC_UNIT_TESTED bool blackboxShouldLogIFrame(void)
1667 {
1668 return blackboxLoopIndex == 0;
1669 }
1670
1671 /*
1672 * If the GPS home point has been updated, or every 128 I-frames (~10 seconds), write the
1673 * GPS home position.
1674 *
1675 * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
1676 * still be interpreted correctly.
1677 */
1678 #ifdef USE_GPS
1679 STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void)
1680 {
1681 if ((GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
1682 || (blackboxPFrameIndex == blackboxIInterval / 2 && blackboxIFrameIndex % 128 == 0)) && isFieldEnabled(FIELD_SELECT(GPS))) {
1683 return true;
1684 }
1685 return false;
1686 }
1687 #endif // GPS
1688
1689 // Called once every FC loop in order to keep track of how many FC loop iterations have passed
1690 STATIC_UNIT_TESTED void blackboxAdvanceIterationTimers(void)
1691 {
1692 ++blackboxSlowFrameIterationTimer;
1693 ++blackboxIteration;
1694
1695 if (++blackboxLoopIndex >= blackboxIInterval) {
1696 blackboxLoopIndex = 0;
1697 blackboxIFrameIndex++;
1698 blackboxPFrameIndex = 0;
1699 } else if (++blackboxPFrameIndex >= blackboxPInterval) {
1700 blackboxPFrameIndex = 0;
1701 }
1702 }
1703
1704 // Called once every FC loop in order to log the current state
1705 STATIC_UNIT_TESTED void blackboxLogIteration(timeUs_t currentTimeUs)
1706 {
1707 // Write a keyframe every blackboxIInterval frames so we can resynchronise upon missing frames
1708 if (blackboxShouldLogIFrame()) {
1709 /*
1710 * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
1711 * an additional item to write at the same time). Unless we're *only* logging "I" frames, then we have no choice.
1712 */
1713 if (blackboxIsOnlyLoggingIntraframes()) {
1714 writeSlowFrameIfNeeded();
1715 }
1716
1717 loadMainState(currentTimeUs);
1718 writeIntraframe();
1719 } else {
1720 blackboxCheckAndLogArmingBeep();
1721 blackboxCheckAndLogFlightMode(); // Check for FlightMode status change event
1722
1723 if (blackboxShouldLogPFrame()) {
1724 /*
1725 * We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
1726 * So only log slow frames during loop iterations where we log a main frame.
1727 */
1728 writeSlowFrameIfNeeded();
1729
1730 loadMainState(currentTimeUs);
1731 writeInterframe();
1732 }
1733 #ifdef USE_GPS
1734 if (featureIsEnabled(FEATURE_GPS) && isFieldEnabled(FIELD_SELECT(GPS))) {
1735 if (blackboxShouldLogGpsHomeFrame()) {
1736 writeGPSHomeFrame();
1737 writeGPSFrame(currentTimeUs);
1738 } else if (gpsSol.numSat != gpsHistory.GPS_numSat
1739 || gpsSol.llh.lat != gpsHistory.GPS_coord[GPS_LATITUDE]
1740 || gpsSol.llh.lon != gpsHistory.GPS_coord[GPS_LONGITUDE]) {
1741 //We could check for velocity changes as well but I doubt it changes independent of position
1742 writeGPSFrame(currentTimeUs);
1743 }
1744 }
1745 #endif
1746 }
1747
1748 //Flush every iteration so that our runtime variance is minimized
1749 blackboxDeviceFlush();
1750 }
1751
1752 /**
1753 * Call each flight loop iteration to perform blackbox logging.
1754 */
1755 void blackboxUpdate(timeUs_t currentTimeUs)
1756 {
1757 static BlackboxState cacheFlushNextState;
1758
1759 switch (blackboxState) {
1760 case BLACKBOX_STATE_STOPPED:
1761 if (ARMING_FLAG(ARMED)) {
1762 blackboxOpen();
1763 blackboxStart();
1764 }
1765 #ifdef USE_FLASHFS
1766 if (IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) {
1767 blackboxSetState(BLACKBOX_STATE_START_ERASE);
1768 }
1769 #endif
1770 break;
1771 case BLACKBOX_STATE_PREPARE_LOG_FILE:
1772 if (blackboxDeviceBeginLog()) {
1773 blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
1774 }
1775 break;
1776 case BLACKBOX_STATE_SEND_HEADER:
1777 blackboxReplenishHeaderBudget();
1778 //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
1779
1780 /*
1781 * Once the UART has had time to init, transmit the header in chunks so we don't overflow its transmit
1782 * buffer, overflow the OpenLog's buffer, or keep the main loop busy for too long.
1783 */
1784 if (millis() > xmitState.u.startTime + 100) {
1785 if (blackboxDeviceReserveBufferSpace(BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION) == BLACKBOX_RESERVE_SUCCESS) {
1786 for (int i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
1787 blackboxWrite(blackboxHeader[xmitState.headerIndex]);
1788 blackboxHeaderBudget--;
1789 }
1790 if (blackboxHeader[xmitState.headerIndex] == '\0') {
1791 blackboxSetState(BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER);
1792 }
1793 }
1794 }
1795 break;
1796 case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
1797 blackboxReplenishHeaderBudget();
1798 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1799 if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAYLEN(blackboxMainFields),
1800 &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
1801 #ifdef USE_GPS
1802 if (featureIsEnabled(FEATURE_GPS) && isFieldEnabled(FIELD_SELECT(GPS))) {
1803 blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADER);
1804 } else
1805 #endif
1806 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
1807 }
1808 break;
1809 #ifdef USE_GPS
1810 case BLACKBOX_STATE_SEND_GPS_H_HEADER:
1811 blackboxReplenishHeaderBudget();
1812 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1813 if (!sendFieldDefinition('H', 0, blackboxGpsHFields, blackboxGpsHFields + 1, ARRAYLEN(blackboxGpsHFields),
1814 NULL, NULL) && isFieldEnabled(FIELD_SELECT(GPS))) {
1815 blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADER);
1816 }
1817 break;
1818 case BLACKBOX_STATE_SEND_GPS_G_HEADER:
1819 blackboxReplenishHeaderBudget();
1820 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1821 if (!sendFieldDefinition('G', 0, blackboxGpsGFields, blackboxGpsGFields + 1, ARRAYLEN(blackboxGpsGFields),
1822 &blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition) && isFieldEnabled(FIELD_SELECT(GPS))) {
1823 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
1824 }
1825 break;
1826 #endif
1827 case BLACKBOX_STATE_SEND_SLOW_HEADER:
1828 blackboxReplenishHeaderBudget();
1829 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1830 if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAYLEN(blackboxSlowFields),
1831 NULL, NULL)) {
1832 cacheFlushNextState = BLACKBOX_STATE_SEND_SYSINFO;
1833 blackboxSetState(BLACKBOX_STATE_CACHE_FLUSH);
1834 }
1835 break;
1836 case BLACKBOX_STATE_SEND_SYSINFO:
1837 blackboxReplenishHeaderBudget();
1838 //On entry of this state, xmitState.headerIndex is 0
1839
1840 //Keep writing chunks of the system info headers until it returns true to signal completion
1841 if (blackboxWriteSysinfo()) {
1842 /*
1843 * Wait for header buffers to drain completely before data logging begins to ensure reliable header delivery
1844 * (overflowing circular buffers causes all data to be discarded, so the first few logged iterations
1845 * could wipe out the end of the header if we weren't careful)
1846 */
1847 cacheFlushNextState = BLACKBOX_STATE_RUNNING;
1848 blackboxSetState(BLACKBOX_STATE_CACHE_FLUSH);
1849 }
1850 break;
1851 case BLACKBOX_STATE_CACHE_FLUSH:
1852 // Flush the cache and wait until all possible entries have been written to the media
1853 if (blackboxDeviceFlushForceComplete()) {
1854 blackboxSetState(cacheFlushNextState);
1855 }
1856 break;
1857 case BLACKBOX_STATE_PAUSED:
1858 // Only allow resume to occur during an I-frame iteration, so that we have an "I" base to work from
1859 if (IS_RC_MODE_ACTIVE(BOXBLACKBOX) && blackboxShouldLogIFrame()) {
1860 // Write a log entry so the decoder is aware that our large time/iteration skip is intended
1861 flightLogEvent_loggingResume_t resume;
1862
1863 resume.logIteration = blackboxIteration;
1864 resume.currentTime = currentTimeUs;
1865
1866 blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
1867 blackboxSetState(BLACKBOX_STATE_RUNNING);
1868
1869 blackboxLogIteration(currentTimeUs);
1870 }
1871 // Keep the logging timers ticking so our log iteration continues to advance
1872 blackboxAdvanceIterationTimers();
1873 break;
1874 case BLACKBOX_STATE_RUNNING:
1875 // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
1876 // Prevent the Pausing of the log on the mode switch if in Motor Test Mode
1877 if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX) && !startedLoggingInTestMode) {
1878 blackboxSetState(BLACKBOX_STATE_PAUSED);
1879 } else {
1880 blackboxLogIteration(currentTimeUs);
1881 }
1882 blackboxAdvanceIterationTimers();
1883 break;
1884 case BLACKBOX_STATE_SHUTTING_DOWN:
1885 //On entry of this state, startTime is set
1886 /*
1887 * Wait for the log we've transmitted to make its way to the logger before we release the serial port,
1888 * since releasing the port clears the Tx buffer.
1889 *
1890 * Don't wait longer than it could possibly take if something funky happens.
1891 */
1892 if (blackboxDeviceEndLog(blackboxLoggedAnyFrames) && (millis() > xmitState.u.startTime + BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS || blackboxDeviceFlushForce())) {
1893 blackboxDeviceClose();
1894 blackboxSetState(BLACKBOX_STATE_STOPPED);
1895 }
1896 break;
1897 #ifdef USE_FLASHFS
1898 case BLACKBOX_STATE_START_ERASE:
1899 blackboxEraseAll();
1900 blackboxSetState(BLACKBOX_STATE_ERASING);
1901 beeper(BEEPER_BLACKBOX_ERASE);
1902 break;
1903 case BLACKBOX_STATE_ERASING:
1904 if (isBlackboxErased()) {
1905 //Done erasing
1906 blackboxSetState(BLACKBOX_STATE_ERASED);
1907 beeper(BEEPER_BLACKBOX_ERASE);
1908 }
1909 break;
1910 case BLACKBOX_STATE_ERASED:
1911 if (!IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) {
1912 blackboxSetState(BLACKBOX_STATE_STOPPED);
1913 }
1914 break;
1915 #endif
1916 default:
1917 break;
1918 }
1919
1920 // Did we run out of room on the device? Stop!
1921 if (isBlackboxDeviceFull()) {
1922 #ifdef USE_FLASHFS
1923 if (blackboxState != BLACKBOX_STATE_ERASING
1924 && blackboxState != BLACKBOX_STATE_START_ERASE
1925 && blackboxState != BLACKBOX_STATE_ERASED)
1926 #endif
1927 {
1928 blackboxSetState(BLACKBOX_STATE_STOPPED);
1929 // ensure we reset the test mode flag if we stop due to full memory card
1930 if (startedLoggingInTestMode) {
1931 startedLoggingInTestMode = false;
1932 }
1933 }
1934 } else { // Only log in test mode if there is room!
1935 switch (blackboxConfig()->mode) {
1936 case BLACKBOX_MODE_MOTOR_TEST:
1937 // Handle Motor Test Mode
1938 if (inMotorTestMode()) {
1939 if (blackboxState==BLACKBOX_STATE_STOPPED) {
1940 startInTestMode();
1941 }
1942 } else {
1943 if (blackboxState!=BLACKBOX_STATE_STOPPED) {
1944 stopInTestMode();
1945 }
1946 }
1947
1948 break;
1949 case BLACKBOX_MODE_ALWAYS_ON:
1950 if (blackboxState==BLACKBOX_STATE_STOPPED) {
1951 startInTestMode();
1952 }
1953
1954 break;
1955 case BLACKBOX_MODE_NORMAL:
1956 default:
1957
1958 break;
1959 }
1960 }
1961 }
1962
1963 int blackboxCalculatePDenom(int rateNum, int rateDenom)
1964 {
1965 return blackboxIInterval * rateNum / rateDenom;
1966 }
1967
1968 uint8_t blackboxGetRateDenom(void)
1969 {
1970 return blackboxPInterval;
1971
1972 }
1973
1974 uint16_t blackboxGetPRatio(void)
1975 {
1976 return blackboxIInterval / blackboxPInterval;
1977 }
1978
1979 uint8_t blackboxCalculateSampleRate(uint16_t pRatio)
1980 {
1981 return LOG2(32000 / (targetPidLooptime * pRatio));
1982 }
1983
1984 /**
1985 * Call during system startup to initialize the blackbox.
1986 */
1987 void blackboxInit(void)
1988 {
1989 blackboxResetIterationTimers();
1990
1991 // an I-frame is written every 32ms
1992 // blackboxUpdate() is run in synchronisation with the PID loop
1993 // targetPidLooptime is 1000 for 1kHz loop, 500 for 2kHz loop etc, targetPidLooptime is rounded for short looptimes
1994 blackboxIInterval = (uint16_t)(32 * 1000 / targetPidLooptime);
1995
1996 blackboxPInterval = 1 << blackboxConfig()->sample_rate;
1997 if (blackboxPInterval > blackboxIInterval) {
1998 blackboxPInterval = 0; // log only I frames if logging frequency is too low
1999 }
2000
2001 if (blackboxConfig()->device) {
2002 blackboxSetState(BLACKBOX_STATE_STOPPED);
2003 } else {
2004 blackboxSetState(BLACKBOX_STATE_DISABLED);
2005 }
2006 blackboxSInterval = blackboxIInterval * 256; // S-frame is written every 256*32 = 8192ms, approx every 8 seconds
2007
2008 blackboxHighResolutionScale = blackboxConfig()->high_resolution ? 10.0f : 1.0f;
2009 }
2010 #endif