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