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