?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
remove test code
[inav.git] / src / main / blackbox / blackbox.c
blob5a72d323b090ccd0230649bf47f236914157a526
1 /*
2 * This file is part of Cleanflight.
3 *
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
8 *
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
16 */
17
18 #include <stdbool.h>
19 #include <stdint.h>
20 #include <string.h>
21 #include <math.h>
22
23 #include "platform.h"
24
25 #ifdef USE_BLACKBOX
26
27 #include "blackbox.h"
28 #include "blackbox_encoding.h"
29 #include "blackbox_io.h"
30
31 #include "build/debug.h"
32 #include "build/version.h"
33
34 #include "common/axis.h"
35 #include "common/encoding.h"
36 #include "common/maths.h"
37 #include "common/time.h"
38 #include "common/utils.h"
39
40 #include "config/feature.h"
41 #include "config/parameter_group.h"
42 #include "config/parameter_group_ids.h"
43
44 #include "drivers/accgyro/accgyro.h"
45 #include "drivers/compass/compass.h"
46 #include "drivers/sensor.h"
47 #include "drivers/time.h"
48 #include "drivers/pwm_output.h"
49
50 #include "fc/config.h"
51 #include "fc/controlrate_profile.h"
52 #include "fc/fc_core.h"
53 #include "fc/rc_controls.h"
54 #include "fc/rc_modes.h"
55 #include "fc/runtime_config.h"
56 #include "fc/settings.h"
57 #include "fc/rc_smoothing.h"
58
59 #include "flight/failsafe.h"
60 #include "flight/imu.h"
61 #include "flight/mixer.h"
62 #include "flight/pid.h"
63 #include "flight/servos.h"
64 #include "flight/rpm_filter.h"
65
66 #include "io/beeper.h"
67 #include "io/gps.h"
68
69 #include "navigation/navigation.h"
70
71 #include "rx/rx.h"
72 #include "rx/msp_override.h"
73
74 #include "sensors/diagnostics.h"
75 #include "sensors/acceleration.h"
76 #include "sensors/barometer.h"
77 #include "sensors/battery.h"
78 #include "sensors/compass.h"
79 #include "sensors/gyro.h"
80 #include "sensors/pitotmeter.h"
81 #include "sensors/rangefinder.h"
82 #include "sensors/sensors.h"
83 #include "sensors/esc_sensor.h"
84 #include "flight/wind_estimator.h"
85 #include "sensors/temperature.h"
86
87
88 #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
89 #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH
90 #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT)
91 #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_SDCARD
92 #else
93 #define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_SERIAL
94 #endif
95
96 #ifdef SDCARD_DETECT_INVERTED
97 #define BLACKBOX_INVERTED_CARD_DETECTION 1
98 #else
99 #define BLACKBOX_INVERTED_CARD_DETECTION 0
100 #endif
101
102 PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 2);
103
104 PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
105 .device = DEFAULT_BLACKBOX_DEVICE,
106 .rate_num = SETTING_BLACKBOX_RATE_NUM_DEFAULT,
107 .rate_denom = SETTING_BLACKBOX_RATE_DENOM_DEFAULT,
108 .invertedCardDetection = BLACKBOX_INVERTED_CARD_DETECTION,
109 .includeFlags = BLACKBOX_FEATURE_NAV_PID | BLACKBOX_FEATURE_NAV_POS |
110 BLACKBOX_FEATURE_MAG | BLACKBOX_FEATURE_ACC | BLACKBOX_FEATURE_ATTITUDE |
111 BLACKBOX_FEATURE_RC_DATA | BLACKBOX_FEATURE_RC_COMMAND | BLACKBOX_FEATURE_MOTORS,
112 );
113
114 void blackboxIncludeFlagSet(uint32_t mask)
115 {
116 blackboxConfigMutable()->includeFlags |= mask;
117 }
118
119 void blackboxIncludeFlagClear(uint32_t mask)
120 {
121 blackboxConfigMutable()->includeFlags &= ~(mask);
122 }
123
124 bool blackboxIncludeFlag(uint32_t mask) {
125 return (blackboxConfig()->includeFlags & mask) == mask;
126 }
127
128 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
129 static const int32_t blackboxSInterval = 4096;
130
131 // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
132
133 #define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
134 #define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)
135 #define CONDITION(x) CONCAT(FLIGHT_LOG_FIELD_CONDITION_, x)
136 #define UNSIGNED FLIGHT_LOG_FIELD_UNSIGNED
137 #define SIGNED FLIGHT_LOG_FIELD_SIGNED
138
139 static const char blackboxHeader[] =
140 "H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
141 "H Data version:2\n";
142
143 static const char* const blackboxFieldHeaderNames[] = {
144 "name",
145 "signed",
146 "predictor",
147 "encoding",
148 "predictor",
149 "encoding"
150 };
151
152 /* All field definition structs should look like this (but with longer arrs): */
153 typedef struct blackboxFieldDefinition_s {
154 const char *name;
155 // If the field name has a number to be included in square brackets [1] afterwards, set it here, or -1 for no brackets:
156 int8_t fieldNameIndex;
157
158 // Each member of this array will be the value to print for this field for the given header index
159 uint8_t arr[1];
160 } blackboxFieldDefinition_t;
161
162 #define BLACKBOX_DELTA_FIELD_HEADER_COUNT ARRAYLEN(blackboxFieldHeaderNames)
163 #define BLACKBOX_SIMPLE_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
164 #define BLACKBOX_CONDITIONAL_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
165
166 typedef struct blackboxSimpleFieldDefinition_s {
167 const char *name;
168 int8_t fieldNameIndex;
169
170 uint8_t isSigned;
171 uint8_t predict;
172 uint8_t encode;
173 } blackboxSimpleFieldDefinition_t;
174
175 typedef struct blackboxConditionalFieldDefinition_s {
176 const char *name;
177 int8_t fieldNameIndex;
178
179 uint8_t isSigned;
180 uint8_t predict;
181 uint8_t encode;
182 uint8_t condition; // Decide whether this field should appear in the log
183 } blackboxConditionalFieldDefinition_t;
184
185 typedef struct blackboxDeltaFieldDefinition_s {
186 const char *name;
187 int8_t fieldNameIndex;
188
189 uint8_t isSigned;
190 uint8_t Ipredict;
191 uint8_t Iencode;
192 uint8_t Ppredict;
193 uint8_t Pencode;
194 uint8_t condition; // Decide whether this field should appear in the log
195 } blackboxDeltaFieldDefinition_t;
196
197 /**
198 * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
199 * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
200 * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
201 * the encoding we've promised here).
202 */
203 static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
204 /* loopIteration doesn't appear in P frames since it always increments */
205 {"loopIteration",-1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC), .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)},
206 /* Time advances pretty steadily so the P-frame prediction is a straight line */
207 {"time", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(STRAIGHT_LINE), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
208 {"axisRate", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
209 {"axisRate", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
210 {"axisRate", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
211 {"axisP", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
212 {"axisP", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
213 {"axisP", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
214 /* I terms get special packed encoding in P frames: */
215 {"axisI", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
216 {"axisI", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
217 {"axisI", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
218 {"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
219 {"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
220 {"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
221 {"axisF", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ALWAYS},
222 {"axisF", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ALWAYS},
223 {"axisF", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ALWAYS},
224
225 {"fwAltP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
226 {"fwAltI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
227 {"fwAltD", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
228 {"fwAltOut", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
229 {"fwPosP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
230 {"fwPosI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
231 {"fwPosD", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
232 {"fwPosOut", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
233
234 {"mcPosAxisP", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
235 {"mcPosAxisP", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
236 {"mcPosAxisP", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
237 {"mcVelAxisP", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
238 {"mcVelAxisP", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
239 {"mcVelAxisP", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
240 {"mcVelAxisI", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
241 {"mcVelAxisI", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
242 {"mcVelAxisI", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
243 {"mcVelAxisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
244 {"mcVelAxisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
245 {"mcVelAxisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
246 {"mcVelAxisFF", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
247 {"mcVelAxisFF", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
248 {"mcVelAxisFF", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
249 {"mcVelAxisOut",0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
250 {"mcVelAxisOut",1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
251 {"mcVelAxisOut",2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
252 {"mcSurfaceP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
253 {"mcSurfaceI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
254 {"mcSurfaceD", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
255 {"mcSurfaceOut",-1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
256
257 /* rcData are encoded together as a group: */
258 {"rcData", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_DATA},
259 {"rcData", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_DATA},
260 {"rcData", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_DATA},
261 {"rcData", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_DATA},
262 /* rcCommands are encoded together as a group in P-frames: */
263 {"rcCommand", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND},
264 {"rcCommand", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND},
265 {"rcCommand", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND},
266 /* Throttle is always in the range [minthrottle..maxthrottle]: */
267 {"rcCommand", 3, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND},
268
269 {"vbat", -1, UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT},
270 {"amperage", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE},
271
272 #ifdef USE_MAG
273 {"magADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
274 {"magADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
275 {"magADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
276 #endif
277 #ifdef USE_BARO
278 {"BaroAlt", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
279 #endif
280 #ifdef USE_PITOT
281 {"AirSpeed", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_PITOT},
282 #endif
283 #ifdef USE_RANGEFINDER
284 {"surfaceRaw", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_SURFACE},
285 #endif
286 {"rssi", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_RSSI},
287
288 /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
289 {"gyroADC", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
290 {"gyroADC", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
291 {"gyroADC", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
292
293 {"gyroRaw", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
294 {"gyroRaw", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
295 {"gyroRaw", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW},
296
297 {"gyroPeakRoll", 0, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL},
298 {"gyroPeakRoll", 1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL},
299 {"gyroPeakRoll", 2, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL},
300
301 {"gyroPeakPitch", 0, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH},
302 {"gyroPeakPitch", 1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH},
303 {"gyroPeakPitch", 2, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH},
304
305 {"gyroPeakYaw", 0, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW},
306 {"gyroPeakYaw", 1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW},
307 {"gyroPeakYaw", 2, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW},
308
309
310 {"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
311 {"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
312 {"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
313 {"accVib", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
314 {"attitude", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
315 {"attitude", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
316 {"attitude", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
317 {"debug", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
318 {"debug", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
319 {"debug", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
320 {"debug", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
321 {"debug", 4, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
322 {"debug", 5, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
323 {"debug", 6, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
324 {"debug", 7, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_DEBUG},
325 /* 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): */
326 {"motor", 0, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
327 /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
328 {"motor", 1, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_2)},
329 {"motor", 2, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_3)},
330 {"motor", 3, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_4)},
331 {"motor", 4, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_5)},
332 {"motor", 5, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_6)},
333 {"motor", 6, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)},
334 {"motor", 7, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)},
335
336 /* servos */
337 {"servo", 0, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
338 {"servo", 1, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
339 {"servo", 2, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
340 {"servo", 3, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
341 {"servo", 4, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
342 {"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
343 {"servo", 6, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
344 {"servo", 7, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
345 {"servo", 8, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
346 {"servo", 9, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
347 {"servo", 10, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
348 {"servo", 11, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
349 {"servo", 12, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
350 {"servo", 13, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
351 {"servo", 14, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
352 {"servo", 15, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
353 {"servo", 16, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
354 {"servo", 17, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(SERVOS)},
355
356 {"navState", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
357 {"navFlags", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
358 {"navEPH", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
359 {"navEPV", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
360 {"navPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
361 {"navPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
362 {"navPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
363 {"navVel", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
364 {"navVel", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
365 {"navVel", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
366 {"navTgtVel", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
367 {"navTgtVel", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
368 {"navTgtVel", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
369 {"navTgtPos", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
370 {"navTgtPos", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
371 {"navTgtPos", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
372 {"navTgtHdg", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
373 {"navSurf", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_POS},
374 {"navAcc", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
375 {"navAcc", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
376 {"navAcc", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_NAV_ACC},
377 };
378
379 #ifdef USE_GPS
380 // GPS position/vel frame
381 static const blackboxConditionalFieldDefinition_t blackboxGpsGFields[] = {
382 {"time", -1, UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)},
383 {"GPS_fixType", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
384 {"GPS_numSat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
385 {"GPS_coord", 0, SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
386 {"GPS_coord", 1, SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
387 {"GPS_altitude", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
388 {"GPS_speed", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
389 {"GPS_ground_course", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
390 {"GPS_hdop", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
391 {"GPS_eph", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
392 {"GPS_epv", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
393 {"GPS_velned", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
394 {"GPS_velned", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
395 {"GPS_velned", 2, SIGNED, PREDICT(0), ENCODING(SIGNED_VB), CONDITION(ALWAYS)}
396 };
397
398 // GPS home frame
399 static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = {
400 {"GPS_home", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
401 {"GPS_home", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}
402 };
403 #endif
404
405 // Rarely-updated fields
406 static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
407 /* "flightModeFlags" renamed internally to more correct ref of rcModeFlags, since it logs rc boxmode selections,
408 * but name kept for external compatibility reasons.
409 * "activeFlightModeFlags" logs actual active flight modes rather than rc boxmodes.
410 * 'active' should at least distinguish it from the existing "flightModeFlags" */
411
412 {"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
413 {"flightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
414 {"flightModeFlags2", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
415 {"activeFlightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
416 {"stateFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
417
418 {"failsafePhase", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)},
419 {"rxSignalReceived", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)},
420 {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)},
421 {"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)},
422
423 {"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
424 {"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
425 {"sagCompensatedVBat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
426 {"wind", 0, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
427 {"wind", 1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
428 {"wind", 2, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
429 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
430 {"mspOverrideFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
431 #endif
432 {"IMUTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
433 #ifdef USE_BARO
434 {"baroTemperature", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
435 #endif
436 #ifdef USE_TEMPERATURE_SENSOR
437 {"sens0Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
438 {"sens1Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
439 {"sens2Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
440 {"sens3Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
441 {"sens4Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
442 {"sens5Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
443 {"sens6Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
444 {"sens7Temp", -1, SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
445 #endif
446 #ifdef USE_ESC_SENSOR
447 {"escRPM", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
448 {"escTemperature", -1, SIGNED, PREDICT(PREVIOUS), ENCODING(SIGNED_VB)},
449 #endif
450 };
451
452 typedef enum BlackboxState {
453 BLACKBOX_STATE_DISABLED = 0,
454 BLACKBOX_STATE_STOPPED,
455 BLACKBOX_STATE_PREPARE_LOG_FILE,
456 BLACKBOX_STATE_SEND_HEADER,
457 BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER,
458 BLACKBOX_STATE_SEND_GPS_H_HEADER,
459 BLACKBOX_STATE_SEND_GPS_G_HEADER,
460 BLACKBOX_STATE_SEND_SLOW_HEADER,
461 BLACKBOX_STATE_SEND_SYSINFO,
462 BLACKBOX_STATE_PAUSED,
463 BLACKBOX_STATE_RUNNING,
464 BLACKBOX_STATE_SHUTTING_DOWN
465 } BlackboxState;
466
467 #define BLACKBOX_FIRST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_HEADER
468 #define BLACKBOX_LAST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_SYSINFO
469
470 typedef struct blackboxMainState_s {
471 uint32_t time;
472
473 int32_t axisPID_P[XYZ_AXIS_COUNT];
474 int32_t axisPID_I[XYZ_AXIS_COUNT];
475 int32_t axisPID_D[XYZ_AXIS_COUNT];
476 int32_t axisPID_F[XYZ_AXIS_COUNT];
477 int32_t axisPID_Setpoint[XYZ_AXIS_COUNT];
478
479 int32_t mcPosAxisP[XYZ_AXIS_COUNT];
480 int32_t mcVelAxisPID[4][XYZ_AXIS_COUNT];
481 int32_t mcVelAxisOutput[XYZ_AXIS_COUNT];
482
483 int32_t mcSurfacePID[3];
484 int32_t mcSurfacePIDOutput;
485
486 int32_t fwAltPID[3];
487 int32_t fwAltPIDOutput;
488 int32_t fwPosPID[3];
489 int32_t fwPosPIDOutput;
490
491 int16_t rcData[4];
492 int16_t rcCommand[4];
493 int16_t gyroADC[XYZ_AXIS_COUNT];
494 int16_t gyroRaw[XYZ_AXIS_COUNT];
495
496 int16_t gyroPeaksRoll[DYN_NOTCH_PEAK_COUNT];
497 int16_t gyroPeaksPitch[DYN_NOTCH_PEAK_COUNT];
498 int16_t gyroPeaksYaw[DYN_NOTCH_PEAK_COUNT];
499
500 int16_t accADC[XYZ_AXIS_COUNT];
501 int16_t accVib;
502 int16_t attitude[XYZ_AXIS_COUNT];
503 int32_t debug[DEBUG32_VALUE_COUNT];
504 int16_t motor[MAX_SUPPORTED_MOTORS];
505 int16_t servo[MAX_SUPPORTED_SERVOS];
506
507 uint16_t vbat;
508 int16_t amperage;
509
510 #ifdef USE_BARO
511 int32_t BaroAlt;
512 #endif
513 #ifdef USE_PITOT
514 int32_t airSpeed;
515 #endif
516 #ifdef USE_MAG
517 int16_t magADC[XYZ_AXIS_COUNT];
518 #endif
519 #ifdef USE_RANGEFINDER
520 int32_t surfaceRaw;
521 #endif
522 uint16_t rssi;
523 int16_t navState;
524 uint16_t navFlags;
525 uint16_t navEPH;
526 uint16_t navEPV;
527 int32_t navPos[XYZ_AXIS_COUNT];
528 int16_t navRealVel[XYZ_AXIS_COUNT];
529 int16_t navAccNEU[XYZ_AXIS_COUNT];
530 int16_t navTargetVel[XYZ_AXIS_COUNT];
531 int32_t navTargetPos[XYZ_AXIS_COUNT];
532 int16_t navHeading;
533 uint16_t navTargetHeading;
534 int16_t navSurface;
535 } blackboxMainState_t;
536
537 typedef struct blackboxGpsState_s {
538 int32_t GPS_home[2];
539 int32_t GPS_coord[2];
540 uint8_t GPS_numSat;
541 } blackboxGpsState_t;
542
543 // This data is updated really infrequently:
544 typedef struct blackboxSlowState_s {
545 uint32_t rcModeFlags;
546 uint32_t rcModeFlags2;
547 uint32_t activeFlightModeFlags;
548 uint32_t stateFlags;
549 uint8_t failsafePhase;
550 bool rxSignalReceived;
551 bool rxFlightChannelsValid;
552 int32_t hwHealthStatus;
553 uint16_t powerSupplyImpedance;
554 uint16_t sagCompensatedVBat;
555 int16_t wind[XYZ_AXIS_COUNT];
556 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
557 uint16_t mspOverrideFlags;
558 #endif
559 int16_t imuTemperature;
560 #ifdef USE_BARO
561 int16_t baroTemperature;
562 #endif
563 #ifdef USE_TEMPERATURE_SENSOR
564 int16_t tempSensorTemperature[MAX_TEMP_SENSORS];
565 #endif
566 #ifdef USE_ESC_SENSOR
567 uint32_t escRPM;
568 int8_t escTemperature;
569 #endif
570 uint16_t rxUpdateRate;
571 uint8_t activeWpNumber;
572 } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
573
574 //From rc_controls.c
575 extern boxBitmask_t rcModeActivationMask;
576
577 static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
578
579 static uint32_t blackboxLastArmingBeep = 0;
580 static uint32_t blackboxLastRcModeFlags = 0;
581
582 static struct {
583 uint32_t headerIndex;
584
585 /* Since these fields are used during different blackbox states (never simultaneously) we can
586 * overlap them to save on RAM
587 */
588 union {
589 int fieldIndex;
590 uint32_t startTime;
591 } u;
592 } xmitState;
593
594 // Cache for FLIGHT_LOG_FIELD_CONDITION_* test results:
595 static uint64_t blackboxConditionCache;
596
597 STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions);
598
599 static uint32_t blackboxIFrameInterval;
600 static uint32_t blackboxIteration;
601 static uint16_t blackboxPFrameIndex;
602 static uint16_t blackboxIFrameIndex;
603 static uint16_t blackboxSlowFrameIterationTimer;
604 static bool blackboxLoggedAnyFrames;
605
606 /*
607 * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
608 * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs
609 * to encode:
610 */
611 static uint16_t vbatReference;
612
613 static blackboxGpsState_t gpsHistory;
614 static blackboxSlowState_t slowHistory;
615
616 // Keep a history of length 2, plus a buffer for MW to store the new values into
617 static EXTENDED_FASTRAM blackboxMainState_t blackboxHistoryRing[3];
618
619 // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
620 static EXTENDED_FASTRAM blackboxMainState_t* blackboxHistory[3];
621
622 static bool blackboxModeActivationConditionPresent = false;
623
624 /**
625 * Return true if it is safe to edit the Blackbox configuration.
626 */
627 bool blackboxMayEditConfig(void)
628 {
629 return blackboxState <= BLACKBOX_STATE_STOPPED;
630 }
631
632 static bool blackboxIsOnlyLoggingIntraframes(void)
633 {
634 return blackboxConfig()->rate_num == 1 && blackboxConfig()->rate_denom == blackboxIFrameInterval;
635 }
636
637 static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
638 {
639 switch (condition) {
640 case FLIGHT_LOG_FIELD_CONDITION_ALWAYS:
641 return true;
642
643 case FLIGHT_LOG_FIELD_CONDITION_MOTORS:
644 return blackboxIncludeFlag(BLACKBOX_FEATURE_MOTORS);
645
646 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1:
647 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2:
648 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3:
649 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4:
650 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5:
651 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6:
652 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7:
653 case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8:
654 return (getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1) && blackboxIncludeFlag(BLACKBOX_FEATURE_MOTORS);
655
656 case FLIGHT_LOG_FIELD_CONDITION_SERVOS:
657 return isMixerUsingServos();
658
659 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
660 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
661 case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
662 // D output can be set by either the D or the FF term
663 return pidBank()->pid[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0].D != 0;
664
665 case FLIGHT_LOG_FIELD_CONDITION_MAG:
666 #ifdef USE_MAG
667 return sensors(SENSOR_MAG) && blackboxIncludeFlag(BLACKBOX_FEATURE_MAG);
668 #else
669 return false;
670 #endif
671
672 case FLIGHT_LOG_FIELD_CONDITION_BARO:
673 #ifdef USE_BARO
674 return sensors(SENSOR_BARO);
675 #else
676 return false;
677 #endif
678
679 case FLIGHT_LOG_FIELD_CONDITION_PITOT:
680 #ifdef USE_PITOT
681 return sensors(SENSOR_PITOT);
682 #else
683 return false;
684 #endif
685
686 case FLIGHT_LOG_FIELD_CONDITION_VBAT:
687 return feature(FEATURE_VBAT);
688
689 case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE:
690 return feature(FEATURE_CURRENT_METER) && batteryMetersConfig()->current.type == CURRENT_SENSOR_ADC;
691
692 case FLIGHT_LOG_FIELD_CONDITION_SURFACE:
693 #ifdef USE_RANGEFINDER
694 return sensors(SENSOR_RANGEFINDER);
695 #else
696 return false;
697 #endif
698
699 case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV:
700
701 return STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID);
702
703 case FLIGHT_LOG_FIELD_CONDITION_MC_NAV:
704 return !STATE(FIXED_WING_LEGACY) && blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_PID);
705
706 case FLIGHT_LOG_FIELD_CONDITION_RSSI:
707 // Assumes blackboxStart() is called after rxInit(), which should be true since
708 // logging can't be started until after all the arming checks take place
709 return getRSSISource() != RSSI_SOURCE_NONE;
710
711 case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
712 return blackboxConfig()->rate_num < blackboxConfig()->rate_denom;
713
714 case FLIGHT_LOG_FIELD_CONDITION_DEBUG:
715 return debugMode != DEBUG_NONE;
716
717 case FLIGHT_LOG_FIELD_CONDITION_NAV_ACC:
718 return blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_ACC);
719
720 case FLIGHT_LOG_FIELD_CONDITION_NAV_POS:
721 return blackboxIncludeFlag(BLACKBOX_FEATURE_NAV_POS);
722
723 case FLIGHT_LOG_FIELD_CONDITION_ACC:
724 return blackboxIncludeFlag(BLACKBOX_FEATURE_ACC);
725
726 case FLIGHT_LOG_FIELD_CONDITION_ATTITUDE:
727 return blackboxIncludeFlag(BLACKBOX_FEATURE_ATTITUDE);
728
729 case FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND:
730 return blackboxIncludeFlag(BLACKBOX_FEATURE_RC_COMMAND);
731
732 case FLIGHT_LOG_FIELD_CONDITION_RC_DATA:
733 return blackboxIncludeFlag(BLACKBOX_FEATURE_RC_DATA);
734
735 case FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW:
736 return blackboxIncludeFlag(BLACKBOX_FEATURE_GYRO_RAW);
737
738 case FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL:
739 return blackboxIncludeFlag(BLACKBOX_FEATURE_GYRO_PEAKS_ROLL);
740
741 case FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH:
742 return blackboxIncludeFlag(BLACKBOX_FEATURE_GYRO_PEAKS_PITCH);
743
744 case FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW:
745 return blackboxIncludeFlag(BLACKBOX_FEATURE_GYRO_PEAKS_YAW);
746
747 case FLIGHT_LOG_FIELD_CONDITION_NEVER:
748 return false;
749
750 default:
751 return false;
752 }
753 }
754
755 static void blackboxBuildConditionCache(void)
756 {
757 blackboxConditionCache = 0;
758 for (uint8_t cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
759
760 const uint64_t position = ((uint64_t)1) << cond;
761
762 if (testBlackboxConditionUncached(cond)) {
763 blackboxConditionCache |= position;
764 }
765 }
766 }
767
768 static bool testBlackboxCondition(FlightLogFieldCondition condition)
769 {
770 const uint64_t position = ((uint64_t)1) << condition;
771 return (blackboxConditionCache & position) != 0;
772 }
773
774 static void blackboxSetState(BlackboxState newState)
775 {
776 //Perform initial setup required for the new state
777 switch (newState) {
778 case BLACKBOX_STATE_PREPARE_LOG_FILE:
779 blackboxLoggedAnyFrames = false;
780 break;
781 case BLACKBOX_STATE_SEND_HEADER:
782 blackboxHeaderBudget = 0;
783 xmitState.headerIndex = 0;
784 xmitState.u.startTime = millis();
785 break;
786 case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
787 case BLACKBOX_STATE_SEND_GPS_G_HEADER:
788 case BLACKBOX_STATE_SEND_GPS_H_HEADER:
789 case BLACKBOX_STATE_SEND_SLOW_HEADER:
790 xmitState.headerIndex = 0;
791 xmitState.u.fieldIndex = -1;
792 break;
793 case BLACKBOX_STATE_SEND_SYSINFO:
794 xmitState.headerIndex = 0;
795 break;
796 case BLACKBOX_STATE_RUNNING:
797 blackboxSlowFrameIterationTimer = blackboxSInterval; //Force a slow frame to be written on the first iteration
798 break;
799 case BLACKBOX_STATE_SHUTTING_DOWN:
800 xmitState.u.startTime = millis();
801 break;
802 default:
803 ;
804 }
805 blackboxState = newState;
806 }
807
808 static void writeIntraframe(void)
809 {
810 blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
811
812 blackboxWrite('I');
813
814 blackboxWriteUnsignedVB(blackboxIteration);
815 blackboxWriteUnsignedVB(blackboxCurrent->time);
816
817 blackboxWriteSignedVBArray(blackboxCurrent->axisPID_Setpoint, XYZ_AXIS_COUNT);
818 blackboxWriteSignedVBArray(blackboxCurrent->axisPID_P, XYZ_AXIS_COUNT);
819 blackboxWriteSignedVBArray(blackboxCurrent->axisPID_I, XYZ_AXIS_COUNT);
820
821 // Don't bother writing the current D term if the corresponding PID setting is zero
822 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
823 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) {
824 blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]);
825 }
826 }
827 blackboxWriteSignedVBArray(blackboxCurrent->axisPID_F, XYZ_AXIS_COUNT);
828
829 if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) {
830 blackboxWriteSignedVBArray(blackboxCurrent->fwAltPID, 3);
831 blackboxWriteSignedVB(blackboxCurrent->fwAltPIDOutput);
832 blackboxWriteSignedVBArray(blackboxCurrent->fwPosPID, 3);
833 blackboxWriteSignedVB(blackboxCurrent->fwPosPIDOutput);
834 }
835
836 if (testBlackboxCondition(CONDITION(MC_NAV))) {
837
838 blackboxWriteSignedVBArray(blackboxCurrent->mcPosAxisP, XYZ_AXIS_COUNT);
839
840 for (int i = 0; i < 4; i++) {
841 blackboxWriteSignedVBArray(blackboxCurrent->mcVelAxisPID[i], XYZ_AXIS_COUNT);
842 }
843
844 blackboxWriteSignedVBArray(blackboxCurrent->mcVelAxisOutput, XYZ_AXIS_COUNT);
845
846 blackboxWriteSignedVBArray(blackboxCurrent->mcSurfacePID, 3);
847 blackboxWriteSignedVB(blackboxCurrent->mcSurfacePIDOutput);
848 }
849
850 // Write raw stick positions
851 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_DATA)) {
852 blackboxWriteSigned16VBArray(blackboxCurrent->rcData, 4);
853 }
854
855 // Write roll, pitch and yaw first:
856 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND)) {
857 blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3);
858
859 /*
860 * Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
861 * Throttle lies in range [minthrottle..maxthrottle]:
862 */
863 blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - getThrottleIdleValue());
864 }
865
866 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
867 /*
868 * Our voltage is expected to decrease over the course of the flight, so store our difference from
869 * the reference:
870 *
871 * Write 14 bits even if the number is negative (which would otherwise result in 32 bits)
872 */
873 blackboxWriteUnsignedVB((vbatReference - blackboxCurrent->vbat) & 0x3FFF);
874 }
875
876 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE)) {
877 // 12bit value directly from ADC
878 blackboxWriteSignedVB(blackboxCurrent->amperage);
879 }
880
881 #ifdef USE_MAG
882 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
883 blackboxWriteSigned16VBArray(blackboxCurrent->magADC, XYZ_AXIS_COUNT);
884 }
885 #endif
886
887 #ifdef USE_BARO
888 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) {
889 blackboxWriteSignedVB(blackboxCurrent->BaroAlt);
890 }
891 #endif
892
893 #ifdef USE_PITOT
894 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_PITOT)) {
895 blackboxWriteSignedVB(blackboxCurrent->airSpeed);
896 }
897 #endif
898
899 #ifdef USE_RANGEFINDER
900 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SURFACE)) {
901 blackboxWriteSignedVB(blackboxCurrent->surfaceRaw);
902 }
903 #endif
904
905 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RSSI)) {
906 blackboxWriteUnsignedVB(blackboxCurrent->rssi);
907 }
908
909 blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
910
911 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW)) {
912 blackboxWriteSigned16VBArray(blackboxCurrent->gyroRaw, XYZ_AXIS_COUNT);
913 }
914
915 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL)) {
916 blackboxWriteUnsignedVB(blackboxCurrent->gyroPeaksRoll[0]);
917 blackboxWriteUnsignedVB(blackboxCurrent->gyroPeaksRoll[1]);
918 blackboxWriteUnsignedVB(blackboxCurrent->gyroPeaksRoll[2]);
919 }
920
921 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH)) {
922 blackboxWriteUnsignedVB(blackboxCurrent->gyroPeaksPitch[0]);
923 blackboxWriteUnsignedVB(blackboxCurrent->gyroPeaksPitch[1]);
924 blackboxWriteUnsignedVB(blackboxCurrent->gyroPeaksPitch[2]);
925 }
926
927 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW)) {
928 blackboxWriteUnsignedVB(blackboxCurrent->gyroPeaksYaw[0]);
929 blackboxWriteUnsignedVB(blackboxCurrent->gyroPeaksYaw[1]);
930 blackboxWriteUnsignedVB(blackboxCurrent->gyroPeaksYaw[2]);
931 }
932
933 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
934 blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
935 blackboxWriteUnsignedVB(blackboxCurrent->accVib);
936 }
937
938 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
939 blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT);
940 }
941
942 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) {
943 blackboxWriteSignedVBArray(blackboxCurrent->debug, DEBUG32_VALUE_COUNT);
944 }
945
946 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MOTORS)) {
947 //Motors can be below minthrottle when disarmed, but that doesn't happen much
948 blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - getThrottleIdleValue());
949
950 //Motors tend to be similar to each other so use the first motor's value as a predictor of the others
951 const int motorCount = getMotorCount();
952 for (int x = 1; x < motorCount; x++) {
953 blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
954 }
955 }
956
957 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) {
958 for (int x = 0; x < MAX_SUPPORTED_SERVOS; x++) {
959 //Assume that servos spends most of its time around the center
960 blackboxWriteSignedVB(blackboxCurrent->servo[x] - 1500);
961 }
962 }
963
964 blackboxWriteSignedVB(blackboxCurrent->navState);
965 blackboxWriteSignedVB(blackboxCurrent->navFlags);
966
967 /*
968 * NAV_POS fields
969 */
970 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_POS)) {
971 blackboxWriteSignedVB(blackboxCurrent->navEPH);
972 blackboxWriteSignedVB(blackboxCurrent->navEPV);
973
974 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
975 blackboxWriteSignedVB(blackboxCurrent->navPos[x]);
976 }
977
978 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
979 blackboxWriteSignedVB(blackboxCurrent->navRealVel[x]);
980 }
981
982 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
983 blackboxWriteSignedVB(blackboxCurrent->navTargetVel[x]);
984 }
985
986 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
987 blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
988 }
989
990 blackboxWriteSignedVB(blackboxCurrent->navTargetHeading);
991 blackboxWriteSignedVB(blackboxCurrent->navSurface);
992 }
993
994 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_ACC)) {
995 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
996 blackboxWriteSignedVB(blackboxCurrent->navAccNEU[x]);
997 }
998 }
999
1000 //Rotate our history buffers:
1001
1002 //The current state becomes the new "before" state
1003 blackboxHistory[1] = blackboxHistory[0];
1004 //And since we have no other history, we also use it for the "before, before" state
1005 blackboxHistory[2] = blackboxHistory[0];
1006 //And advance the current state over to a blank space ready to be filled
1007 blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
1008
1009 blackboxLoggedAnyFrames = true;
1010 }
1011
1012 static void blackboxWriteArrayUsingAveragePredictor16(int arrOffsetInHistory, int count)
1013 {
1014 int16_t *curr = (int16_t*) ((char*) (blackboxHistory[0]) + arrOffsetInHistory);
1015 int16_t *prev1 = (int16_t*) ((char*) (blackboxHistory[1]) + arrOffsetInHistory);
1016 int16_t *prev2 = (int16_t*) ((char*) (blackboxHistory[2]) + arrOffsetInHistory);
1017
1018 for (int i = 0; i < count; i++) {
1019 // Predictor is the average of the previous two history states
1020 int32_t predictor = (prev1[i] + prev2[i]) / 2;
1021
1022 blackboxWriteSignedVB(curr[i] - predictor);
1023 }
1024 }
1025
1026 static void blackboxWriteArrayUsingAveragePredictor32(int arrOffsetInHistory, int count)
1027 {
1028 int32_t *curr = (int32_t*) ((char*) (blackboxHistory[0]) + arrOffsetInHistory);
1029 int32_t *prev1 = (int32_t*) ((char*) (blackboxHistory[1]) + arrOffsetInHistory);
1030 int32_t *prev2 = (int32_t*) ((char*) (blackboxHistory[2]) + arrOffsetInHistory);
1031
1032 for (int i = 0; i < count; i++) {
1033 // Predictor is the average of the previous two history states
1034 int32_t predictor = ((int64_t)prev1[i] + (int64_t)prev2[i]) / 2;
1035
1036 blackboxWriteSignedVB(curr[i] - predictor);
1037 }
1038 }
1039
1040 static void writeInterframe(void)
1041 {
1042 blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
1043 blackboxMainState_t *blackboxLast = blackboxHistory[1];
1044
1045 blackboxWrite('P');
1046
1047 //No need to store iteration count since its delta is always 1
1048
1049 /*
1050 * Since the difference between the difference between successive times will be nearly zero (due to consistent
1051 * looptime spacing), use second-order differences.
1052 */
1053 blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
1054
1055 int32_t deltas[8];
1056 arraySubInt32(deltas, blackboxCurrent->axisPID_Setpoint, blackboxLast->axisPID_Setpoint, XYZ_AXIS_COUNT);
1057 blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
1058
1059 arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
1060 blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
1061
1062 /*
1063 * The PID I field changes very slowly, most of the time +-2, so use an encoding
1064 * that can pack all three fields into one byte in that situation.
1065 */
1066 arraySubInt32(deltas, blackboxCurrent->axisPID_I, blackboxLast->axisPID_I, XYZ_AXIS_COUNT);
1067 blackboxWriteTag2_3S32(deltas);
1068
1069 /*
1070 * The PID D term is frequently set to zero for yaw, which makes the result from the calculation
1071 * always zero. So don't bother recording D results when PID D terms are zero.
1072 */
1073 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
1074 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) {
1075 blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]);
1076 }
1077 }
1078
1079 arraySubInt32(deltas, blackboxCurrent->axisPID_F, blackboxLast->axisPID_F, XYZ_AXIS_COUNT);
1080 blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
1081
1082 if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) {
1083
1084 arraySubInt32(deltas, blackboxCurrent->fwAltPID, blackboxLast->fwAltPID, 3);
1085 blackboxWriteSignedVBArray(deltas, 3);
1086
1087 blackboxWriteSignedVB(blackboxCurrent->fwAltPIDOutput - blackboxLast->fwAltPIDOutput);
1088
1089 arraySubInt32(deltas, blackboxCurrent->fwPosPID, blackboxLast->fwPosPID, 3);
1090 blackboxWriteSignedVBArray(deltas, 3);
1091
1092 blackboxWriteSignedVB(blackboxCurrent->fwPosPIDOutput - blackboxLast->fwPosPIDOutput);
1093
1094 }
1095
1096 if (testBlackboxCondition(CONDITION(MC_NAV))) {
1097 arraySubInt32(deltas, blackboxCurrent->mcPosAxisP, blackboxLast->mcPosAxisP, XYZ_AXIS_COUNT);
1098 blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
1099
1100 for (int i = 0; i < 4; i++) {
1101 arraySubInt32(deltas, blackboxCurrent->mcVelAxisPID[i], blackboxLast->mcVelAxisPID[i], XYZ_AXIS_COUNT);
1102 blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
1103 }
1104
1105 arraySubInt32(deltas, blackboxCurrent->mcVelAxisOutput, blackboxLast->mcVelAxisOutput, XYZ_AXIS_COUNT);
1106 blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
1107
1108 arraySubInt32(deltas, blackboxCurrent->mcSurfacePID, blackboxLast->mcSurfacePID, 3);
1109 blackboxWriteSignedVBArray(deltas, 3);
1110
1111 blackboxWriteSignedVB(blackboxCurrent->mcSurfacePIDOutput - blackboxLast->mcSurfacePIDOutput);
1112 }
1113
1114 /*
1115 * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
1116 * can pack multiple values per byte:
1117 */
1118
1119 // rcData
1120 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_DATA)) {
1121 for (int x = 0; x < 4; x++) {
1122 deltas[x] = blackboxCurrent->rcData[x] - blackboxLast->rcData[x];
1123 }
1124
1125 blackboxWriteTag8_4S16(deltas);
1126 }
1127
1128 // rcCommand
1129 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RC_COMMAND)) {
1130 for (int x = 0; x < 4; x++) {
1131 deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x];
1132 }
1133
1134 blackboxWriteTag8_4S16(deltas);
1135 }
1136
1137 //Check for sensors that are updated periodically (so deltas are normally zero)
1138 int optionalFieldCount = 0;
1139
1140 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
1141 deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->vbat - blackboxLast->vbat;
1142 }
1143
1144 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE)) {
1145 deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->amperage - blackboxLast->amperage;
1146 }
1147
1148 #ifdef USE_MAG
1149 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
1150 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
1151 deltas[optionalFieldCount++] = blackboxCurrent->magADC[x] - blackboxLast->magADC[x];
1152 }
1153 }
1154 #endif
1155
1156 #ifdef USE_BARO
1157 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) {
1158 deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt;
1159 }
1160 #endif
1161
1162 #ifdef USE_PITOT
1163 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_PITOT)) {
1164 deltas[optionalFieldCount++] = blackboxCurrent->airSpeed - blackboxLast->airSpeed;
1165 }
1166 #endif
1167
1168 #ifdef USE_RANGEFINDER
1169 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SURFACE)) {
1170 deltas[optionalFieldCount++] = blackboxCurrent->surfaceRaw - blackboxLast->surfaceRaw;
1171 }
1172 #endif
1173
1174 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RSSI)) {
1175 deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->rssi - blackboxLast->rssi;
1176 }
1177
1178 blackboxWriteTag8_8SVB(deltas, optionalFieldCount);
1179
1180 //Since gyros, accs and motors are noisy, base their predictions on the average of the history:
1181 blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroADC), XYZ_AXIS_COUNT);
1182
1183 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_RAW)) {
1184 blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroRaw), XYZ_AXIS_COUNT);
1185 }
1186
1187 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_ROLL)) {
1188 blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroPeaksRoll), DYN_NOTCH_PEAK_COUNT);
1189 }
1190
1191 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_PITCH)) {
1192 blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroPeaksPitch), DYN_NOTCH_PEAK_COUNT);
1193 }
1194
1195 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_GYRO_PEAKS_YAW)) {
1196 blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, gyroPeaksYaw), DYN_NOTCH_PEAK_COUNT);
1197 }
1198
1199 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
1200 blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
1201 blackboxWriteSignedVB(blackboxCurrent->accVib - blackboxLast->accVib);
1202 }
1203
1204 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
1205 blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
1206 }
1207
1208 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_DEBUG)) {
1209 blackboxWriteArrayUsingAveragePredictor32(offsetof(blackboxMainState_t, debug), DEBUG32_VALUE_COUNT);
1210 }
1211
1212 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MOTORS)) {
1213 blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, motor), getMotorCount());
1214 }
1215
1216 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SERVOS)) {
1217 blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, servo), MAX_SUPPORTED_SERVOS);
1218 }
1219
1220 blackboxWriteSignedVB(blackboxCurrent->navState - blackboxLast->navState);
1221
1222 blackboxWriteSignedVB(blackboxCurrent->navFlags - blackboxLast->navFlags);
1223
1224 /*
1225 * NAV_POS fields
1226 */
1227 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_POS)) {
1228 blackboxWriteSignedVB(blackboxCurrent->navEPH - blackboxLast->navEPH);
1229 blackboxWriteSignedVB(blackboxCurrent->navEPV - blackboxLast->navEPV);
1230
1231 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
1232 blackboxWriteSignedVB(blackboxCurrent->navPos[x] - blackboxLast->navPos[x]);
1233 }
1234
1235 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
1236 blackboxWriteSignedVB(blackboxHistory[0]->navRealVel[x] - (blackboxHistory[1]->navRealVel[x] + blackboxHistory[2]->navRealVel[x]) / 2);
1237 }
1238
1239
1240 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
1241 blackboxWriteSignedVB(blackboxHistory[0]->navTargetVel[x] - (blackboxHistory[1]->navTargetVel[x] + blackboxHistory[2]->navTargetVel[x]) / 2);
1242 }
1243
1244 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
1245 blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - blackboxLast->navTargetPos[x]);
1246 }
1247
1248 blackboxWriteSignedVB(blackboxCurrent->navTargetHeading - blackboxLast->navTargetHeading);
1249 blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
1250 }
1251
1252 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NAV_ACC)) {
1253 for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
1254 blackboxWriteSignedVB(blackboxHistory[0]->navAccNEU[x] - (blackboxHistory[1]->navAccNEU[x] + blackboxHistory[2]->navAccNEU[x]) / 2);
1255 }
1256 }
1257
1258 //Rotate our history buffers
1259 blackboxHistory[2] = blackboxHistory[1];
1260 blackboxHistory[1] = blackboxHistory[0];
1261 blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
1262
1263 blackboxLoggedAnyFrames = true;
1264 }
1265
1266 /* Write the contents of the global "slowHistory" to the log as an "S" frame. Because this data is logged so
1267 * infrequently, delta updates are not reasonable, so we log independent frames. */
1268 static void writeSlowFrame(void)
1269 {
1270 int32_t values[3];
1271
1272 blackboxWrite('S');
1273
1274 blackboxWriteUnsignedVB(slowHistory.activeWpNumber);
1275 blackboxWriteUnsignedVB(slowHistory.rcModeFlags);
1276 blackboxWriteUnsignedVB(slowHistory.rcModeFlags2);
1277 blackboxWriteUnsignedVB(slowHistory.activeFlightModeFlags);
1278 blackboxWriteUnsignedVB(slowHistory.stateFlags);
1279
1280 /*
1281 * Most of the time these three values will be able to pack into one byte for us:
1282 */
1283 values[0] = slowHistory.failsafePhase;
1284 values[1] = slowHistory.rxSignalReceived ? 1 : 0;
1285 values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0;
1286 blackboxWriteTag2_3S32(values);
1287
1288 blackboxWriteUnsignedVB(slowHistory.rxUpdateRate);
1289
1290 blackboxWriteUnsignedVB(slowHistory.hwHealthStatus);
1291
1292 blackboxWriteUnsignedVB(slowHistory.powerSupplyImpedance);
1293 blackboxWriteUnsignedVB(slowHistory.sagCompensatedVBat);
1294
1295 blackboxWriteSigned16VBArray(slowHistory.wind, XYZ_AXIS_COUNT);
1296
1297 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
1298 blackboxWriteUnsignedVB(slowHistory.mspOverrideFlags);
1299 #endif
1300
1301 blackboxWriteSignedVB(slowHistory.imuTemperature);
1302
1303 #ifdef USE_BARO
1304 blackboxWriteSignedVB(slowHistory.baroTemperature);
1305 #endif
1306
1307 #ifdef USE_TEMPERATURE_SENSOR
1308 blackboxWriteSigned16VBArray(slowHistory.tempSensorTemperature, MAX_TEMP_SENSORS);
1309 #endif
1310
1311 #ifdef USE_ESC_SENSOR
1312 blackboxWriteUnsignedVB(slowHistory.escRPM);
1313 blackboxWriteSignedVB(slowHistory.escTemperature);
1314 #endif
1315
1316 blackboxSlowFrameIterationTimer = 0;
1317 }
1318
1319 /**
1320 * Load rarely-changing values from the FC into the given structure
1321 */
1322 static void loadSlowState(blackboxSlowState_t *slow)
1323 {
1324 slow->activeWpNumber = getActiveWpNumber();
1325
1326 slow->rcModeFlags = rcModeActivationMask.bits[0]; // first 32 bits of boxId_e
1327 slow->rcModeFlags2 = rcModeActivationMask.bits[1]; // remaining bits of boxId_e
1328
1329 // Also log Nav auto enabled flight modes rather than just those selected by boxmode
1330 if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) {
1331 slow->rcModeFlags |= (1 << BOXHEADINGHOLD);
1332 }
1333 slow->activeFlightModeFlags = flightModeFlags;
1334 slow->stateFlags = stateFlags;
1335 slow->failsafePhase = failsafePhase();
1336 slow->rxSignalReceived = rxIsReceivingSignal();
1337 slow->rxFlightChannelsValid = rxAreFlightChannelsValid();
1338 slow->rxUpdateRate = getRcUpdateFrequency();
1339 slow->hwHealthStatus = (getHwGyroStatus() << 2 * 0) | // Pack hardware health status into a bit field.
1340 (getHwAccelerometerStatus() << 2 * 1) | // Use raw hardwareSensorStatus_e values and pack them using 2 bits per value
1341 (getHwCompassStatus() << 2 * 2) | // Report GYRO in 2 lowest bits, then ACC, COMPASS, BARO, GPS, RANGEFINDER and PITOT
1342 (getHwBarometerStatus() << 2 * 3) |
1343 (getHwGPSStatus() << 2 * 4) |
1344 (getHwRangefinderStatus() << 2 * 5) |
1345 (getHwPitotmeterStatus() << 2 * 6);
1346 slow->powerSupplyImpedance = getPowerSupplyImpedance();
1347 slow->sagCompensatedVBat = getBatterySagCompensatedVoltage();
1348
1349 for (int i = 0; i < XYZ_AXIS_COUNT; i++)
1350 {
1351 #ifdef USE_WIND_ESTIMATOR
1352 slow->wind[i] = getEstimatedWindSpeed(i);
1353 #else
1354 slow->wind[i] = 0;
1355 #endif
1356 }
1357
1358 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
1359 slow->mspOverrideFlags = (IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE) ? 2 : 0) + (mspOverrideIsInFailsafe() ? 1 : 0);
1360 #endif
1361
1362 bool valid_temp;
1363 int16_t newTemp = 0;
1364 valid_temp = getIMUTemperature(&newTemp);
1365 if (valid_temp)
1366 slow->imuTemperature = newTemp;
1367 else
1368 slow->imuTemperature = TEMPERATURE_INVALID_VALUE;
1369
1370 #ifdef USE_BARO
1371 valid_temp = getBaroTemperature(&newTemp);
1372 if (valid_temp)
1373 slow->baroTemperature = newTemp;
1374 else
1375 slow->baroTemperature = TEMPERATURE_INVALID_VALUE;
1376 #endif
1377
1378 #ifdef USE_TEMPERATURE_SENSOR
1379 for (uint8_t index = 0; index < MAX_TEMP_SENSORS; ++index) {
1380 valid_temp = getSensorTemperature(index, slow->tempSensorTemperature + index);
1381 if (!valid_temp) slow->tempSensorTemperature[index] = TEMPERATURE_INVALID_VALUE;
1382 }
1383 #endif
1384
1385 #ifdef USE_ESC_SENSOR
1386 escSensorData_t * escSensor = escSensorGetData();
1387 slow->escRPM = escSensor->rpm;
1388 slow->escTemperature = escSensor->temperature;
1389 #endif
1390 }
1391
1392 /**
1393 * If the data in the slow frame has changed, log a slow frame.
1394 *
1395 * If allowPeriodicWrite is true, the frame is also logged if it has been more than blackboxSInterval logging iterations
1396 * since the field was last logged.
1397 */
1398 static bool writeSlowFrameIfNeeded(bool allowPeriodicWrite)
1399 {
1400 // Write the slow frame peridocially so it can be recovered if we ever lose sync
1401 bool shouldWrite = allowPeriodicWrite && blackboxSlowFrameIterationTimer >= blackboxSInterval;
1402
1403 if (shouldWrite) {
1404 loadSlowState(&slowHistory);
1405 } else {
1406 blackboxSlowState_t newSlowState;
1407
1408 loadSlowState(&newSlowState);
1409
1410 // Only write a slow frame if it was different from the previous state
1411 if (memcmp(&newSlowState, &slowHistory, sizeof(slowHistory)) != 0) {
1412 // Use the new state as our new history
1413 memcpy(&slowHistory, &newSlowState, sizeof(slowHistory));
1414 shouldWrite = true;
1415 }
1416 }
1417
1418 if (shouldWrite) {
1419 writeSlowFrame();
1420 }
1421 return shouldWrite;
1422 }
1423
1424 static void blackboxValidateConfig(void)
1425 {
1426 if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0
1427 || blackboxConfig()->rate_num >= blackboxConfig()->rate_denom) {
1428 blackboxConfigMutable()->rate_num = 1;
1429 blackboxConfigMutable()->rate_denom = 1;
1430 } else {
1431 /* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
1432 * itself more frequently)
1433 */
1434 const int div = gcd(blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
1435
1436 blackboxConfigMutable()->rate_num /= div;
1437 blackboxConfigMutable()->rate_denom /= div;
1438 }
1439
1440 // If we've chosen an unsupported device, change the device to serial
1441 switch (blackboxConfig()->device) {
1442 #ifdef USE_FLASHFS
1443 case BLACKBOX_DEVICE_FLASH:
1444 #endif
1445 #ifdef USE_SDCARD
1446 case BLACKBOX_DEVICE_SDCARD:
1447 #endif
1448 #if defined(SITL_BUILD)
1449 case BLACKBOX_DEVICE_FILE:
1450 #endif
1451 case BLACKBOX_DEVICE_SERIAL:
1452 // Device supported, leave the setting alone
1453 break;
1454
1455 default:
1456 blackboxConfigMutable()->device = BLACKBOX_DEVICE_SERIAL;
1457 }
1458 }
1459
1460 static void blackboxResetIterationTimers(void)
1461 {
1462 blackboxIteration = 0;
1463 blackboxPFrameIndex = 0;
1464 blackboxIFrameIndex = 0;
1465 }
1466
1467 /**
1468 * Start Blackbox logging if it is not already running. Intended to be called upon arming.
1469 */
1470 void blackboxStart(void)
1471 {
1472 if (blackboxState != BLACKBOX_STATE_STOPPED) {
1473 return;
1474 }
1475
1476 blackboxValidateConfig();
1477
1478 if (!blackboxDeviceOpen()) {
1479 blackboxSetState(BLACKBOX_STATE_DISABLED);
1480 return;
1481 }
1482
1483 memset(&gpsHistory, 0, sizeof(gpsHistory));
1484
1485 blackboxHistory[0] = &blackboxHistoryRing[0];
1486 blackboxHistory[1] = &blackboxHistoryRing[1];
1487 blackboxHistory[2] = &blackboxHistoryRing[2];
1488
1489 vbatReference = getBatteryRawVoltage();
1490
1491 //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
1492
1493 /*
1494 * We use conditional tests to decide whether or not certain fields should be logged. Since our headers
1495 * must always agree with the logged data, the results of these tests must not change during logging. So
1496 * cache those now.
1497 */
1498 blackboxBuildConditionCache();
1499
1500 blackboxModeActivationConditionPresent = isModeActivationConditionPresent(BOXBLACKBOX);
1501
1502 blackboxResetIterationTimers();
1503
1504 /*
1505 * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
1506 * it finally plays the beep for this arming event.
1507 */
1508 blackboxLastArmingBeep = getArmingBeepTimeMicros();
1509 memcpy(&blackboxLastRcModeFlags, &rcModeActivationMask, sizeof(blackboxLastRcModeFlags)); // record startup status
1510
1511 blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
1512 }
1513
1514 /**
1515 * Begin Blackbox shutdown.
1516 */
1517 void blackboxFinish(void)
1518 {
1519 switch (blackboxState) {
1520 case BLACKBOX_STATE_DISABLED:
1521 case BLACKBOX_STATE_STOPPED:
1522 case BLACKBOX_STATE_SHUTTING_DOWN:
1523 // We're already stopped/shutting down
1524 break;
1525
1526 case BLACKBOX_STATE_RUNNING:
1527 case BLACKBOX_STATE_PAUSED:
1528 blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END, NULL);
1529 FALLTHROUGH;
1530
1531 default:
1532 blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN);
1533 }
1534 }
1535
1536 #ifdef USE_GPS
1537 static void writeGPSHomeFrame(void)
1538 {
1539 blackboxWrite('H');
1540
1541 blackboxWriteSignedVB(GPS_home.lat);
1542 blackboxWriteSignedVB(GPS_home.lon);
1543 //TODO it'd be great if we could grab the GPS current time and write that too
1544
1545 gpsHistory.GPS_home[0] = GPS_home.lat;
1546 gpsHistory.GPS_home[1] = GPS_home.lon;
1547 }
1548
1549 static void writeGPSFrame(timeUs_t currentTimeUs)
1550 {
1551 blackboxWrite('G');
1552
1553 /*
1554 * If we're logging every frame, then a GPS frame always appears just after a frame with the
1555 * currentTime timestamp in the log, so the reader can just use that timestamp for the GPS frame.
1556 *
1557 * If we're not logging every frame, we need to store the time of this GPS frame.
1558 */
1559 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) {
1560 // Predict the time of the last frame in the main log
1561 blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time);
1562 }
1563
1564 blackboxWriteUnsignedVB(gpsSol.fixType);
1565 blackboxWriteUnsignedVB(gpsSol.numSat);
1566 blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[0]);
1567 blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[1]);
1568 blackboxWriteSignedVB(gpsSol.llh.alt / 100); // meters
1569 blackboxWriteUnsignedVB(gpsSol.groundSpeed);
1570 blackboxWriteUnsignedVB(gpsSol.groundCourse);
1571 blackboxWriteUnsignedVB(gpsSol.hdop);
1572 blackboxWriteUnsignedVB(gpsSol.eph);
1573 blackboxWriteUnsignedVB(gpsSol.epv);
1574 blackboxWriteSigned16VBArray(gpsSol.velNED, XYZ_AXIS_COUNT);
1575
1576 gpsHistory.GPS_numSat = gpsSol.numSat;
1577 gpsHistory.GPS_coord[0] = gpsSol.llh.lat;
1578 gpsHistory.GPS_coord[1] = gpsSol.llh.lon;
1579 }
1580 #endif
1581
1582 /**
1583 * Fill the current state of the blackbox using values read from the flight controller
1584 */
1585 static void loadMainState(timeUs_t currentTimeUs)
1586 {
1587 blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
1588
1589 blackboxCurrent->time = currentTimeUs;
1590
1591 const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
1592
1593 for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
1594 blackboxCurrent->axisPID_Setpoint[i] = axisPID_Setpoint[i];
1595 blackboxCurrent->axisPID_P[i] = axisPID_P[i];
1596 blackboxCurrent->axisPID_I[i] = axisPID_I[i];
1597 blackboxCurrent->axisPID_D[i] = axisPID_D[i];
1598 blackboxCurrent->axisPID_F[i] = axisPID_F[i];
1599 blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
1600 blackboxCurrent->accADC[i] = lrintf(acc.accADCf[i] * acc.dev.acc_1G);
1601 blackboxCurrent->gyroRaw[i] = lrintf(gyro.gyroRaw[i]);
1602
1603 #ifdef USE_DYNAMIC_FILTERS
1604 for (uint8_t i = 0; i < DYN_NOTCH_PEAK_COUNT ; i++) {
1605 blackboxCurrent->gyroPeaksRoll[i] = dynamicGyroNotchState.frequency[FD_ROLL][i];
1606 blackboxCurrent->gyroPeaksPitch[i] = dynamicGyroNotchState.frequency[FD_PITCH][i];
1607 blackboxCurrent->gyroPeaksYaw[i] = dynamicGyroNotchState.frequency[FD_YAW][i];
1608 }
1609 #endif
1610
1611 #ifdef USE_MAG
1612 blackboxCurrent->magADC[i] = mag.magADC[i];
1613 #endif
1614 if (!STATE(FIXED_WING_LEGACY)) {
1615 // log requested velocity in cm/s
1616 blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained);
1617
1618 // log requested acceleration in cm/s^2 and throttle adjustment in µs
1619 blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional);
1620 blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral);
1621 blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative);
1622 blackboxCurrent->mcVelAxisPID[3][i] = lrintf(nav_pids->vel[i].feedForward);
1623 blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
1624 }
1625 }
1626 blackboxCurrent->accVib = lrintf(accGetVibrationLevel() * acc.dev.acc_1G);
1627
1628 if (STATE(FIXED_WING_LEGACY)) {
1629
1630 // log requested pitch in decidegrees
1631 blackboxCurrent->fwAltPID[0] = lrintf(nav_pids->fw_alt.proportional);
1632 blackboxCurrent->fwAltPID[1] = lrintf(nav_pids->fw_alt.integral);
1633 blackboxCurrent->fwAltPID[2] = lrintf(nav_pids->fw_alt.derivative);
1634 blackboxCurrent->fwAltPIDOutput = lrintf(nav_pids->fw_alt.output_constrained);
1635
1636 // log requested roll in decidegrees
1637 blackboxCurrent->fwPosPID[0] = lrintf(nav_pids->fw_nav.proportional / 10);
1638 blackboxCurrent->fwPosPID[1] = lrintf(nav_pids->fw_nav.integral / 10);
1639 blackboxCurrent->fwPosPID[2] = lrintf(nav_pids->fw_nav.derivative / 10);
1640 blackboxCurrent->fwPosPIDOutput = lrintf(nav_pids->fw_nav.output_constrained / 10);
1641
1642 } else {
1643 blackboxCurrent->mcSurfacePID[0] = lrintf(nav_pids->surface.proportional / 10);
1644 blackboxCurrent->mcSurfacePID[1] = lrintf(nav_pids->surface.integral / 10);
1645 blackboxCurrent->mcSurfacePID[2] = lrintf(nav_pids->surface.derivative / 10);
1646 blackboxCurrent->mcSurfacePIDOutput = lrintf(nav_pids->surface.output_constrained / 10);
1647 }
1648
1649 for (int i = 0; i < 4; i++) {
1650 blackboxCurrent->rcData[i] = rxGetChannelValue(i);
1651 blackboxCurrent->rcCommand[i] = rcCommand[i];
1652 }
1653
1654 blackboxCurrent->attitude[0] = attitude.values.roll;
1655 blackboxCurrent->attitude[1] = attitude.values.pitch;
1656 blackboxCurrent->attitude[2] = attitude.values.yaw;
1657
1658 for (int i = 0; i < DEBUG32_VALUE_COUNT; i++) {
1659 blackboxCurrent->debug[i] = debug[i];
1660 }
1661
1662 const int motorCount = getMotorCount();
1663 for (int i = 0; i < motorCount; i++) {
1664 blackboxCurrent->motor[i] = motor[i];
1665 }
1666
1667 blackboxCurrent->vbat = getBatteryRawVoltage();
1668 blackboxCurrent->amperage = getAmperage();
1669
1670 #ifdef USE_BARO
1671 blackboxCurrent->BaroAlt = baro.BaroAlt;
1672 #endif
1673
1674 #ifdef USE_PITOT
1675 blackboxCurrent->airSpeed = getAirspeedEstimate();
1676 #endif
1677
1678 #ifdef USE_RANGEFINDER
1679 // Store the raw rangefinder surface readout without applying tilt correction
1680 blackboxCurrent->surfaceRaw = rangefinderGetLatestRawAltitude();
1681 #endif
1682
1683 blackboxCurrent->rssi = getRSSI();
1684
1685 for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
1686 blackboxCurrent->servo[i] = servo[i];
1687 }
1688
1689 blackboxCurrent->navState = navCurrentState;
1690 blackboxCurrent->navFlags = navFlags;
1691 blackboxCurrent->navEPH = navEPH;
1692 blackboxCurrent->navEPV = navEPV;
1693 for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
1694 blackboxCurrent->navPos[i] = navLatestActualPosition[i];
1695 blackboxCurrent->navRealVel[i] = navActualVelocity[i];
1696 blackboxCurrent->navAccNEU[i] = navAccNEU[i];
1697 blackboxCurrent->navTargetVel[i] = navDesiredVelocity[i];
1698 blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
1699 }
1700 blackboxCurrent->navTargetHeading = navDesiredHeading;
1701 blackboxCurrent->navSurface = navActualSurface;
1702 }
1703
1704 /**
1705 * Transmit the header information for the given field definitions. Transmitted header lines look like:
1706 *
1707 * H Field I name:a,b,c
1708 * H Field I predictor:0,1,2
1709 *
1710 * For all header types, provide a "mainFrameChar" which is the name for the field and will be used to refer to it in the
1711 * header (e.g. P, I etc). For blackboxDeltaField_t fields, also provide deltaFrameChar, otherwise set this to zero.
1712 *
1713 * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
1714 * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
1715 *
1716 * Set xmitState.headerIndex to 0 and xmitState.u.fieldIndex to -1 before calling for the first time.
1717 *
1718 * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the
1719 * fieldDefinition and secondCondition arrays.
1720 *
1721 * Returns true if there is still header left to transmit (so call again to continue transmission).
1722 */
1723 static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const void *fieldDefinitions,
1724 const void *secondFieldDefinition, int fieldCount, const uint8_t *conditions, const uint8_t *secondCondition)
1725 {
1726 const blackboxFieldDefinition_t *def;
1727 unsigned int headerCount;
1728 static bool needComma = false;
1729 size_t definitionStride = (char*) secondFieldDefinition - (char*) fieldDefinitions;
1730 size_t conditionsStride = (char*) secondCondition - (char*) conditions;
1731
1732 if (deltaFrameChar) {
1733 headerCount = BLACKBOX_DELTA_FIELD_HEADER_COUNT;
1734 } else {
1735 headerCount = BLACKBOX_SIMPLE_FIELD_HEADER_COUNT;
1736 }
1737
1738 /*
1739 * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit
1740 * the whole header.
1741 */
1742
1743 // On our first call we need to print the name of the header and a colon
1744 if (xmitState.u.fieldIndex == -1) {
1745 if (xmitState.headerIndex >= headerCount) {
1746 return false; //Someone probably called us again after we had already completed transmission
1747 }
1748
1749 uint32_t charsToBeWritten = strlen("H Field x :") + strlen(blackboxFieldHeaderNames[xmitState.headerIndex]);
1750
1751 if (blackboxDeviceReserveBufferSpace(charsToBeWritten) != BLACKBOX_RESERVE_SUCCESS) {
1752 return true; // Try again later
1753 }
1754
1755 blackboxHeaderBudget -= blackboxPrintf("H Field %c %s:", xmitState.headerIndex >= BLACKBOX_SIMPLE_FIELD_HEADER_COUNT ? deltaFrameChar : mainFrameChar, blackboxFieldHeaderNames[xmitState.headerIndex]);
1756
1757 xmitState.u.fieldIndex++;
1758 needComma = false;
1759 }
1760
1761 // The longest we expect an integer to be as a string:
1762 const uint32_t LONGEST_INTEGER_STRLEN = 2;
1763
1764 for (; xmitState.u.fieldIndex < fieldCount; xmitState.u.fieldIndex++) {
1765 def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * xmitState.u.fieldIndex);
1766
1767 if (!conditions || testBlackboxCondition(conditions[conditionsStride * xmitState.u.fieldIndex])) {
1768 // First (over)estimate the length of the string we want to print
1769
1770 int32_t bytesToWrite = 1; // Leading comma
1771
1772 // The first header is a field name
1773 if (xmitState.headerIndex == 0) {
1774 bytesToWrite += strlen(def->name) + strlen("[]") + LONGEST_INTEGER_STRLEN;
1775 } else {
1776 //The other headers are integers
1777 bytesToWrite += LONGEST_INTEGER_STRLEN;
1778 }
1779
1780 // Now perform the write if the buffer is large enough
1781 if (blackboxDeviceReserveBufferSpace(bytesToWrite) != BLACKBOX_RESERVE_SUCCESS) {
1782 // Ran out of space!
1783 return true;
1784 }
1785
1786 blackboxHeaderBudget -= bytesToWrite;
1787
1788 if (needComma) {
1789 blackboxWrite(',');
1790 } else {
1791 needComma = true;
1792 }
1793
1794 // The first header is a field name
1795 if (xmitState.headerIndex == 0) {
1796 blackboxPrint(def->name);
1797
1798 // Do we need to print an index in brackets after the name?
1799 if (def->fieldNameIndex != -1) {
1800 blackboxPrintf("[%d]", def->fieldNameIndex);
1801 }
1802 } else {
1803 //The other headers are integers
1804 blackboxPrintf("%d", def->arr[xmitState.headerIndex - 1]);
1805 }
1806 }
1807 }
1808
1809 // Did we complete this line?
1810 if (xmitState.u.fieldIndex == fieldCount && blackboxDeviceReserveBufferSpace(1) == BLACKBOX_RESERVE_SUCCESS) {
1811 blackboxHeaderBudget--;
1812 blackboxWrite('\n');
1813 xmitState.headerIndex++;
1814 xmitState.u.fieldIndex = -1;
1815 }
1816
1817 return xmitState.headerIndex < headerCount;
1818 }
1819
1820 // Buf must be at least FORMATTED_DATE_TIME_BUFSIZE
1821 static char *blackboxGetStartDateTime(char *buf)
1822 {
1823 dateTime_t dt;
1824 // rtcGetDateTime will fill dt with 0000-01-01T00:00:00
1825 // when time is not known.
1826 rtcGetDateTime(&dt);
1827 dateTimeFormatLocal(buf, &dt);
1828 return buf;
1829 }
1830
1831 #ifndef BLACKBOX_PRINT_HEADER_LINE
1832 #define BLACKBOX_PRINT_HEADER_LINE(name, format, ...) case __COUNTER__: \
1833 blackboxPrintfHeaderLine(name, format, __VA_ARGS__); \
1834 break;
1835 #define BLACKBOX_PRINT_HEADER_LINE_CUSTOM(...) case __COUNTER__: \
1836 {__VA_ARGS__}; \
1837 break;
1838 #endif
1839
1840 /**
1841 * Transmit a portion of the system information headers. Call the first time with xmitState.headerIndex == 0. Returns
1842 * true iff transmission is complete, otherwise call again later to continue transmission.
1843 */
1844 static bool blackboxWriteSysinfo(void)
1845 {
1846 // Make sure we have enough room in the buffer for our longest line (as of this writing, the "Firmware date" line)
1847 if (blackboxDeviceReserveBufferSpace(64) != BLACKBOX_RESERVE_SUCCESS) {
1848 return false;
1849 }
1850
1851 char buf[FORMATTED_DATE_TIME_BUFSIZE];
1852
1853 switch (xmitState.headerIndex) {
1854 BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight");
1855 BLACKBOX_PRINT_HEADER_LINE("Firmware revision", "INAV %s (%s) %s", FC_VERSION_STRING, shortGitRevision, targetName);
1856 BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s", buildDate, buildTime);
1857 BLACKBOX_PRINT_HEADER_LINE("Log start datetime", "%s", blackboxGetStartDateTime(buf));
1858 BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->craftName);
1859 BLACKBOX_PRINT_HEADER_LINE("P interval", "%u/%u", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
1860 BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", getThrottleIdleValue());
1861 BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", getMaxThrottle());
1862 BLACKBOX_PRINT_HEADER_LINE("gyro_scale", "0x%x", castFloatBytesToInt(1.0f));
1863 BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", getThrottleIdleValue(),getMaxThrottle());
1864 BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
1865
1866 #ifdef USE_ADC
1867 BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
1868 if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
1869 blackboxPrintfHeaderLine("vbat_scale", "%u", batteryMetersConfig()->voltage.scale / 10);
1870 } else {
1871 xmitState.headerIndex += 2; // Skip the next two vbat fields too
1872 }
1873 );
1874 BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", currentBatteryProfile->voltage.cellMin / 10,
1875 currentBatteryProfile->voltage.cellWarning / 10,
1876 currentBatteryProfile->voltage.cellMax / 10);
1877 BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference);
1878 #endif
1879
1880 BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
1881 //Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too:
1882 if (feature(FEATURE_CURRENT_METER)) {
1883 blackboxPrintfHeaderLine("currentMeter", "%d,%d", batteryMetersConfig()->current.offset,
1884 batteryMetersConfig()->current.scale);
1885 }
1886 );
1887
1888 BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", getLooptime());
1889 BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", 100); //For compatibility reasons write rc_rate 100
1890 BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->stabilized.rcExpo8);
1891 BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->stabilized.rcYawExpo8);
1892 BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->throttle.rcMid8);
1893 BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->throttle.rcExpo8);
1894 BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->throttle.dynPID);
1895 BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->throttle.pa_breakpoint);
1896 BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->stabilized.rates[ROLL],
1897 currentControlRateProfile->stabilized.rates[PITCH],
1898 currentControlRateProfile->stabilized.rates[YAW]);
1899 BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d,%d", pidBank()->pid[PID_ROLL].P,
1900 pidBank()->pid[PID_ROLL].I,
1901 pidBank()->pid[PID_ROLL].D,
1902 pidBank()->pid[PID_ROLL].FF);
1903 BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d,%d", pidBank()->pid[PID_PITCH].P,
1904 pidBank()->pid[PID_PITCH].I,
1905 pidBank()->pid[PID_PITCH].D,
1906 pidBank()->pid[PID_PITCH].FF);
1907 BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d,%d", pidBank()->pid[PID_YAW].P,
1908 pidBank()->pid[PID_YAW].I,
1909 pidBank()->pid[PID_YAW].D,
1910 pidBank()->pid[PID_YAW].FF);
1911 BLACKBOX_PRINT_HEADER_LINE("altPID", "%d,%d,%d", pidBank()->pid[PID_POS_Z].P,
1912 pidBank()->pid[PID_POS_Z].I,
1913 pidBank()->pid[PID_POS_Z].D);
1914 BLACKBOX_PRINT_HEADER_LINE("posPID", "%d,%d,%d", pidBank()->pid[PID_POS_XY].P,
1915 pidBank()->pid[PID_POS_XY].I,
1916 pidBank()->pid[PID_POS_XY].D);
1917 BLACKBOX_PRINT_HEADER_LINE("posrPID", "%d,%d,%d", pidBank()->pid[PID_VEL_XY].P,
1918 pidBank()->pid[PID_VEL_XY].I,
1919 pidBank()->pid[PID_VEL_XY].D);
1920 BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d", pidBank()->pid[PID_LEVEL].P,
1921 pidBank()->pid[PID_LEVEL].I,
1922 pidBank()->pid[PID_LEVEL].D);
1923 BLACKBOX_PRINT_HEADER_LINE("magPID", "%d", pidBank()->pid[PID_HEADING].P);
1924 BLACKBOX_PRINT_HEADER_LINE("velPID", "%d,%d,%d", pidBank()->pid[PID_VEL_Z].P,
1925 pidBank()->pid[PID_VEL_Z].I,
1926 pidBank()->pid[PID_VEL_Z].D);
1927 BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz", "%d", pidProfile()->yaw_lpf_hz);
1928 BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz", "%d", pidProfile()->dterm_lpf_hz);
1929 BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_type", "%d", pidProfile()->dterm_lpf_type);
1930 BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband);
1931 BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband);
1932 BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", GYRO_LPF_256HZ);
1933 BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_main_lpf_hz);
1934 #ifdef USE_DYNAMIC_FILTERS
1935 BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
1936 BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
1937 #endif
1938 BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz);
1939 BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware);
1940 #ifdef USE_BARO
1941 BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware);
1942 #endif
1943 #ifdef USE_MAG
1944 BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware);
1945 #else
1946 BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", MAG_NONE);
1947 #endif
1948 BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider);
1949 BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->motorPwmProtocol);
1950 BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", getEscUpdateFrequency());
1951 BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode);
1952 BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures);
1953 BLACKBOX_PRINT_HEADER_LINE("waypoints", "%d,%d", getWaypointCount(),isWaypointListValid());
1954 BLACKBOX_PRINT_HEADER_LINE("acc_notch_hz", "%d", accelerometerConfig()->acc_notch_hz);
1955 BLACKBOX_PRINT_HEADER_LINE("acc_notch_cutoff", "%d", accelerometerConfig()->acc_notch_cutoff);
1956 BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitYaw", "%d", pidProfile()->axisAccelerationLimitYaw);
1957 BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitRollPitch", "%d", pidProfile()->axisAccelerationLimitRollPitch);
1958 #ifdef USE_RPM_FILTER
1959 BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_filter_enabled", "%d", rpmFilterConfig()->gyro_filter_enabled);
1960 BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_harmonics", "%d", rpmFilterConfig()->gyro_harmonics);
1961 BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_min_hz", "%d", rpmFilterConfig()->gyro_min_hz);
1962 BLACKBOX_PRINT_HEADER_LINE("rpm_gyro_q", "%d", rpmFilterConfig()->gyro_q);
1963 #endif
1964 default:
1965 return true;
1966 }
1967
1968 xmitState.headerIndex++;
1969 return false;
1970 }
1971
1972 /**
1973 * Write the given event to the log immediately
1974 */
1975 void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
1976 {
1977 // Only allow events to be logged after headers have been written
1978 if (!(blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_PAUSED)) {
1979 return;
1980 }
1981
1982 //Shared header for event frames
1983 blackboxWrite('E');
1984 blackboxWrite(event);
1985
1986 //Now serialize the data for this specific frame type
1987 switch (event) {
1988 case FLIGHT_LOG_EVENT_SYNC_BEEP:
1989 blackboxWriteUnsignedVB(data->syncBeep.time);
1990 break;
1991 case FLIGHT_LOG_EVENT_FLIGHTMODE: // New flightmode flags write
1992 blackboxWriteUnsignedVB(data->flightMode.flags);
1993 blackboxWriteUnsignedVB(data->flightMode.lastFlags);
1994 break;
1995 case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT:
1996 if (data->inflightAdjustment.floatFlag) {
1997 blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG);
1998 blackboxWriteFloat(data->inflightAdjustment.newFloatValue);
1999 } else {
2000 blackboxWrite(data->inflightAdjustment.adjustmentFunction);
2001 blackboxWriteSignedVB(data->inflightAdjustment.newValue);
2002 }
2003 break;
2004 case FLIGHT_LOG_EVENT_LOGGING_RESUME:
2005 blackboxWriteUnsignedVB(data->loggingResume.logIteration);
2006 blackboxWriteUnsignedVB(data->loggingResume.currentTimeUs);
2007 break;
2008 case FLIGHT_LOG_EVENT_IMU_FAILURE:
2009 blackboxWriteUnsignedVB(data->imuError.errorCode);
2010 break;
2011 case FLIGHT_LOG_EVENT_LOG_END:
2012 blackboxPrintf("End of log (disarm reason:%d)", getDisarmReason());
2013 blackboxWrite(0);
2014 break;
2015 }
2016 }
2017
2018 /* 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 */
2019 static void blackboxCheckAndLogArmingBeep(void)
2020 {
2021 // Use != so that we can still detect a change if the counter wraps
2022 if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) {
2023 blackboxLastArmingBeep = getArmingBeepTimeMicros();
2024 flightLogEvent_syncBeep_t eventData;
2025 eventData.time = blackboxLastArmingBeep;
2026 blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData);
2027 }
2028 }
2029
2030 /* monitor the flight mode event status and trigger an event record if the state changes */
2031 static void blackboxCheckAndLogFlightMode(void)
2032 {
2033 // Use != so that we can still detect a change if the counter wraps
2034 if (memcmp(&rcModeActivationMask, &blackboxLastRcModeFlags, sizeof(blackboxLastRcModeFlags))) {
2035 flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
2036 eventData.lastFlags = blackboxLastRcModeFlags;
2037 memcpy(&blackboxLastRcModeFlags, &rcModeActivationMask, sizeof(blackboxLastRcModeFlags));
2038 memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
2039 blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
2040 }
2041 }
2042
2043 /*
2044 * Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control
2045 * the portion of logged loop iterations.
2046 */
2047 static bool blackboxShouldLogPFrame(uint32_t pFrameIndex)
2048 {
2049 /* Adding a magic shift of "blackboxConfig()->rate_num - 1" in here creates a better spread of
2050 * recorded / skipped frames when the I frame's position is considered:
2051 */
2052 return (pFrameIndex + blackboxConfig()->rate_num - 1) % blackboxConfig()->rate_denom < blackboxConfig()->rate_num;
2053 }
2054
2055 static bool blackboxShouldLogIFrame(void)
2056 {
2057 return blackboxPFrameIndex == 0;
2058 }
2059
2060 // Called once every FC loop in order to keep track of how many FC loop iterations have passed
2061 static void blackboxAdvanceIterationTimers(void)
2062 {
2063 blackboxSlowFrameIterationTimer++;
2064 blackboxIteration++;
2065 blackboxPFrameIndex++;
2066
2067 if (blackboxPFrameIndex == blackboxIFrameInterval) {
2068 blackboxPFrameIndex = 0;
2069 blackboxIFrameIndex++;
2070 }
2071 }
2072
2073 // Called once every FC loop in order to log the current state
2074 static void blackboxLogIteration(timeUs_t currentTimeUs)
2075 {
2076 // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
2077 if (blackboxShouldLogIFrame()) {
2078 /*
2079 * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
2080 * an additional item to write at the same time). Unless we're *only* logging "I" frames, then we have no choice.
2081 */
2082 writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes());
2083
2084 loadMainState(currentTimeUs);
2085 writeIntraframe();
2086 } else {
2087 blackboxCheckAndLogArmingBeep();
2088 blackboxCheckAndLogFlightMode();
2089
2090 if (blackboxShouldLogPFrame(blackboxPFrameIndex)) {
2091 /*
2092 * We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
2093 * So only log slow frames during loop iterations where we log a main frame.
2094 */
2095 writeSlowFrameIfNeeded(true);
2096
2097 loadMainState(currentTimeUs);
2098 writeInterframe();
2099 }
2100 #ifdef USE_GPS
2101 if (feature(FEATURE_GPS)) {
2102 /*
2103 * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the
2104 * GPS home position.
2105 *
2106 * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
2107 * still be interpreted correctly.
2108 */
2109 if (GPS_home.lat != gpsHistory.GPS_home[0] || GPS_home.lon != gpsHistory.GPS_home[1]
2110 || (blackboxPFrameIndex == (blackboxIFrameInterval / 2) && blackboxIFrameIndex % 128 == 0)) {
2111
2112 writeGPSHomeFrame();
2113 writeGPSFrame(currentTimeUs);
2114 } else if (gpsSol.numSat != gpsHistory.GPS_numSat || gpsSol.llh.lat != gpsHistory.GPS_coord[0]
2115 || gpsSol.llh.lon != gpsHistory.GPS_coord[1]) {
2116 //We could check for velocity changes as well but I doubt it changes independent of position
2117 writeGPSFrame(currentTimeUs);
2118 }
2119 }
2120 #endif
2121 }
2122
2123 //Flush every iteration so that our runtime variance is minimized
2124 blackboxDeviceFlush();
2125 }
2126
2127 /**
2128 * Call each flight loop iteration to perform blackbox logging.
2129 */
2130 void blackboxUpdate(timeUs_t currentTimeUs)
2131 {
2132 if (blackboxState >= BLACKBOX_FIRST_HEADER_SENDING_STATE && blackboxState <= BLACKBOX_LAST_HEADER_SENDING_STATE) {
2133 blackboxReplenishHeaderBudget();
2134 }
2135
2136 switch (blackboxState) {
2137 case BLACKBOX_STATE_PREPARE_LOG_FILE:
2138 if (blackboxDeviceBeginLog()) {
2139 blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
2140 }
2141 break;
2142 case BLACKBOX_STATE_SEND_HEADER:
2143 //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
2144
2145 /*
2146 * Once the UART has had time to init, transmit the header in chunks so we don't overflow its transmit
2147 * buffer, overflow the OpenLog's buffer, or keep the main loop busy for too long.
2148 */
2149 if (millis() > xmitState.u.startTime + 100) {
2150 if (blackboxDeviceReserveBufferSpace(BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION) == BLACKBOX_RESERVE_SUCCESS) {
2151 for (int i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
2152 blackboxWrite(blackboxHeader[xmitState.headerIndex]);
2153 blackboxHeaderBudget--;
2154 }
2155
2156 if (blackboxHeader[xmitState.headerIndex] == '\0') {
2157 blackboxPrintfHeaderLine("I interval", "%d", blackboxIFrameInterval);
2158 blackboxSetState(BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER);
2159 }
2160 }
2161 }
2162 break;
2163 case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
2164 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
2165 if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAYLEN(blackboxMainFields),
2166 &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
2167 #ifdef USE_GPS
2168 if (feature(FEATURE_GPS)) {
2169 blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADER);
2170 } else
2171 #endif
2172 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
2173 }
2174 break;
2175 #ifdef USE_GPS
2176 case BLACKBOX_STATE_SEND_GPS_H_HEADER:
2177 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
2178 if (!sendFieldDefinition('H', 0, blackboxGpsHFields, blackboxGpsHFields + 1, ARRAYLEN(blackboxGpsHFields),
2179 NULL, NULL)) {
2180 blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADER);
2181 }
2182 break;
2183 case BLACKBOX_STATE_SEND_GPS_G_HEADER:
2184 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
2185 if (!sendFieldDefinition('G', 0, blackboxGpsGFields, blackboxGpsGFields + 1, ARRAYLEN(blackboxGpsGFields),
2186 &blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition)) {
2187 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
2188 }
2189 break;
2190 #endif
2191 case BLACKBOX_STATE_SEND_SLOW_HEADER:
2192 //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
2193 if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAYLEN(blackboxSlowFields),
2194 NULL, NULL)) {
2195 blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
2196 }
2197 break;
2198 case BLACKBOX_STATE_SEND_SYSINFO:
2199 //On entry of this state, xmitState.headerIndex is 0
2200
2201 //Keep writing chunks of the system info headers until it returns true to signal completion
2202 if (blackboxWriteSysinfo()) {
2203 /*
2204 * Wait for header buffers to drain completely before data logging begins to ensure reliable header delivery
2205 * (overflowing circular buffers causes all data to be discarded, so the first few logged iterations
2206 * could wipe out the end of the header if we weren't careful)
2207 */
2208 if (blackboxDeviceFlushForce()) {
2209 blackboxSetState(BLACKBOX_STATE_RUNNING);
2210 }
2211 }
2212 break;
2213 case BLACKBOX_STATE_PAUSED:
2214 // Only allow resume to occur during an I-frame iteration, so that we have an "I" base to work from
2215 if (IS_RC_MODE_ACTIVE(BOXBLACKBOX) && blackboxShouldLogIFrame()) {
2216 // Write a log entry so the decoder is aware that our large time/iteration skip is intended
2217 flightLogEvent_loggingResume_t resume;
2218
2219 resume.logIteration = blackboxIteration;
2220 resume.currentTimeUs = currentTimeUs;
2221
2222 blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
2223 blackboxSetState(BLACKBOX_STATE_RUNNING);
2224
2225 blackboxLogIteration(currentTimeUs);
2226 }
2227 // Keep the logging timers ticking so our log iteration continues to advance
2228 blackboxAdvanceIterationTimers();
2229 break;
2230 case BLACKBOX_STATE_RUNNING:
2231 // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
2232 if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
2233 blackboxSetState(BLACKBOX_STATE_PAUSED);
2234 } else {
2235 blackboxLogIteration(currentTimeUs);
2236 }
2237 blackboxAdvanceIterationTimers();
2238 break;
2239 case BLACKBOX_STATE_SHUTTING_DOWN:
2240 //On entry of this state, startTime is set
2241 /*
2242 * Wait for the log we've transmitted to make its way to the logger before we release the serial port,
2243 * since releasing the port clears the Tx buffer.
2244 *
2245 * Don't wait longer than it could possibly take if something funky happens.
2246 */
2247 if (blackboxDeviceEndLog(blackboxLoggedAnyFrames) && (millis() > xmitState.u.startTime + BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS || blackboxDeviceFlushForce())) {
2248 blackboxDeviceClose();
2249 blackboxSetState(BLACKBOX_STATE_STOPPED);
2250 }
2251 break;
2252 default:
2253 break;
2254 }
2255
2256 // Did we run out of room on the device? Stop!
2257 if (isBlackboxDeviceFull()) {
2258 blackboxSetState(BLACKBOX_STATE_STOPPED);
2259 }
2260 }
2261
2262 static bool canUseBlackboxWithCurrentConfiguration(void)
2263 {
2264 return feature(FEATURE_BLACKBOX);
2265 }
2266
2267 /**
2268 * Call during system startup to initialize the blackbox.
2269 */
2270 void blackboxInit(void)
2271 {
2272 if (canUseBlackboxWithCurrentConfiguration()) {
2273 blackboxSetState(BLACKBOX_STATE_STOPPED);
2274 } else {
2275 blackboxSetState(BLACKBOX_STATE_DISABLED);
2276 }
2277
2278 /* FIXME is this really necessary ? Why? */
2279 int max_denom = 4096*1000 / gyroConfig()->looptime;
2280 if (blackboxConfig()->rate_denom > max_denom) {
2281 blackboxConfigMutable()->rate_denom = max_denom;
2282 }
2283 /* Decide on how ofter are we going to log I-frames*/
2284 if (blackboxConfig()->rate_denom <= 32) {
2285 blackboxIFrameInterval = 32;
2286 }
2287 else {
2288 // Use next higher power of two via GCC builtin
2289 blackboxIFrameInterval = 1 << (32 - __builtin_clz (blackboxConfig()->rate_denom - 1));
2290 }
2291 }
2292 #endif
INAV: Navigation-enabled flight control software