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