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