src/main/blackbox/blackbox.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_BLACKBOX
  29
  30 #include "blackbox.h"
  31 #include "blackbox_encoding.h"
  32 #include "blackbox_fielddefs.h"
  33 #include "blackbox_io.h"
  34
  35 #include "build/build_config.h"
  36 #include "build/debug.h"
  37 #include "build/version.h"
  38
  39 #include "common/axis.h"
  40 #include "common/encoding.h"
  41 #include "common/maths.h"
  42 #include "common/time.h"
  43 #include "common/utils.h"
  44
  45 #include "config/config.h"
  46 #include "config/feature.h"
  47
  48 #include "cli/settings.h"
  49
  50 #include "drivers/compass/compass.h"
  51 #include "drivers/sensor.h"
  52 #include "drivers/time.h"
  53 #ifdef USE_DSHOT_TELEMETRY
  54 #include "drivers/dshot.h"
  55 #endif
  56
  57 #include "fc/board_info.h"
  58 #include "fc/controlrate_profile.h"
  59 #include "fc/parameter_names.h"
  60 #include "fc/rc.h"
  61 #include "fc/rc_controls.h"
  62 #include "fc/rc_modes.h"
  63 #include "fc/runtime_config.h"
  64
  65 #include "flight/failsafe.h"
  66 #include "flight/gps_rescue.h"
  67 #include "flight/mixer.h"
  68 #include "flight/pid.h"
  69 #include "flight/position.h"
  70 #include "flight/rpm_filter.h"
  71 #include "flight/servos.h"
  72 #include "flight/imu.h"
  73
  74 #include "io/beeper.h"
  75 #include "io/gps.h"
  76 #include "io/serial.h"
  77
  78 #include "pg/pg.h"
  79 #include "pg/pg_ids.h"
  80
  81 #include "pg/alt_hold.h"
  82 #include "pg/autopilot.h"
  83 #include "pg/motor.h"
  84 #include "pg/pilot.h"
  85 #include "pg/pos_hold.h"
  86 #include "pg/rx.h"
  87
  88 #include "rx/rx.h"
  89
  90 #include "sensors/acceleration.h"
  91 #include "sensors/barometer.h"
  92 #include "sensors/battery.h"
  93 #include "sensors/compass.h"
  94 #include "sensors/gyro.h"
  95 #include "sensors/gyro_init.h"
  96 #include "sensors/rangefinder.h"
  97
  98 #ifdef USE_FLASH_TEST_PRBS
  99 void checkFlashStart(void);
 100 void checkFlashStop(void);
 101 #endif
 102
 103 #if !defined(DEFAULT_BLACKBOX_DEVICE)
 104 #define DEFAULT_BLACKBOX_DEVICE     BLACKBOX_DEVICE_NONE
 105 #endif
 106
 107 PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 4);
 108
 109 PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
 110     .fields_disabled_mask = 0, // default log all fields
 111     .sample_rate = BLACKBOX_RATE_QUARTER,
 112     .device = DEFAULT_BLACKBOX_DEVICE,
 113     .mode = BLACKBOX_MODE_NORMAL,
 114     .high_resolution = false
 115 );
 116
 117 STATIC_ASSERT((sizeof(blackboxConfig()->fields_disabled_mask) * 8) >= FLIGHT_LOG_FIELD_SELECT_COUNT, too_many_flight_log_fields_selections);
 118
 119 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
 120
 121 // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
 122
 123 #define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
 124 #define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)
 125 #define CONDITION(x) CONCAT(FLIGHT_LOG_FIELD_CONDITION_, x)
 126 #define FIELD_SELECT(x) CONCAT(FLIGHT_LOG_FIELD_SELECT_, x)
 127 #define UNSIGNED FLIGHT_LOG_FIELD_UNSIGNED
 128 #define SIGNED FLIGHT_LOG_FIELD_SIGNED
 129
 130 static const char blackboxHeader[] =
 131     "H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
 132     "H Data version:2\n";
 133
 134 static const char* const blackboxFieldHeaderNames[] = {
 135     "name",
 136     "signed",
 137     "predictor",
 138     "encoding",
 139     "predictor",
 140     "encoding"
 141 };
 142
 143 /* All field definition structs should look like this (but with longer arrs): */
 144 typedef struct blackboxFieldDefinition_s {
 145     const char *name;
 146     // If the field name has a number to be included in square brackets [1] afterwards, set it here, or -1 for no brackets:
 147     int8_t fieldNameIndex;
 148
 149     // Each member of this array will be the value to print for this field for the given header index
 150     uint8_t arr[1];
 151 } blackboxFieldDefinition_t;
 152
 153 #define BLACKBOX_DELTA_FIELD_HEADER_COUNT       ARRAYLEN(blackboxFieldHeaderNames)
 154 #define BLACKBOX_SIMPLE_FIELD_HEADER_COUNT      (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
 155 #define BLACKBOX_CONDITIONAL_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
 156
 157 typedef struct blackboxSimpleFieldDefinition_s {
 158     const char *name;
 159     int8_t fieldNameIndex;
 160
 161     uint8_t isSigned;
 162     uint8_t predict;
 163     uint8_t encode;
 164 } blackboxSimpleFieldDefinition_t;
 165
 166 typedef struct blackboxConditionalFieldDefinition_s {
 167     const char *name;
 168     int8_t fieldNameIndex;
 169
 170     uint8_t isSigned;
 171     uint8_t predict;
 172     uint8_t encode;
 173     uint8_t condition; // Decide whether this field should appear in the log
 174 } blackboxConditionalFieldDefinition_t;
 175
 176 typedef struct blackboxDeltaFieldDefinition_s {
 177     const char *name;
 178     int8_t fieldNameIndex;
 179
 180     uint8_t isSigned;
 181     uint8_t Ipredict;
 182     uint8_t Iencode;
 183     uint8_t Ppredict;
 184     uint8_t Pencode;
 185     uint8_t condition; // Decide whether this field should appear in the log
 186 } blackboxDeltaFieldDefinition_t;
 187
 188 /**
 189  * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
 190  * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
 191  * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
 192  * the encoding we've promised here).
 193  */
 194 static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
 195     /* loopIteration doesn't appear in P frames since it always increments */
 196     {"loopIteration",-1, UNSIGNED, .Ipredict = PREDICT(0),     .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC),           .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)},
 197     /* Time advances pretty steadily so the P-frame prediction is a straight line */
 198     {"time",       -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(STRAIGHT_LINE), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 199     {"axisP",       0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
 200     {"axisP",       1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
 201     {"axisP",       2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
 202     /* I terms get special packed encoding in P frames: */
 203     {"axisI",       0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG2_3S32), CONDITION(PID)},
 204     {"axisI",       1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG2_3S32), CONDITION(PID)},
 205     {"axisI",       2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG2_3S32), CONDITION(PID)},
 206     {"axisD",       0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
 207     {"axisD",       1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
 208     {"axisD",       2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
 209     {"axisF",       0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
 210     {"axisF",       1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
 211     {"axisF",       2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(PID)},
 212 #ifdef USE_WING
 213     {"axisS",       0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_WING_S_0)},
 214     {"axisS",       1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_WING_S_1)},
 215     {"axisS",       2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_WING_S_2)},
 216 #endif
 217     /* rcCommands are encoded together as a group in P-frames: */
 218     {"rcCommand",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
 219     {"rcCommand",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
 220     {"rcCommand",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
 221     {"rcCommand",   3, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(RC_COMMANDS)},
 222
 223     // setpoint - define 4 fields like rcCommand to use the same encoding. setpoint[4] contains the mixer throttle
 224     {"setpoint",    0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
 225     {"setpoint",    1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
 226     {"setpoint",    2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
 227     {"setpoint",    3, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(SETPOINT)},
 228
 229     {"vbatLatest",    -1, UNSIGNED, .Ipredict = PREDICT(VBATREF),  .Iencode = ENCODING(NEG_14BIT),   .Ppredict = PREDICT(PREVIOUS),  .Pencode = ENCODING(TAG8_8SVB), CONDITION(VBAT)},
 230     {"amperageLatest",-1, SIGNED,   .Ipredict = PREDICT(0),        .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),  .Pencode = ENCODING(TAG8_8SVB), CONDITION(AMPERAGE_ADC)},
 231
 232 #ifdef USE_MAG
 233     {"magADC",      0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)},
 234     {"magADC",      1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)},
 235     {"magADC",      2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)},
 236 #endif
 237 #ifdef USE_BARO
 238     {"baroAlt",    -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(BARO)},
 239 #endif
 240 #ifdef USE_RANGEFINDER
 241     {"surfaceRaw",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(RANGEFINDER)},
 242 #endif
 243     {"rssi",       -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(RSSI)},
 244
 245     /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
 246     {"gyroADC",     0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(GYRO)},
 247     {"gyroADC",     1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(GYRO)},
 248     {"gyroADC",     2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(GYRO)},
 249     {"gyroUnfilt",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(GYROUNFILT)},
 250     {"gyroUnfilt",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(GYROUNFILT)},
 251     {"gyroUnfilt",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(GYROUNFILT)},
 252 #ifdef USE_ACC
 253     {"accSmooth",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
 254     {"accSmooth",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
 255     {"accSmooth",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ACC)},
 256     {"imuQuaternion",    0, SIGNED,   .Ipredict = PREDICT(0),  .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ATTITUDE)},
 257     {"imuQuaternion",    1, SIGNED,   .Ipredict = PREDICT(0),  .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ATTITUDE)},
 258     {"imuQuaternion",    2, SIGNED,   .Ipredict = PREDICT(0),  .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ATTITUDE)},
 259 #endif
 260     {"debug",       0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
 261     {"debug",       1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
 262     {"debug",       2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
 263     {"debug",       3, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
 264     {"debug",       4, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
 265     {"debug",       5, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
 266     {"debug",       6, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
 267     {"debug",       7, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(DEBUG_LOG)},
 268     /* 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): */
 269     {"motor",       0, UNSIGNED, .Ipredict = PREDICT(MINMOTOR), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
 270     /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
 271     {"motor",       1, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_2)},
 272     {"motor",       2, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_3)},
 273     {"motor",       3, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_4)},
 274     {"motor",       4, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_5)},
 275     {"motor",       5, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_6)},
 276     {"motor",       6, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)},
 277     {"motor",       7, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)},
 278
 279 #ifdef USE_SERVOS
 280     /* NOTE (ledvinap, hwarhurst): Decoding would fail if previous encoding is also TAG8_8SVB and does not have exactly 8 values. To fix it, inserting ENCODING_NULL dummy value should force end of previous group. */
 281     {"servo",       0, UNSIGNED, .Ipredict = PREDICT(1500),    .Iencode = ENCODING(TAG8_8SVB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)},
 282     {"servo",       1, UNSIGNED, .Ipredict = PREDICT(1500),    .Iencode = ENCODING(TAG8_8SVB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)},
 283     {"servo",       2, UNSIGNED, .Ipredict = PREDICT(1500),    .Iencode = ENCODING(TAG8_8SVB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)},
 284     {"servo",       3, UNSIGNED, .Ipredict = PREDICT(1500),    .Iencode = ENCODING(TAG8_8SVB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)},
 285     {"servo",       4, UNSIGNED, .Ipredict = PREDICT(1500),    .Iencode = ENCODING(TAG8_8SVB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)},
 286     {"servo",       5, UNSIGNED, .Ipredict = PREDICT(1500),    .Iencode = ENCODING(TAG8_8SVB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)},
 287     {"servo",       6, UNSIGNED, .Ipredict = PREDICT(1500),    .Iencode = ENCODING(TAG8_8SVB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)},
 288     {"servo",       7, UNSIGNED, .Ipredict = PREDICT(1500),    .Iencode = ENCODING(TAG8_8SVB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(SERVOS)},
 289 #endif
 290
 291 #ifdef USE_DSHOT_TELEMETRY
 292     // eRPM / 100
 293     {"eRPM",  0, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_1_HAS_RPM)},
 294     {"eRPM",  1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_2_HAS_RPM)},
 295     {"eRPM",  2, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_3_HAS_RPM)},
 296     {"eRPM",  3, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_4_HAS_RPM)},
 297     {"eRPM",  4, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_5_HAS_RPM)},
 298     {"eRPM",  5, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_6_HAS_RPM)},
 299     {"eRPM",  6, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_7_HAS_RPM)},
 300     {"eRPM",  7, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_8_HAS_RPM)},
 301 #endif /* USE_DSHOT_TELEMETRY */
 302 };
 303
 304 #ifdef USE_GPS
 305 // GPS position/vel frame
 306 static const blackboxConditionalFieldDefinition_t blackboxGpsGFields[] = {
 307     {"time",              -1, UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)},
 308     {"GPS_numSat",        -1, UNSIGNED, PREDICT(0),          ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
 309     {"GPS_coord",          0, SIGNED,   PREDICT(HOME_COORD), ENCODING(SIGNED_VB),   CONDITION(ALWAYS)},
 310     {"GPS_coord",          1, SIGNED,   PREDICT(HOME_COORD), ENCODING(SIGNED_VB),   CONDITION(ALWAYS)},
 311     {"GPS_altitude",      -1, SIGNED,   PREDICT(0),          ENCODING(SIGNED_VB),   CONDITION(ALWAYS)},
 312     {"GPS_speed",         -1, UNSIGNED, PREDICT(0),          ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
 313     {"GPS_ground_course", -1, UNSIGNED, PREDICT(0),          ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}
 314 };
 315
 316 // GPS home frame
 317 static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = {
 318     {"GPS_home",           0, SIGNED,   PREDICT(0),          ENCODING(SIGNED_VB)},
 319     {"GPS_home",           1, SIGNED,   PREDICT(0),          ENCODING(SIGNED_VB)},
 320     {"GPS_home",           2, SIGNED,   PREDICT(0),          ENCODING(SIGNED_VB)}
 321 };
 322 #endif
 323
 324 // Rarely-updated fields
 325 static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
 326     {"flightModeFlags",       -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
 327     {"stateFlags",            -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
 328
 329     {"failsafePhase",         -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)},
 330     {"rxSignalReceived",      -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)},
 331     {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)}
 332 };
 333
 334 typedef enum BlackboxState {
 335     BLACKBOX_STATE_DISABLED = 0,
 336     BLACKBOX_STATE_STOPPED,
 337     BLACKBOX_STATE_PREPARE_LOG_FILE,
 338     BLACKBOX_STATE_SEND_HEADER,
 339     BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER,
 340     BLACKBOX_STATE_SEND_GPS_H_HEADER,
 341     BLACKBOX_STATE_SEND_GPS_G_HEADER,
 342     BLACKBOX_STATE_SEND_SLOW_HEADER,
 343     BLACKBOX_STATE_SEND_SYSINFO,
 344     BLACKBOX_STATE_CACHE_FLUSH,
 345     BLACKBOX_STATE_PAUSED,
 346     BLACKBOX_STATE_RUNNING,
 347     BLACKBOX_STATE_SHUTTING_DOWN,
 348     BLACKBOX_STATE_START_ERASE,
 349     BLACKBOX_STATE_ERASING,
 350     BLACKBOX_STATE_ERASED
 351 } BlackboxState;
 352
 353 typedef struct blackboxMainState_s {
 354     uint32_t time;
 355
 356     int32_t axisPID_P[XYZ_AXIS_COUNT];
 357     int32_t axisPID_I[XYZ_AXIS_COUNT];
 358     int32_t axisPID_D[XYZ_AXIS_COUNT];
 359     int32_t axisPID_F[XYZ_AXIS_COUNT];
 360     int32_t axisPID_S[XYZ_AXIS_COUNT];
 361
 362     int16_t rcCommand[4];
 363     int16_t setpoint[4];
 364     int16_t gyroADC[XYZ_AXIS_COUNT];
 365     int16_t gyroUnfilt[XYZ_AXIS_COUNT];
 366 #ifdef USE_ACC
 367     int16_t accADC[XYZ_AXIS_COUNT];
 368     int16_t imuAttitudeQuaternion[XYZ_AXIS_COUNT];
 369 #endif
 370     int16_t debug[DEBUG16_VALUE_COUNT];
 371     int16_t motor[MAX_SUPPORTED_MOTORS];
 372     int16_t servo[MAX_SUPPORTED_SERVOS];
 373 #ifdef USE_DSHOT_TELEMETRY
 374     int16_t erpm[MAX_SUPPORTED_MOTORS];
 375 #endif
 376
 377     uint16_t vbatLatest;
 378     int32_t amperageLatest;
 379
 380 #ifdef USE_BARO
 381     int32_t baroAlt;
 382 #endif
 383 #ifdef USE_MAG
 384     int16_t magADC[XYZ_AXIS_COUNT];
 385 #endif
 386 #ifdef USE_RANGEFINDER
 387     int32_t surfaceRaw;
 388 #endif
 389     uint16_t rssi;
 390 } blackboxMainState_t;
 391
 392 typedef struct blackboxGpsState_s {
 393     gpsLocation_t GPS_home;
 394     gpsLocation_t GPS_coord;
 395     uint8_t GPS_numSat;
 396 } blackboxGpsState_t;
 397
 398 // This data is updated really infrequently:
 399 typedef struct blackboxSlowState_s {
 400     uint32_t flightModeFlags; // extend this data size (from uint16_t)
 401     uint8_t stateFlags;
 402     uint8_t failsafePhase;
 403     bool rxSignalReceived;
 404     bool rxFlightChannelsValid;
 405 } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
 406
 407 //From rc_controls.c
 408 extern boxBitmask_t rcModeActivationMask;
 409
 410 static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
 411
 412 static uint32_t blackboxLastArmingBeep = 0;
 413 static uint32_t blackboxLastFlightModeFlags = 0; // New event tracking of flight modes
 414
 415 static struct {
 416     uint32_t headerIndex;
 417
 418     /* Since these fields are used during different blackbox states (never simultaneously) we can
 419      * overlap them to save on RAM
 420      */
 421     union {
 422         int fieldIndex;
 423         uint32_t startTime;
 424     } u;
 425 } xmitState;
 426
 427 // Cache for FLIGHT_LOG_FIELD_CONDITION_* test results:
 428 static uint64_t blackboxConditionCache;
 429
 430 STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_LAST, too_many_flight_log_conditions);
 431
 432 static uint32_t blackboxIteration;
 433 static uint16_t blackboxLoopIndex;
 434 static uint16_t blackboxPFrameIndex;
 435 static uint16_t blackboxIFrameIndex;
 436 // number of flight loop iterations before logging I-frame
 437 // typically 32 for 1kHz loop, 64 for 2kHz loop etc
 438 STATIC_UNIT_TESTED int16_t blackboxIInterval = 0;
 439 // number of flight loop iterations before logging P-frame
 440 STATIC_UNIT_TESTED int8_t blackboxPInterval = 0;
 441 STATIC_UNIT_TESTED int32_t blackboxSInterval = 0;
 442 STATIC_UNIT_TESTED int32_t blackboxSlowFrameIterationTimer;
 443 static bool blackboxLoggedAnyFrames;
 444 static float blackboxHighResolutionScale;
 445
 446 /*
 447  * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
 448  * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs
 449  * to encode:
 450  */
 451 static uint16_t vbatReference;
 452
 453 static blackboxGpsState_t gpsHistory;
 454 static blackboxSlowState_t slowHistory;
 455
 456 // Keep a history of length 2, plus a buffer for MW to store the new values into
 457 static blackboxMainState_t blackboxHistoryRing[3];
 458
 459 // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
 460 static blackboxMainState_t* blackboxHistory[3];
 461
 462 static bool blackboxModeActivationConditionPresent = false;
 463
 464 /**
 465  * Return true if it is safe to edit the Blackbox configuration.
 466  */
 467 bool blackboxMayEditConfig(void)
 468 {
 469     return blackboxState <= BLACKBOX_STATE_STOPPED;
 470 }
 471
 472 static bool blackboxIsOnlyLoggingIntraframes(void)
 473 {
 474     return blackboxPInterval == 0;
 475 }
 476
 477 static bool isFieldEnabled(FlightLogFieldSelect_e field)
 478 {
 479     return (blackboxConfig()->fields_disabled_mask & (1 << field)) == 0;
 480 }
 481
 482 static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
 483 {
 484     switch (condition) {
 485     case CONDITION(ALWAYS):
 486         return true;
 487
 488     case CONDITION(AT_LEAST_MOTORS_1):
 489     case CONDITION(AT_LEAST_MOTORS_2):
 490     case CONDITION(AT_LEAST_MOTORS_3):
 491     case CONDITION(AT_LEAST_MOTORS_4):
 492     case CONDITION(AT_LEAST_MOTORS_5):
 493     case CONDITION(AT_LEAST_MOTORS_6):
 494     case CONDITION(AT_LEAST_MOTORS_7):
 495     case CONDITION(AT_LEAST_MOTORS_8):
 496         return (getMotorCount() >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1) && isFieldEnabled(FIELD_SELECT(MOTOR));
 497
 498 #ifdef USE_DSHOT_TELEMETRY
 499     case CONDITION(MOTOR_1_HAS_RPM):
 500     case CONDITION(MOTOR_2_HAS_RPM):
 501     case CONDITION(MOTOR_3_HAS_RPM):
 502     case CONDITION(MOTOR_4_HAS_RPM):
 503     case CONDITION(MOTOR_5_HAS_RPM):
 504     case CONDITION(MOTOR_6_HAS_RPM):
 505     case CONDITION(MOTOR_7_HAS_RPM):
 506     case CONDITION(MOTOR_8_HAS_RPM):
 507         return (getMotorCount() >= condition - CONDITION(MOTOR_1_HAS_RPM) + 1) && useDshotTelemetry && isFieldEnabled(FIELD_SELECT(RPM));
 508 #endif
 509
 510 #ifdef USE_SERVOS
 511     case CONDITION(SERVOS):
 512         return hasServos() && (FIELD_SELECT(SERVO));
 513 #endif
 514
 515     case CONDITION(PID):
 516         return isFieldEnabled(FIELD_SELECT(PID));
 517
 518     case CONDITION(NONZERO_PID_D_0):
 519     case CONDITION(NONZERO_PID_D_1):
 520     case CONDITION(NONZERO_PID_D_2):
 521         return (currentPidProfile->pid[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0].D != 0) && isFieldEnabled(FIELD_SELECT(PID));
 522
 523 #ifdef USE_WING
 524     case CONDITION(NONZERO_WING_S_0):
 525     case CONDITION(NONZERO_WING_S_1):
 526     case CONDITION(NONZERO_WING_S_2):
 527         return (currentPidProfile->pid[condition - CONDITION(NONZERO_WING_S_0)].S != 0) && isFieldEnabled(FIELD_SELECT(PID));
 528 #endif
 529
 530     case CONDITION(RC_COMMANDS):
 531         return isFieldEnabled(FIELD_SELECT(RC_COMMANDS));
 532
 533     case CONDITION(SETPOINT):
 534         return isFieldEnabled(FIELD_SELECT(SETPOINT));
 535
 536     case CONDITION(MAG):
 537 #ifdef USE_MAG
 538         return sensors(SENSOR_MAG) && isFieldEnabled(FIELD_SELECT(MAG));
 539 #else
 540         return false;
 541 #endif
 542
 543     case CONDITION(BARO):
 544 #ifdef USE_BARO
 545         return sensors(SENSOR_BARO) && isFieldEnabled(FIELD_SELECT(ALTITUDE));
 546 #else
 547         return false;
 548 #endif
 549
 550     case CONDITION(VBAT):
 551         return (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) && isFieldEnabled(FIELD_SELECT(BATTERY));
 552
 553     case CONDITION(AMPERAGE_ADC):
 554         return (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) && (batteryConfig()->currentMeterSource != CURRENT_METER_VIRTUAL) && isFieldEnabled(FIELD_SELECT(BATTERY));
 555
 556     case CONDITION(RANGEFINDER):
 557 #ifdef USE_RANGEFINDER
 558         return sensors(SENSOR_RANGEFINDER) && isFieldEnabled(FIELD_SELECT(ALTITUDE));
 559 #else
 560         return false;
 561 #endif
 562
 563     case CONDITION(RSSI):
 564         return isRssiConfigured() && isFieldEnabled(FIELD_SELECT(RSSI));
 565
 566     case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
 567         return blackboxPInterval != blackboxIInterval;
 568
 569     case CONDITION(GYRO):
 570         return isFieldEnabled(FIELD_SELECT(GYRO));
 571
 572     case CONDITION(GYROUNFILT):
 573         return isFieldEnabled(FIELD_SELECT(GYROUNFILT));
 574
 575     case CONDITION(ACC):
 576         return sensors(SENSOR_ACC) && isFieldEnabled(FIELD_SELECT(ACC));
 577
 578     case CONDITION(ATTITUDE):
 579         return sensors(SENSOR_ACC) && isFieldEnabled(FIELD_SELECT(ATTITUDE));
 580
 581     case CONDITION(DEBUG_LOG):
 582         return (debugMode != DEBUG_NONE) && isFieldEnabled(FIELD_SELECT(DEBUG_LOG));
 583
 584     case CONDITION(NEVER):
 585         return false;
 586
 587     default:
 588         return false;
 589     }
 590 }
 591
 592 static void blackboxBuildConditionCache(void)
 593 {
 594     blackboxConditionCache = 0;
 595     for (FlightLogFieldCondition cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
 596         if (testBlackboxConditionUncached(cond)) {
 597             blackboxConditionCache |= (uint64_t) 1 << cond;
 598         }
 599     }
 600 }
 601
 602 static bool testBlackboxCondition(FlightLogFieldCondition condition)
 603 {
 604     return (blackboxConditionCache & (uint64_t) 1 << condition) != 0;
 605 }
 606
 607 static void blackboxSetState(BlackboxState newState)
 608 {
 609     //Perform initial setup required for the new state
 610     switch (newState) {
 611     case BLACKBOX_STATE_PREPARE_LOG_FILE:
 612         blackboxLoggedAnyFrames = false;
 613         break;
 614     case BLACKBOX_STATE_SEND_HEADER:
 615         blackboxHeaderBudget = 0;
 616         xmitState.headerIndex = 0;
 617         xmitState.u.startTime = millis();
 618         break;
 619     case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
 620     case BLACKBOX_STATE_SEND_GPS_G_HEADER:
 621     case BLACKBOX_STATE_SEND_GPS_H_HEADER:
 622     case BLACKBOX_STATE_SEND_SLOW_HEADER:
 623         xmitState.headerIndex = 0;
 624         xmitState.u.fieldIndex = -1;
 625         break;
 626     case BLACKBOX_STATE_SEND_SYSINFO:
 627         xmitState.headerIndex = 0;
 628         break;
 629     case BLACKBOX_STATE_RUNNING:
 630         blackboxSlowFrameIterationTimer = blackboxSInterval; //Force a slow frame to be written on the first iteration
 631 #ifdef USE_FLASH_TEST_PRBS
 632         // Start writing a known pattern as the running state is entered
 633         checkFlashStart();
 634 #endif
 635         break;
 636     case BLACKBOX_STATE_SHUTTING_DOWN:
 637         xmitState.u.startTime = millis();
 638         break;
 639
 640 #ifdef USE_FLASH_TEST_PRBS
 641     case BLACKBOX_STATE_STOPPED:
 642         // Now that the log is shut down, verify it
 643         checkFlashStop();
 644         break;
 645 #endif
 646     default:
 647         ;
 648     }
 649     blackboxState = newState;
 650 }
 651
 652 static void writeIntraframe(void)
 653 {
 654     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
 655
 656     blackboxWrite('I');
 657
 658     blackboxWriteUnsignedVB(blackboxIteration);
 659     blackboxWriteUnsignedVB(blackboxCurrent->time);
 660
 661     if (testBlackboxCondition(CONDITION(PID))) {
 662         blackboxWriteSignedVBArray(blackboxCurrent->axisPID_P, XYZ_AXIS_COUNT);
 663         blackboxWriteSignedVBArray(blackboxCurrent->axisPID_I, XYZ_AXIS_COUNT);
 664
 665         // Don't bother writing the current D term if the corresponding PID setting is zero
 666         for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
 667             if (testBlackboxCondition(CONDITION(NONZERO_PID_D_0) + x)) {
 668                 blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]);
 669             }
 670         }
 671
 672         blackboxWriteSignedVBArray(blackboxCurrent->axisPID_F, XYZ_AXIS_COUNT);
 673
 674 #ifdef USE_WING
 675         for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
 676             if (testBlackboxCondition(CONDITION(NONZERO_WING_S_0) + x)) {
 677                 blackboxWriteSignedVB(blackboxCurrent->axisPID_S[x]);
 678             }
 679         }
 680 #endif
 681     }
 682
 683     if (testBlackboxCondition(CONDITION(RC_COMMANDS))) {
 684         // Write roll, pitch and yaw first:
 685         blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3);
 686
 687         /*
 688          * Write the throttle separately from the rest of the RC data as it's unsigned.
 689          * Throttle lies in range [PWM_RANGE_MIN..PWM_RANGE_MAX]:
 690          */
 691         blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE]);
 692     }
 693
 694     if (testBlackboxCondition(CONDITION(SETPOINT))) {
 695         // Write setpoint roll, pitch, yaw, and throttle
 696         blackboxWriteSigned16VBArray(blackboxCurrent->setpoint, 4);
 697     }
 698
 699     if (testBlackboxCondition(CONDITION(VBAT))) {
 700         /*
 701          * Our voltage is expected to decrease over the course of the flight, so store our difference from
 702          * the reference:
 703          *
 704          * Write 14 bits even if the number is negative (which would otherwise result in 32 bits)
 705          */
 706         blackboxWriteUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF);
 707     }
 708
 709     if (testBlackboxCondition(CONDITION(AMPERAGE_ADC))) {
 710         // 12bit value directly from ADC
 711         blackboxWriteSignedVB(blackboxCurrent->amperageLatest);
 712     }
 713
 714 #ifdef USE_MAG
 715     if (testBlackboxCondition(CONDITION(MAG))) {
 716         blackboxWriteSigned16VBArray(blackboxCurrent->magADC, XYZ_AXIS_COUNT);
 717     }
 718 #endif
 719
 720 #ifdef USE_BARO
 721     if (testBlackboxCondition(CONDITION(BARO))) {
 722         blackboxWriteSignedVB(blackboxCurrent->baroAlt);
 723     }
 724 #endif
 725
 726 #ifdef USE_RANGEFINDER
 727     if (testBlackboxCondition(CONDITION(RANGEFINDER))) {
 728         blackboxWriteSignedVB(blackboxCurrent->surfaceRaw);
 729     }
 730 #endif
 731
 732     if (testBlackboxCondition(CONDITION(RSSI))) {
 733         blackboxWriteUnsignedVB(blackboxCurrent->rssi);
 734     }
 735
 736     if (testBlackboxCondition(CONDITION(GYRO))) {
 737         blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
 738     }
 739
 740     if (testBlackboxCondition(CONDITION(GYROUNFILT))) {
 741         blackboxWriteSigned16VBArray(blackboxCurrent->gyroUnfilt, XYZ_AXIS_COUNT);
 742     }
 743
 744 #ifdef USE_ACC
 745     if (testBlackboxCondition(CONDITION(ACC))) {
 746         blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
 747     }
 748
 749     if (testBlackboxCondition(CONDITION(ATTITUDE))) {
 750         blackboxWriteSigned16VBArray(blackboxCurrent->imuAttitudeQuaternion, XYZ_AXIS_COUNT);
 751     }
 752 #endif
 753
 754     if (testBlackboxCondition(CONDITION(DEBUG_LOG))) {
 755         blackboxWriteSigned16VBArray(blackboxCurrent->debug, DEBUG16_VALUE_COUNT);
 756     }
 757
 758     if (isFieldEnabled(FIELD_SELECT(MOTOR))) {
 759         //Motors can be below minimum output when disarmed, but that doesn't happen much
 760         blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - getMotorOutputLow());
 761
 762         //Motors tend to be similar to each other so use the first motor's value as a predictor of the others
 763         const int motorCount = getMotorCount();
 764         for (int x = 1; x < motorCount; x++) {
 765             blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
 766         }
 767     }
 768
 769 #ifdef USE_SERVOS
 770     if (testBlackboxCondition(CONDITION(SERVOS))) {
 771         int32_t out[ARRAYLEN(servo)];
 772         for (unsigned x = 0; x < ARRAYLEN(servo); ++x) {
 773             out[x] = blackboxCurrent->servo[x] - 1500;
 774         }
 775
 776         blackboxWriteTag8_8SVB(out, ARRAYLEN(out));
 777     }
 778 #endif
 779
 780 #ifdef USE_DSHOT_TELEMETRY
 781     if (isFieldEnabled(FIELD_SELECT(RPM))) {
 782         const int motorCount = getMotorCount();
 783         for (int x = 0; x < motorCount; x++) {
 784             if (testBlackboxCondition(CONDITION(MOTOR_1_HAS_RPM) + x)) {
 785                 blackboxWriteUnsignedVB(blackboxCurrent->erpm[x]);
 786             }
 787         }
 788     }
 789 #endif
 790
 791     //Rotate our history buffers:
 792
 793     //The current state becomes the new "before" state
 794     blackboxHistory[1] = blackboxHistory[0];
 795     //And since we have no other history, we also use it for the "before, before" state
 796     blackboxHistory[2] = blackboxHistory[0];
 797     //And advance the current state over to a blank space ready to be filled
 798     blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
 799
 800     blackboxLoggedAnyFrames = true;
 801 }
 802
 803 static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHistory, int count)
 804 {
 805     int16_t *curr  = (int16_t*) ((char*) (blackboxHistory[0]) + arrOffsetInHistory);
 806     int16_t *prev1 = (int16_t*) ((char*) (blackboxHistory[1]) + arrOffsetInHistory);
 807     int16_t *prev2 = (int16_t*) ((char*) (blackboxHistory[2]) + arrOffsetInHistory);
 808
 809     for (int i = 0; i < count; i++) {
 810         // Predictor is the average of the previous two history states
 811         int32_t predictor = (prev1[i] + prev2[i]) / 2;
 812
 813         blackboxWriteSignedVB(curr[i] - predictor);
 814     }
 815 }
 816
 817 static void writeInterframe(void)
 818 {
 819     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
 820     blackboxMainState_t *blackboxLast = blackboxHistory[1];
 821
 822     blackboxWrite('P');
 823
 824     //No need to store iteration count since its delta is always 1
 825
 826     /*
 827      * Since the difference between the difference between successive times will be nearly zero (due to consistent
 828      * looptime spacing), use second-order differences.
 829      */
 830     blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
 831
 832     int32_t deltas[8];
 833     int32_t setpointDeltas[4];
 834
 835     if (testBlackboxCondition(CONDITION(PID))) {
 836         arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
 837         blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
 838
 839         /*
 840          * The PID I field changes very slowly, most of the time +-2, so use an encoding
 841          * that can pack all three fields into one byte in that situation.
 842          */
 843         arraySubInt32(deltas, blackboxCurrent->axisPID_I, blackboxLast->axisPID_I, XYZ_AXIS_COUNT);
 844         blackboxWriteTag2_3S32(deltas);
 845
 846         /*
 847          * The PID D term is frequently set to zero for yaw, which makes the result from the calculation
 848          * always zero. So don't bother recording D results when PID D terms are zero.
 849          */
 850         for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
 851             if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) {
 852                 blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]);
 853             }
 854         }
 855
 856         arraySubInt32(deltas, blackboxCurrent->axisPID_F, blackboxLast->axisPID_F, XYZ_AXIS_COUNT);
 857         blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
 858
 859 #ifdef USE_WING
 860         for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
 861             if (testBlackboxCondition(CONDITION(NONZERO_WING_S_0) + x)) {
 862                 blackboxWriteSignedVB(blackboxCurrent->axisPID_S[x] - blackboxLast->axisPID_S[x]);
 863             }
 864         }
 865 #endif
 866     }
 867
 868     /*
 869      * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
 870      * can pack multiple values per byte:
 871      */
 872     for (int x = 0; x < 4; x++) {
 873         deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x];
 874         setpointDeltas[x] = blackboxCurrent->setpoint[x] - blackboxLast->setpoint[x];
 875     }
 876
 877     if (testBlackboxCondition(CONDITION(RC_COMMANDS))) {
 878         blackboxWriteTag8_4S16(deltas);
 879     }
 880     if (testBlackboxCondition(CONDITION(SETPOINT))) {
 881         blackboxWriteTag8_4S16(setpointDeltas);
 882     }
 883
 884     //Check for sensors that are updated periodically (so deltas are normally zero)
 885     int optionalFieldCount = 0;
 886
 887     if (testBlackboxCondition(CONDITION(VBAT))) {
 888         deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->vbatLatest - blackboxLast->vbatLatest;
 889     }
 890
 891     if (testBlackboxCondition(CONDITION(AMPERAGE_ADC))) {
 892         deltas[optionalFieldCount++] = blackboxCurrent->amperageLatest - blackboxLast->amperageLatest;
 893     }
 894
 895 #ifdef USE_MAG
 896     if (testBlackboxCondition(CONDITION(MAG))) {
 897         for (int x = 0; x < XYZ_AXIS_COUNT; x++) {
 898             deltas[optionalFieldCount++] = blackboxCurrent->magADC[x] - blackboxLast->magADC[x];
 899         }
 900     }
 901 #endif
 902
 903 #ifdef USE_BARO
 904     if (testBlackboxCondition(CONDITION(BARO))) {
 905         deltas[optionalFieldCount++] = blackboxCurrent->baroAlt - blackboxLast->baroAlt;
 906     }
 907 #endif
 908
 909 #ifdef USE_RANGEFINDER
 910     if (testBlackboxCondition(CONDITION(RANGEFINDER))) {
 911         deltas[optionalFieldCount++] = blackboxCurrent->surfaceRaw - blackboxLast->surfaceRaw;
 912     }
 913 #endif
 914
 915     if (testBlackboxCondition(CONDITION(RSSI))) {
 916         deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->rssi - blackboxLast->rssi;
 917     }
 918
 919     blackboxWriteTag8_8SVB(deltas, optionalFieldCount);
 920
 921     //Since gyros, accs and motors are noisy, base their predictions on the average of the history:
 922     if (testBlackboxCondition(CONDITION(GYRO))) {
 923         blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC),   XYZ_AXIS_COUNT);
 924     }
 925     if (testBlackboxCondition(CONDITION(GYROUNFILT))) {
 926         blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroUnfilt),   XYZ_AXIS_COUNT);
 927     }
 928
 929 #ifdef USE_ACC
 930     if (testBlackboxCondition(CONDITION(ACC))) {
 931         blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
 932     }
 933
 934     if (testBlackboxCondition(CONDITION(ATTITUDE))) {
 935         blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, imuAttitudeQuaternion), XYZ_AXIS_COUNT);
 936     }
 937 #endif
 938
 939     if (testBlackboxCondition(CONDITION(DEBUG_LOG))) {
 940         blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, debug), DEBUG16_VALUE_COUNT);
 941     }
 942
 943     if (isFieldEnabled(FIELD_SELECT(MOTOR))) {
 944         blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor),     getMotorCount());
 945     }
 946
 947 #ifdef USE_SERVOS
 948     if (testBlackboxCondition(CONDITION(SERVOS))) {
 949         STATIC_ASSERT(ARRAYLEN(servo) <= 8, "TAG8_8SVB supports at most 8 values");
 950         int32_t out[ARRAYLEN(servo)];
 951         for (unsigned x = 0; x < ARRAYLEN(servo); ++x) {
 952             out[x] = blackboxCurrent->servo[x] - blackboxLast->servo[x];
 953         }
 954
 955         blackboxWriteTag8_8SVB(out, ARRAYLEN(out));
 956     }
 957 #endif
 958
 959 #ifdef USE_DSHOT_TELEMETRY
 960     if (isFieldEnabled(FIELD_SELECT(RPM))) {
 961         const int motorCount = getMotorCount();
 962         for (int x = 0; x < motorCount; x++) {
 963             if (testBlackboxCondition(CONDITION(MOTOR_1_HAS_RPM) + x)) {
 964                 blackboxWriteSignedVB(blackboxCurrent->erpm[x] - blackboxLast->erpm[x]);
 965             }
 966         }
 967     }
 968 #endif
 969
 970     //Rotate our history buffers
 971     blackboxHistory[2] = blackboxHistory[1];
 972     blackboxHistory[1] = blackboxHistory[0];
 973     blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
 974
 975     blackboxLoggedAnyFrames = true;
 976 }
 977
 978 /* Write the contents of the global "slowHistory" to the log as an "S" frame. Because this data is logged so
 979  * infrequently, delta updates are not reasonable, so we log independent frames. */
 980 static void writeSlowFrame(void)
 981 {
 982     int32_t values[3];
 983
 984     blackboxWrite('S');
 985
 986     blackboxWriteUnsignedVB(slowHistory.flightModeFlags);
 987     blackboxWriteUnsignedVB(slowHistory.stateFlags);
 988
 989     /*
 990      * Most of the time these three values will be able to pack into one byte for us:
 991      */
 992     values[0] = slowHistory.failsafePhase;
 993     values[1] = slowHistory.rxSignalReceived ? 1 : 0;
 994     values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0;
 995     blackboxWriteTag2_3S32(values);
 996
 997     blackboxSlowFrameIterationTimer = 0;
 998 }
 999
1000 /**
1001  * Load rarely-changing values from the FC into the given structure
1002  */
1003 static void loadSlowState(blackboxSlowState_t *slow)
1004 {
1005     memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
1006     slow->stateFlags = stateFlags;
1007     slow->failsafePhase = failsafePhase();
1008     slow->rxSignalReceived = isRxReceivingSignal();
1009     slow->rxFlightChannelsValid = rxAreFlightChannelsValid();
1010 }
1011
1012 /**
1013  * If the data in the slow frame has changed, log a slow frame.
1014  *
1015  * If allowPeriodicWrite is true, the frame is also logged if it has been more than blackboxSInterval logging iterations
1016  * since the field was last logged.
1017  */
1018 STATIC_UNIT_TESTED bool writeSlowFrameIfNeeded(void)
1019 {
1020     // Write the slow frame peridocially so it can be recovered if we ever lose sync
1021     bool shouldWrite = blackboxSlowFrameIterationTimer >= blackboxSInterval;
1022
1023     if (shouldWrite) {
1024         loadSlowState(&slowHistory);
1025     } else {
1026         blackboxSlowState_t newSlowState;
1027
1028         loadSlowState(&newSlowState);
1029
1030         // Only write a slow frame if it was different from the previous state
1031         if (memcmp(&newSlowState, &slowHistory, sizeof(slowHistory)) != 0) {
1032             // Use the new state as our new history
1033             memcpy(&slowHistory, &newSlowState, sizeof(slowHistory));
1034             shouldWrite = true;
1035         }
1036     }
1037
1038     if (shouldWrite) {
1039         writeSlowFrame();
1040     }
1041     return shouldWrite;
1042 }
1043
1044 void blackboxValidateConfig(void)
1045 {
1046     // If we've chosen an unsupported device, change the device to NONE
1047     switch (blackboxConfig()->device) {
1048 #ifdef USE_FLASHFS
1049     case BLACKBOX_DEVICE_FLASH:
1050 #endif
1051 #ifdef USE_SDCARD
1052     case BLACKBOX_DEVICE_SDCARD:
1053 #endif
1054     case BLACKBOX_DEVICE_SERIAL:
1055         // Device supported, leave the setting alone
1056         break;
1057
1058     default:
1059         blackboxConfigMutable()->device = BLACKBOX_DEVICE_NONE;
1060     }
1061 }
1062
1063 static void blackboxResetIterationTimers(void)
1064 {
1065     blackboxIteration = 0;
1066     blackboxLoopIndex = 0;
1067     blackboxIFrameIndex = 0;
1068     blackboxPFrameIndex = 0;
1069     blackboxSlowFrameIterationTimer = 0;
1070 }
1071
1072 /**
1073  * Start Blackbox logging if it is not already running. Intended to be called upon arming.
1074  */
1075 static void blackboxStart(void)
1076 {
1077     blackboxValidateConfig();
1078
1079     if (!blackboxDeviceOpen()) {
1080         blackboxSetState(BLACKBOX_STATE_DISABLED);
1081         return;
1082     }
1083
1084     memset(&gpsHistory, 0, sizeof(gpsHistory));
1085
1086     blackboxHistory[0] = &blackboxHistoryRing[0];
1087     blackboxHistory[1] = &blackboxHistoryRing[1];
1088     blackboxHistory[2] = &blackboxHistoryRing[2];
1089
1090     vbatReference = getBatteryVoltageLatest();
1091
1092     //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
1093
1094     /*
1095      * We use conditional tests to decide whether or not certain fields should be logged. Since our headers
1096      * must always agree with the logged data, the results of these tests must not change during logging. So
1097      * cache those now.
1098      */
1099     blackboxBuildConditionCache();
1100
1101     blackboxModeActivationConditionPresent = isModeActivationConditionPresent(BOXBLACKBOX);
1102
1103     blackboxResetIterationTimers();
1104
1105     /*
1106      * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
1107      * it finally plays the beep for this arming event.
1108      */
1109     blackboxLastArmingBeep = getArmingBeepTimeMicros();
1110     memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); // record startup status
1111
1112     blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE);
1113 }
1114
1115 /**
1116  * Begin Blackbox shutdown.
1117  */
1118 void blackboxFinish(void)
1119 {
1120     switch (blackboxState) {
1121     case BLACKBOX_STATE_DISABLED:
1122     case BLACKBOX_STATE_STOPPED:
1123     case BLACKBOX_STATE_SHUTTING_DOWN:
1124         // We're already stopped/shutting down
1125         break;
1126     case BLACKBOX_STATE_RUNNING:
1127     case BLACKBOX_STATE_PAUSED:
1128         blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END, NULL);
1129         FALLTHROUGH;
1130     default:
1131         blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN);
1132     }
1133 }
1134
1135 /**
1136  * Test Motors Blackbox Logging
1137  */
1138 static bool startedLoggingInTestMode = false;
1139
1140 static void startInTestMode(void)
1141 {
1142     if (!startedLoggingInTestMode) {
1143         if (blackboxConfig()->device == BLACKBOX_DEVICE_SERIAL) {
1144             serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
1145             if (sharedBlackboxAndMspPort) {
1146                 return; // When in test mode, we cannot share the MSP and serial logger port!
1147             }
1148         }
1149         blackboxStart();
1150         startedLoggingInTestMode = true;
1151     }
1152 }
1153
1154 static void stopInTestMode(void)
1155 {
1156     if (startedLoggingInTestMode) {
1157         blackboxFinish();
1158         startedLoggingInTestMode = false;
1159     }
1160 }
1161 /**
1162  * We are going to monitor the MSP_SET_MOTOR target variables motor_disarmed[] for values other than minthrottle
1163  * on reading a value (i.e. the user is testing the motors), then we enable test mode logging;
1164  * we monitor when the values return to minthrottle and start a delay timer (5 seconds); if
1165  * the test motors are left at minimum throttle for this delay timer, then we assume we are done testing and
1166  * shutdown the logger.
1167  *
1168  * Of course, after the 5 seconds and shutdown of the logger, the system will be re-enabled to allow the
1169  * test mode to trigger again; its just that the data will be in a second, third, fourth etc log file.
1170  */
1171 static bool inMotorTestMode(void)
1172 {
1173     static uint32_t resetTime = 0;
1174
1175     if (!ARMING_FLAG(ARMED) && areMotorsRunning()) {
1176         resetTime = millis() + 5000; // add 5 seconds
1177         return true;
1178     } else {
1179         // Monitor the duration at minimum
1180         return (millis() < resetTime);
1181     }
1182     return false;
1183 }
1184
1185 #ifdef USE_GPS
1186 static void writeGPSHomeFrame(void)
1187 {
1188     blackboxWrite('H');
1189
1190     blackboxWriteSignedVB(GPS_home_llh.lat);
1191     blackboxWriteSignedVB(GPS_home_llh.lon);
1192     blackboxWriteSignedVB(GPS_home_llh.altCm / 10); //log homes altitude, in increments of 0.1m
1193     //TODO it'd be great if we could grab the GPS current time and write that too
1194
1195     gpsHistory.GPS_home = GPS_home_llh;
1196 }
1197
1198 static void writeGPSFrame(timeUs_t currentTimeUs)
1199 {
1200     blackboxWrite('G');
1201
1202     /*
1203      * If we're logging every frame, then a GPS frame always appears just after a frame with the
1204      * currentTime timestamp in the log, so the reader can just use that timestamp for the GPS frame.
1205      *
1206      * If we're not logging every frame, we need to store the time of this GPS frame.
1207      */
1208     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) {
1209         // Predict the time of the last frame in the main log
1210         blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time);
1211     }
1212
1213     blackboxWriteUnsignedVB(gpsSol.numSat);
1214     blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home.lat);
1215     blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home.lon);
1216     blackboxWriteSignedVB(gpsSol.llh.altCm / 10);  // log altitude in increments of 0.1m
1217
1218     if (gpsConfig()->gps_use_3d_speed) {
1219         blackboxWriteUnsignedVB(gpsSol.speed3d);
1220     } else {
1221         blackboxWriteUnsignedVB(gpsSol.groundSpeed);
1222     }
1223
1224     blackboxWriteUnsignedVB(gpsSol.groundCourse);
1225
1226     gpsHistory.GPS_numSat = gpsSol.numSat;
1227     gpsHistory.GPS_coord = gpsSol.llh;
1228 }
1229 #endif
1230
1231 /**
1232  * Fill the current state of the blackbox using values read from the flight controller
1233  */
1234 static void loadMainState(timeUs_t currentTimeUs)
1235 {
1236 #ifndef UNIT_TEST
1237     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
1238
1239     blackboxCurrent->time = currentTimeUs;
1240     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
1241         blackboxCurrent->axisPID_P[i] = lrintf(pidData[i].P);
1242         blackboxCurrent->axisPID_I[i] = lrintf(pidData[i].I);
1243         blackboxCurrent->axisPID_D[i] = lrintf(pidData[i].D);
1244         blackboxCurrent->axisPID_F[i] = lrintf(pidData[i].F);
1245 #ifdef USE_WING
1246         blackboxCurrent->axisPID_S[i] = lrintf(pidData[i].S);
1247 #endif
1248         blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i] * blackboxHighResolutionScale);
1249         blackboxCurrent->gyroUnfilt[i] = lrintf(gyro.gyroADC[i] * blackboxHighResolutionScale);
1250
1251 #if defined(USE_ACC)
1252         blackboxCurrent->accADC[i] = lrintf(acc.accADC.v[i]);
1253         STATIC_ASSERT(offsetof(quaternion_t, w) == 0, "Code expects quaternion in w, x, y, z order");
1254         blackboxCurrent->imuAttitudeQuaternion[i] = lrintf(imuAttitudeQuaternion.v[i + 1] * 0x7FFF);  //Scale to int16 by value 0x7FFF = 2^15 - 1; Use i+1 index for x,y,z components access, [0] - w
1255 #endif
1256 #ifdef USE_MAG
1257         blackboxCurrent->magADC[i] = lrintf(mag.magADC.v[i]);
1258 #endif
1259     }
1260
1261     for (int i = 0; i < 4; i++) {
1262         blackboxCurrent->rcCommand[i] = lrintf(rcCommand[i] * blackboxHighResolutionScale);
1263     }
1264
1265     // log the currentPidSetpoint values applied to the PID controller
1266     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
1267         blackboxCurrent->setpoint[i] = lrintf(pidGetPreviousSetpoint(i) * blackboxHighResolutionScale);
1268     }
1269     // log the final throttle value used in the mixer
1270     blackboxCurrent->setpoint[3] = lrintf(mixerGetThrottle() * 1000);
1271
1272     for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) {
1273         blackboxCurrent->debug[i] = debug[i];
1274     }
1275
1276     const int motorCount = getMotorCount();
1277     for (int i = 0; i < motorCount; i++) {
1278         blackboxCurrent->motor[i] = lrintf(motor[i]);
1279     }
1280
1281 #ifdef USE_DSHOT_TELEMETRY
1282     for (int i = 0; i < motorCount; i++) {
1283         blackboxCurrent->erpm[i] = getDshotErpm(i);
1284     }
1285 #endif
1286
1287     blackboxCurrent->vbatLatest = getBatteryVoltageLatest();
1288     blackboxCurrent->amperageLatest = getAmperageLatest();
1289
1290 #ifdef USE_BARO
1291     blackboxCurrent->baroAlt = baro.altitude;
1292 #endif
1293
1294 #ifdef USE_RANGEFINDER
1295     // Store the raw sonar value without applying tilt correction
1296     blackboxCurrent->surfaceRaw = rangefinderGetLatestAltitude();
1297 #endif
1298
1299     blackboxCurrent->rssi = getRssi();
1300
1301 #ifdef USE_SERVOS
1302     for (unsigned i = 0; i < ARRAYLEN(blackboxCurrent->servo); i++) {
1303         blackboxCurrent->servo[i] = servo[i];
1304     }
1305 #endif
1306 #else
1307     UNUSED(currentTimeUs);
1308 #endif // UNIT_TEST
1309 }
1310
1311 /**
1312  * Transmit the header information for the given field definitions. Transmitted header lines look like:
1313  *
1314  * H Field I name:a,b,c
1315  * H Field I predictor:0,1,2
1316  *
1317  * For all header types, provide a "mainFrameChar" which is the name for the field and will be used to refer to it in the
1318  * header (e.g. P, I etc). For blackboxDeltaField_t fields, also provide deltaFrameChar, otherwise set this to zero.
1319  *
1320  * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
1321  * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
1322  *
1323  * Set xmitState.headerIndex to 0 and xmitState.u.fieldIndex to -1 before calling for the first time.
1324  *
1325  * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the
1326  * fieldDefinition and secondCondition arrays.
1327  *
1328  * Returns true if there is still header left to transmit (so call again to continue transmission).
1329  */
1330 static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const void *fieldDefinitions,
1331         const void *secondFieldDefinition, int fieldCount, const uint8_t *conditions, const uint8_t *secondCondition)
1332 {
1333     const blackboxFieldDefinition_t *def;
1334     unsigned int headerCount;
1335     static bool needComma = false;
1336     size_t definitionStride = (char*) secondFieldDefinition - (char*) fieldDefinitions;
1337     size_t conditionsStride = (char*) secondCondition - (char*) conditions;
1338
1339     if (deltaFrameChar) {
1340         headerCount = BLACKBOX_DELTA_FIELD_HEADER_COUNT;
1341     } else {
1342         headerCount = BLACKBOX_SIMPLE_FIELD_HEADER_COUNT;
1343     }
1344
1345     /*
1346      * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit
1347      * the whole header.
1348      */
1349
1350     // On our first call we need to print the name of the header and a colon
1351     if (xmitState.u.fieldIndex == -1) {
1352         if (xmitState.headerIndex >= headerCount) {
1353             return false; //Someone probably called us again after we had already completed transmission
1354         }
1355
1356         uint32_t charsToBeWritten = strlen("H Field x :") + strlen(blackboxFieldHeaderNames[xmitState.headerIndex]);
1357
1358         if (blackboxDeviceReserveBufferSpace(charsToBeWritten) != BLACKBOX_RESERVE_SUCCESS) {
1359             return true; // Try again later
1360         }
1361
1362         blackboxHeaderBudget -= blackboxPrintf("H Field %c %s:", xmitState.headerIndex >= BLACKBOX_SIMPLE_FIELD_HEADER_COUNT ? deltaFrameChar : mainFrameChar, blackboxFieldHeaderNames[xmitState.headerIndex]);
1363
1364         xmitState.u.fieldIndex++;
1365         needComma = false;
1366     }
1367
1368     // The longest we expect an integer to be as a string:
1369     const uint32_t LONGEST_INTEGER_STRLEN = 2;
1370
1371     for (; xmitState.u.fieldIndex < fieldCount; xmitState.u.fieldIndex++) {
1372         def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * xmitState.u.fieldIndex);
1373
1374         if (!conditions || testBlackboxCondition(conditions[conditionsStride * xmitState.u.fieldIndex])) {
1375             // First (over)estimate the length of the string we want to print
1376
1377             int32_t bytesToWrite = 1; // Leading comma
1378
1379             // The first header is a field name
1380             if (xmitState.headerIndex == 0) {
1381                 bytesToWrite += strlen(def->name) + strlen("[]") + LONGEST_INTEGER_STRLEN;
1382             } else {
1383                 //The other headers are integers
1384                 bytesToWrite += LONGEST_INTEGER_STRLEN;
1385             }
1386
1387             // Now perform the write if the buffer is large enough
1388             if (blackboxDeviceReserveBufferSpace(bytesToWrite) != BLACKBOX_RESERVE_SUCCESS) {
1389                 // Ran out of space!
1390                 return true;
1391             }
1392
1393             blackboxHeaderBudget -= bytesToWrite;
1394
1395             if (needComma) {
1396                 blackboxWrite(',');
1397             } else {
1398                 needComma = true;
1399             }
1400
1401             // The first header is a field name
1402             if (xmitState.headerIndex == 0) {
1403                 blackboxWriteString(def->name);
1404
1405                 // Do we need to print an index in brackets after the name?
1406                 if (def->fieldNameIndex != -1) {
1407                     blackboxPrintf("[%d]", def->fieldNameIndex);
1408                 }
1409             } else {
1410                 //The other headers are integers
1411                 blackboxPrintf("%d", def->arr[xmitState.headerIndex - 1]);
1412             }
1413         }
1414     }
1415
1416     // Did we complete this line?
1417     if (xmitState.u.fieldIndex == fieldCount && blackboxDeviceReserveBufferSpace(1) == BLACKBOX_RESERVE_SUCCESS) {
1418         blackboxHeaderBudget--;
1419         blackboxWrite('\n');
1420         xmitState.headerIndex++;
1421         xmitState.u.fieldIndex = -1;
1422     }
1423
1424     return xmitState.headerIndex < headerCount;
1425 }
1426
1427 // Buf must be at least FORMATTED_DATE_TIME_BUFSIZE
1428 STATIC_UNIT_TESTED char *blackboxGetStartDateTime(char *buf)
1429 {
1430 #ifdef USE_RTC_TIME
1431     dateTime_t dt;
1432     // rtcGetDateTime will fill dt with 0000-01-01T00:00:00
1433     // when time is not known.
1434     rtcGetDateTime(&dt);
1435     dateTimeFormatLocal(buf, &dt);
1436 #else
1437     buf = "0000-01-01T00:00:00.000";
1438 #endif
1439
1440     return buf;
1441 }
1442
1443 #ifndef BLACKBOX_PRINT_HEADER_LINE
1444 #define BLACKBOX_PRINT_HEADER_LINE(name, format, ...) case __COUNTER__: \
1445                                                 blackboxPrintfHeaderLine(name, format, __VA_ARGS__); \
1446                                                 break;
1447 #define BLACKBOX_PRINT_HEADER_LINE_CUSTOM(...) case __COUNTER__: \
1448                                                     {__VA_ARGS__}; \
1449                                                break;
1450 #endif
1451
1452 /**
1453  * Transmit a portion of the system information headers. Call the first time with xmitState.headerIndex == 0. Returns
1454  * true iff transmission is complete, otherwise call again later to continue transmission.
1455  */
1456 static bool blackboxWriteSysinfo(void)
1457 {
1458 #ifndef UNIT_TEST
1459     const uint16_t motorOutputLowInt = lrintf(getMotorOutputLow());
1460     const uint16_t motorOutputHighInt = lrintf(getMotorOutputHigh());
1461
1462     // Make sure we have enough room in the buffer for our longest line (as of this writing, the "Firmware date" line)
1463     if (blackboxDeviceReserveBufferSpace(64) != BLACKBOX_RESERVE_SUCCESS) {
1464         return false;
1465     }
1466
1467     char buf[FORMATTED_DATE_TIME_BUFSIZE];
1468
1469 #ifdef USE_RC_SMOOTHING_FILTER
1470     rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData();
1471 #endif
1472
1473     const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile);
1474     switch (xmitState.headerIndex) {
1475         BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s",                   "Betaflight");
1476         BLACKBOX_PRINT_HEADER_LINE("Firmware revision", "%s %s (%s) %s",    FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName);
1477         BLACKBOX_PRINT_HEADER_LINE("Firmware date", "%s %s",                buildDate, buildTime);
1478         BLACKBOX_PRINT_HEADER_LINE("DeviceUID", "%08x%08x%08x",             U_ID_0, U_ID_1, U_ID_2);
1479 #ifdef USE_BOARD_INFO
1480         BLACKBOX_PRINT_HEADER_LINE("Board information", "%s %s",            getManufacturerId(), getBoardName());
1481 #endif
1482         BLACKBOX_PRINT_HEADER_LINE("Log start datetime", "%s",              blackboxGetStartDateTime(buf));
1483         BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s",                      pilotConfig()->craftName);
1484         BLACKBOX_PRINT_HEADER_LINE("I interval", "%d",                      blackboxIInterval);
1485         BLACKBOX_PRINT_HEADER_LINE("P interval", "%d",                      blackboxPInterval);
1486         BLACKBOX_PRINT_HEADER_LINE("P ratio", "%d",                         (uint16_t)(blackboxIInterval / blackboxPInterval));
1487         BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d",                     motorConfig()->maxthrottle);
1488         BLACKBOX_PRINT_HEADER_LINE("gyro_scale","0x%x",                     castFloatBytesToInt(1.0f));
1489         BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d",                  motorOutputLowInt, motorOutputHighInt);
1490         BLACKBOX_PRINT_HEADER_LINE("motor_kv", "%d",                        motorConfig()->kv);
1491 #if defined(USE_ACC)
1492         BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u",                          acc.dev.acc_1G);
1493 #endif
1494
1495         BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
1496             if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
1497                 blackboxPrintfHeaderLine("vbat_scale", "%u", voltageSensorADCConfig(VOLTAGE_SENSOR_ADC_VBAT)->vbatscale);
1498             } else {
1499                 xmitState.headerIndex += 2; // Skip the next two vbat fields too
1500             }
1501         );
1502
1503         BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u",           batteryConfig()->vbatmincellvoltage,
1504                                                                             batteryConfig()->vbatwarningcellvoltage,
1505                                                                             batteryConfig()->vbatmaxcellvoltage);
1506         BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u",                         vbatReference);
1507
1508         BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
1509             if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) {
1510                 blackboxPrintfHeaderLine("currentSensor", "%d,%d",currentSensorADCConfig()->offset, currentSensorADCConfig()->scale);
1511             }
1512         );
1513
1514         BLACKBOX_PRINT_HEADER_LINE("looptime", "%d",                        gyro.sampleLooptime);
1515         BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d",                 1);
1516         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PID_PROCESS_DENOM, "%d",      activePidLoopDenom);
1517         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_MID, "%d",                currentControlRateProfile->thrMid8);
1518         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THR_EXPO, "%d",               currentControlRateProfile->thrExpo8);
1519         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_MODE, "%d",               currentPidProfile->tpa_mode);
1520         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE, "%d",               currentPidProfile->tpa_rate);
1521         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT, "%d",         currentPidProfile->tpa_breakpoint);
1522         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_RATE, "%d",           currentPidProfile->tpa_low_rate);
1523         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_BREAKPOINT, "%d",     currentPidProfile->tpa_low_breakpoint);
1524         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_ALWAYS, "%d",         currentPidProfile->tpa_low_always);
1525         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MIXER_TYPE, "%s",             lookupTableMixerType[mixerConfig()->mixer_type]);
1526         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_EZ_LANDING_THRESHOLD, "%d",   currentPidProfile->ez_landing_threshold);
1527         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_EZ_LANDING_LIMIT, "%d",       currentPidProfile->ez_landing_limit);
1528         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_EZ_LANDING_SPEED, "%d",       currentPidProfile->ez_landing_speed);
1529         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_DISARM_THRESHOLD, "%d", currentPidProfile->landing_disarm_threshold);
1530
1531 #ifdef USE_WING
1532         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_ROLL_CENTER, "%d",        currentPidProfile->spa_center[FD_ROLL]);
1533         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_ROLL_WIDTH, "%d",         currentPidProfile->spa_width[FD_ROLL]);
1534         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_ROLL_MODE, "%d",          currentPidProfile->spa_mode[FD_ROLL]);
1535         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_PITCH_CENTER, "%d",       currentPidProfile->spa_center[FD_PITCH]);
1536         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_PITCH_WIDTH, "%d",        currentPidProfile->spa_width[FD_PITCH]);
1537         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_PITCH_MODE, "%d",         currentPidProfile->spa_mode[FD_PITCH]);
1538         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_YAW_CENTER, "%d",         currentPidProfile->spa_center[FD_YAW]);
1539         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_YAW_WIDTH, "%d",          currentPidProfile->spa_width[FD_YAW]);
1540         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SPA_YAW_MODE, "%d",           currentPidProfile->spa_mode[FD_YAW]);
1541 #endif // USE_WING
1542
1543 #ifdef USE_ADVANCED_TPA
1544         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_CURVE_TYPE, "%d",           currentPidProfile->tpa_curve_type);
1545         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_CURVE_STALL_THROTTLE, "%d", currentPidProfile->tpa_curve_stall_throttle);
1546         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_CURVE_PID_THR0, "%d",       currentPidProfile->tpa_curve_pid_thr0);
1547         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_CURVE_PID_THR100, "%d",     currentPidProfile->tpa_curve_pid_thr100);
1548         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_CURVE_EXPO, "%d",           currentPidProfile->tpa_curve_expo);
1549 #endif // USE_ADVANCED_TPA
1550
1551         BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d",                  currentControlRateProfile->rcRates[ROLL],
1552                                                                             currentControlRateProfile->rcRates[PITCH],
1553                                                                             currentControlRateProfile->rcRates[YAW]);
1554         BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d,%d,%d",                   currentControlRateProfile->rcExpo[ROLL],
1555                                                                             currentControlRateProfile->rcExpo[PITCH],
1556                                                                             currentControlRateProfile->rcExpo[YAW]);
1557         BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d",                     currentControlRateProfile->rates[ROLL],
1558                                                                             currentControlRateProfile->rates[PITCH],
1559                                                                             currentControlRateProfile->rates[YAW]);
1560         BLACKBOX_PRINT_HEADER_LINE("rate_limits", "%d,%d,%d",               currentControlRateProfile->rate_limit[ROLL],
1561                                                                             currentControlRateProfile->rate_limit[PITCH],
1562                                                                             currentControlRateProfile->rate_limit[YAW]);
1563         BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d",                   currentPidProfile->pid[PID_ROLL].P,
1564                                                                             currentPidProfile->pid[PID_ROLL].I,
1565                                                                             currentPidProfile->pid[PID_ROLL].D);
1566         BLACKBOX_PRINT_HEADER_LINE("pitchPID", "%d,%d,%d",                  currentPidProfile->pid[PID_PITCH].P,
1567                                                                             currentPidProfile->pid[PID_PITCH].I,
1568                                                                             currentPidProfile->pid[PID_PITCH].D);
1569         BLACKBOX_PRINT_HEADER_LINE("yawPID", "%d,%d,%d",                    currentPidProfile->pid[PID_YAW].P,
1570                                                                             currentPidProfile->pid[PID_YAW].I,
1571                                                                             currentPidProfile->pid[PID_YAW].D);
1572         BLACKBOX_PRINT_HEADER_LINE("levelPID", "%d,%d,%d",                  currentPidProfile->pid[PID_LEVEL].P,
1573                                                                             currentPidProfile->pid[PID_LEVEL].I,
1574                                                                             currentPidProfile->pid[PID_LEVEL].D);
1575         BLACKBOX_PRINT_HEADER_LINE("magPID", "%d",                          currentPidProfile->pid[PID_MAG].P);
1576 #ifdef USE_D_MAX
1577         BLACKBOX_PRINT_HEADER_LINE("d_max", "%d,%d,%d",                     currentPidProfile->d_max[ROLL],
1578                                                                             currentPidProfile->d_max[PITCH],
1579                                                                             currentPidProfile->d_max[YAW]);
1580         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_D_MAX_GAIN, "%d",             currentPidProfile->d_max_gain);
1581         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_D_MAX_ADVANCE, "%d",          currentPidProfile->d_max_advance);
1582 #endif
1583         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_LPF1_TYPE, "%d",        currentPidProfile->dterm_lpf1_type);
1584         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_LPF1_STATIC_HZ, "%d",   currentPidProfile->dterm_lpf1_static_hz);
1585 #ifdef USE_DYN_LPF
1586         BLACKBOX_PRINT_HEADER_LINE("dterm_lpf1_dyn_hz", "%d,%d",            currentPidProfile->dterm_lpf1_dyn_min_hz,
1587                                                                             currentPidProfile->dterm_lpf1_dyn_max_hz);
1588         BLACKBOX_PRINT_HEADER_LINE("dterm_lpf1_dyn_expo", "%d",             currentPidProfile->dterm_lpf1_dyn_expo);
1589 #endif
1590         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_LPF2_TYPE, "%d",        currentPidProfile->dterm_lpf2_type);
1591         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_LPF2_STATIC_HZ, "%d",   currentPidProfile->dterm_lpf2_static_hz);
1592         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_YAW_LOWPASS_HZ, "%d",         currentPidProfile->yaw_lowpass_hz);
1593         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_NOTCH_HZ, "%d",         currentPidProfile->dterm_notch_hz);
1594         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_NOTCH_CUTOFF, "%d",     currentPidProfile->dterm_notch_cutoff);
1595         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_WINDUP, "%d",           currentPidProfile->itermWindup);
1596 #if defined(USE_ITERM_RELAX)
1597         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_RELAX, "%d",            currentPidProfile->iterm_relax);
1598         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_RELAX_TYPE, "%d",       currentPidProfile->iterm_relax_type);
1599         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ITERM_RELAX_CUTOFF, "%d",     currentPidProfile->iterm_relax_cutoff);
1600 #endif
1601         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PID_AT_MIN_THROTTLE, "%d",    currentPidProfile->pidAtMinThrottle);
1602
1603         // Betaflight PID controller parameters
1604         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_GAIN, "%d",      currentPidProfile->anti_gravity_gain);
1605         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_CUTOFF_HZ, "%d",    currentPidProfile->anti_gravity_cutoff_hz);
1606         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANTI_GRAVITY_P_GAIN, "%d",    currentPidProfile->anti_gravity_p_gain);
1607
1608 #ifdef USE_ABSOLUTE_CONTROL
1609         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ABS_CONTROL_GAIN, "%d",       currentPidProfile->abs_control_gain);
1610 #endif
1611 #ifdef USE_INTEGRATED_YAW_CONTROL
1612         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_USE_INTEGRATED_YAW, "%d",     currentPidProfile->use_integrated_yaw);
1613 #endif
1614         BLACKBOX_PRINT_HEADER_LINE("ff_weight", "%d,%d,%d",                 currentPidProfile->pid[PID_ROLL].F,
1615                                                                             currentPidProfile->pid[PID_PITCH].F,
1616                                                                             currentPidProfile->pid[PID_YAW].F);
1617 #ifdef USE_FEEDFORWARD
1618         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_TRANSITION, "%d",     currentPidProfile->feedforward_transition);
1619         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_AVERAGING, "%d",      currentPidProfile->feedforward_averaging);
1620         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_SMOOTH_FACTOR, "%d",  currentPidProfile->feedforward_smooth_factor);
1621         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_JITTER_FACTOR, "%d",  currentPidProfile->feedforward_jitter_factor);
1622         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_BOOST, "%d",          currentPidProfile->feedforward_boost);
1623         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_MAX_RATE_LIMIT, "%d", currentPidProfile->feedforward_max_rate_limit);
1624         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FEEDFORWARD, "%d",          currentPidProfile->pid[PID_LEVEL].F);
1625         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_FF_SMOOTHING_MS, "%d",      currentPidProfile->angle_feedforward_smoothing_ms);
1626         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_YAW_HOLD_GAIN, "%d",  currentPidProfile->feedforward_yaw_hold_gain);
1627         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_FEEDFORWARD_YAW_HOLD_TIME, "%d",  currentPidProfile->feedforward_yaw_hold_time);
1628 #endif
1629         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_LIMIT, "%d",                currentPidProfile->angle_limit);
1630         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_EARTH_REF, "%d",            currentPidProfile->angle_earth_ref);
1631         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HORIZON_LIMIT_DEGREES, "%d",      currentPidProfile->horizon_limit_degrees);
1632         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HORIZON_DELAY_MS, "%d",           currentPidProfile->horizon_delay_ms);
1633
1634         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT_YAW, "%d",          currentPidProfile->yawRateAccelLimit);
1635         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LIMIT, "%d",              currentPidProfile->rateAccelLimit);
1636         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PIDSUM_LIMIT, "%d",           currentPidProfile->pidSumLimit);
1637         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_PIDSUM_LIMIT_YAW, "%d",       currentPidProfile->pidSumLimitYaw);
1638
1639 #ifdef USE_WING
1640         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_S_ROLL,  "%d", currentPidProfile->pid[PID_ROLL].S);
1641         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_S_PITCH, "%d", currentPidProfile->pid[PID_PITCH].S);
1642         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_S_YAW,   "%d", currentPidProfile->pid[PID_YAW].S);
1643 #endif // USE_WING
1644
1645         // End of Betaflight controller parameters
1646
1647         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEADBAND, "%d",               rcControlsConfig()->deadband);
1648         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_YAW_DEADBAND, "%d",           rcControlsConfig()->yaw_deadband);
1649
1650         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_HARDWARE_LPF, "%d",      gyroConfig()->gyro_hardware_lpf);
1651         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF1_TYPE, "%d",         gyroConfig()->gyro_lpf1_type);
1652         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF1_STATIC_HZ, "%d",    gyroConfig()->gyro_lpf1_static_hz);
1653 #ifdef USE_DYN_LPF
1654         BLACKBOX_PRINT_HEADER_LINE("gyro_lpf1_dyn_hz", "%d,%d",             gyroConfig()->gyro_lpf1_dyn_min_hz,
1655                                                                             gyroConfig()->gyro_lpf1_dyn_max_hz);
1656         BLACKBOX_PRINT_HEADER_LINE("gyro_lpf1_dyn_expo", "%d",              gyroConfig()->gyro_lpf1_dyn_expo);
1657 #endif
1658         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF2_TYPE, "%d",         gyroConfig()->gyro_lpf2_type);
1659         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF2_STATIC_HZ, "%d",    gyroConfig()->gyro_lpf2_static_hz);
1660         BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d",                gyroConfig()->gyro_soft_notch_hz_1,
1661                                                                             gyroConfig()->gyro_soft_notch_hz_2);
1662         BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d",            gyroConfig()->gyro_soft_notch_cutoff_1,
1663                                                                             gyroConfig()->gyro_soft_notch_cutoff_2);
1664         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_TO_USE, "%d",            gyroConfig()->gyro_to_use);
1665         BLACKBOX_PRINT_HEADER_LINE("gyro_debug_axis", "%d",                 gyroConfig()->gyro_filter_debug_axis);
1666 #ifdef USE_DYN_NOTCH_FILTER
1667         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_MAX_HZ, "%d",       dynNotchConfig()->dyn_notch_max_hz);
1668         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_COUNT, "%d",        dynNotchConfig()->dyn_notch_count);
1669         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_Q, "%d",            dynNotchConfig()->dyn_notch_q);
1670         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_MIN_HZ, "%d",       dynNotchConfig()->dyn_notch_min_hz);
1671 #endif
1672 #ifdef USE_DSHOT_TELEMETRY
1673         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DSHOT_BIDIR, "%d",            useDshotTelemetry);
1674         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_POLES, "%d",            motorConfig()->motorPoleCount);
1675 #endif
1676 #ifdef USE_RPM_FILTER
1677         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_HARMONICS, "%d",     rpmFilterConfig()->rpm_filter_harmonics);
1678         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_WEIGHTS, "%d,%d,%d", rpmFilterConfig()->rpm_filter_weights[0],
1679                                                                               rpmFilterConfig()->rpm_filter_weights[1],
1680                                                                               rpmFilterConfig()->rpm_filter_weights[2]);
1681         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_Q, "%d",             rpmFilterConfig()->rpm_filter_q);
1682         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_MIN_HZ, "%d",        rpmFilterConfig()->rpm_filter_min_hz);
1683         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, "%d", rpmFilterConfig()->rpm_filter_fade_range_hz);
1684         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_LPF_HZ, "%d",        rpmFilterConfig()->rpm_filter_lpf_hz);
1685 #endif
1686 #if defined(USE_ACC)
1687         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_LPF_HZ, "%d",        (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
1688         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ACC_HARDWARE, "%d",            accelerometerConfig()->acc_hardware);
1689 #endif
1690 #ifdef USE_BARO
1691         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d",        barometerConfig()->baro_hardware);
1692 #endif
1693         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_SOURCE, "%d",      positionConfig()->altitude_source);
1694         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_PREFER_BARO, "%d", positionConfig()->altitude_prefer_baro);
1695         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_LPF, "%d",         positionConfig()->altitude_lpf);
1696         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D_LPF, "%d",       positionConfig()->altitude_d_lpf);
1697
1698         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_LANDING_ALTITUDE, "%d",     apConfig()->landing_altitude_m);
1699         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_HOVER_THROTTLE, "%d",       apConfig()->hover_throttle);
1700         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MIN, "%d",         apConfig()->throttle_min);
1701         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_MAX, "%d",         apConfig()->throttle_max);
1702         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_P, "%d",           apConfig()->altitude_P);
1703         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_I, "%d",           apConfig()->altitude_I);
1704         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_D, "%d",           apConfig()->altitude_D);
1705         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALTITUDE_F, "%d",           apConfig()->altitude_F);
1706         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_P, "%d",           apConfig()->position_P);
1707         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_I, "%d",           apConfig()->position_I);
1708         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_D, "%d",           apConfig()->position_D);
1709         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_A, "%d",           apConfig()->position_A);
1710         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_CUTOFF, "%d",      apConfig()->position_cutoff);
1711         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_AP_MAX_ANGLE, "%d",         apConfig()->max_angle);
1712
1713 #ifdef USE_MAG
1714         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d",           compassConfig()->mag_hardware);
1715 #endif
1716
1717         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_CAL_ON_FIRST_ARM, "%d",  armingConfig()->gyro_cal_on_first_arm);
1718         BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d",       rxConfig()->airModeActivateThreshold);
1719         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SERIAL_RX_PROVIDER, "%d",     rxConfig()->serialrx_provider);
1720         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_USE_UNSYNCED_PWM, "%d",       motorConfig()->dev.useUnsyncedPwm);
1721         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_PROTOCOL, "%d",     motorConfig()->dev.motorPwmProtocol);
1722         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_RATE, "%d",         motorConfig()->dev.motorPwmRate);
1723         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_IDLE, "%d",             motorConfig()->motorIdle);
1724         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEBUG_MODE, "%d",             debugMode);
1725         BLACKBOX_PRINT_HEADER_LINE("features", "%d",                        featureConfig()->enabledFeatures);
1726
1727 #ifdef USE_RC_SMOOTHING_FILTER
1728         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING, "%d",                      rxConfig()->rc_smoothing_mode);
1729         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR, "%d",          rxConfig()->rc_smoothing_auto_factor_rpy);
1730         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE, "%d", rxConfig()->rc_smoothing_auto_factor_throttle);
1731
1732         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_FEEDFORWARD_CUTOFF, "%d", rcSmoothingData->feedforwardCutoffSetting);
1733         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_SETPOINT_CUTOFF, "%d",    rcSmoothingData->setpointCutoffSetting);
1734         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_THROTTLE_CUTOFF, "%d",    rcSmoothingData->throttleCutoffSetting);
1735
1736         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_DEBUG_AXIS, "%d",         rcSmoothingData->debugAxis);
1737         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RC_SMOOTHING_ACTIVE_CUTOFFS, "%d,%d,%d", rcSmoothingData->feedforwardCutoffFrequency,
1738                                                                             rcSmoothingData->setpointCutoffFrequency,
1739                                                                             rcSmoothingData->throttleCutoffFrequency);
1740         BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_smoothed", "%d",        lrintf(rcSmoothingData->smoothedRxRateHz));
1741 #endif // USE_RC_SMOOTHING_FILTER
1742         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RATES_TYPE, "%d",             currentControlRateProfile->rates_type);
1743
1744         BLACKBOX_PRINT_HEADER_LINE("fields_disabled_mask", "%d",            blackboxConfig()->fields_disabled_mask);
1745         BLACKBOX_PRINT_HEADER_LINE("blackbox_high_resolution", "%d",        blackboxConfig()->high_resolution);
1746
1747 #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
1748         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_VBAT_SAG_COMPENSATION, "%d",   currentPidProfile->vbat_sag_compensation);
1749 #endif
1750
1751 #if defined(USE_DYN_IDLE)
1752         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_MIN_RPM, "%d",        currentPidProfile->dyn_idle_min_rpm);
1753         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_P_GAIN, "%d",         currentPidProfile->dyn_idle_p_gain);
1754         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_I_GAIN, "%d",         currentPidProfile->dyn_idle_i_gain);
1755         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_D_GAIN, "%d",         currentPidProfile->dyn_idle_d_gain);
1756         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_IDLE_MAX_INCREASE, "%d",   currentPidProfile->dyn_idle_max_increase);
1757 #endif
1758
1759 #ifdef USE_SIMPLIFIED_TUNING
1760         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PIDS_MODE, "%d",            currentPidProfile->simplified_pids_mode);
1761         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_MASTER_MULTIPLIER, "%d",    currentPidProfile->simplified_master_multiplier);
1762         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_I_GAIN, "%d",               currentPidProfile->simplified_i_gain);
1763         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_D_GAIN, "%d",               currentPidProfile->simplified_d_gain);
1764         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PI_GAIN, "%d",              currentPidProfile->simplified_pi_gain);
1765         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_D_MAX_GAIN, "%d",            currentPidProfile->simplified_d_max_gain);
1766         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_FEEDFORWARD_GAIN, "%d",     currentPidProfile->simplified_feedforward_gain);
1767         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PITCH_D_GAIN, "%d",         currentPidProfile->simplified_roll_pitch_ratio);
1768         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_PITCH_PI_GAIN, "%d",        currentPidProfile->simplified_pitch_pi_gain);
1769         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_DTERM_FILTER, "%d",         currentPidProfile->simplified_dterm_filter);
1770         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_DTERM_FILTER_MULTIPLIER, "%d", currentPidProfile->simplified_dterm_filter_multiplier);
1771         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_GYRO_FILTER, "%d",             gyroConfig()->simplified_gyro_filter);
1772         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SIMPLIFIED_GYRO_FILTER_MULTIPLIER, "%d",  gyroConfig()->simplified_gyro_filter_multiplier);
1773 #endif
1774
1775         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_OUTPUT_LIMIT, "%d",        currentPidProfile->motor_output_limit);
1776         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_LIMIT_TYPE, "%d",       currentControlRateProfile->throttle_limit_type);
1777         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_LIMIT_PERCENT, "%d",    currentControlRateProfile->throttle_limit_percent);
1778         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_BOOST, "%d",            currentPidProfile->throttle_boost);
1779         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THROTTLE_BOOST_CUTOFF, "%d",     currentPidProfile->throttle_boost_cutoff);
1780         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_THRUST_LINEARIZATION,  "%d",     currentPidProfile->thrustLinearization);
1781
1782 #ifdef USE_GPS
1783         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_PROVIDER, "%d",               gpsConfig()->provider)
1784         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_SET_HOME_POINT_ONCE, "%d",    gpsConfig()->gps_set_home_point_once)
1785         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d",           gpsConfig()->gps_use_3d_speed)
1786
1787 #ifdef USE_GPS_RESCUE
1788         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d",  gpsRescueConfig()->minStartDistM)
1789         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d",        gpsRescueConfig()->altitudeMode)
1790         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d",   gpsRescueConfig()->initialClimbM)
1791         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ASCEND_RATE, "%d",     gpsRescueConfig()->ascendRate)
1792
1793         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d",      gpsRescueConfig()->returnAltitudeM)
1794         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_GROUND_SPEED, "%d",    gpsRescueConfig()->groundSpeedCmS)
1795         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle)
1796         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d",        gpsRescueConfig()->rollMix)
1797         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d",    gpsRescueConfig()->pitchCutoffHz)
1798         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_IMU_YAW_GAIN, "%d",    gpsRescueConfig()->imuYawGain)
1799
1800         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d",    gpsRescueConfig()->descentDistanceM)
1801         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d",    gpsRescueConfig()->descendRate)
1802         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold)
1803
1804         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_SANITY_CHECKS, "%d",   gpsRescueConfig()->sanityChecks)
1805         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_SATS, "%d",        gpsRescueConfig()->minSats)
1806         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALLOW_ARMING_WITHOUT_FIX, "%d", gpsRescueConfig()->allowArmingWithoutFix)
1807
1808         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_P, "%d",      gpsRescueConfig()->velP)
1809         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_I, "%d",      gpsRescueConfig()->velI)
1810         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_VELOCITY_D, "%d",      gpsRescueConfig()->velD)
1811         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_YAW_P, "%d",           gpsRescueConfig()->yawP)
1812
1813 #ifdef USE_MAG
1814         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_USE_MAG, "%d",         gpsRescueConfig()->useMag)
1815 #endif // USE_MAG
1816 #endif // USE_GPS_RESCUE
1817 #endif // USE_GPS
1818
1819 #ifdef USE_ALTITUDE_HOLD
1820         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_THROTTLE_RESPONSE, "%d", altHoldConfig()->alt_hold_adjust_rate);
1821         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_DEADBAND,    "%d", altHoldConfig()->alt_hold_deadband);
1822 #endif
1823
1824 #ifdef USE_POSITION_HOLD
1825         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_WITHOUT_MAG, "%d", posHoldConfig()->pos_hold_without_mag);
1826         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_DEADBAND,    "%d", posHoldConfig()->pos_hold_deadband);
1827 #endif
1828
1829 #ifdef USE_WING
1830         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_TYPE, "%d", currentPidProfile->tpa_speed_type);
1831         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_BASIC_DELAY, "%d", currentPidProfile->tpa_speed_basic_delay);
1832         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_BASIC_GRAVITY, "%d", currentPidProfile->tpa_speed_basic_gravity);
1833         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_PROP_PITCH, "%d", currentPidProfile->tpa_speed_adv_prop_pitch);
1834         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_MASS, "%d", currentPidProfile->tpa_speed_adv_mass);
1835         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_DRAG_K, "%d", currentPidProfile->tpa_speed_adv_drag_k);
1836         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_ADV_THRUST, "%d", currentPidProfile->tpa_speed_adv_thrust);
1837         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_MAX_VOLTAGE, "%d", currentPidProfile->tpa_speed_max_voltage);
1838         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_SPEED_PITCH_OFFSET, "%d", currentPidProfile->tpa_speed_pitch_offset);
1839         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_YAW_TYPE, "%d", currentPidProfile->yaw_type);
1840         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ANGLE_PITCH_OFFSET, "%d", currentPidProfile->angle_pitch_offset);
1841 #endif // USE_WING
1842
1843         default:
1844             return true;
1845     }
1846
1847     xmitState.headerIndex++;
1848 #endif // UNIT_TEST
1849     return false;
1850 }
1851
1852 /**
1853  * Write the given event to the log immediately
1854  */
1855 void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
1856 {
1857     // Only allow events to be logged after headers have been written
1858     if (!(blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_PAUSED)) {
1859         return;
1860     }
1861
1862     //Shared header for event frames
1863     blackboxWrite('E');
1864     blackboxWrite(event);
1865
1866     //Now serialize the data for this specific frame type
1867     switch (event) {
1868     case FLIGHT_LOG_EVENT_SYNC_BEEP:
1869         blackboxWriteUnsignedVB(data->syncBeep.time);
1870         break;
1871     case FLIGHT_LOG_EVENT_FLIGHTMODE: // New flightmode flags write
1872         blackboxWriteUnsignedVB(data->flightMode.flags);
1873         blackboxWriteUnsignedVB(data->flightMode.lastFlags);
1874         break;
1875     case FLIGHT_LOG_EVENT_DISARM:
1876         blackboxWriteUnsignedVB(data->disarm.reason);
1877         break;
1878     case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT:
1879         if (data->inflightAdjustment.floatFlag) {
1880             blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG);
1881             blackboxWriteFloat(data->inflightAdjustment.newFloatValue);
1882         } else {
1883             blackboxWrite(data->inflightAdjustment.adjustmentFunction);
1884             blackboxWriteSignedVB(data->inflightAdjustment.newValue);
1885         }
1886         break;
1887     case FLIGHT_LOG_EVENT_LOGGING_RESUME:
1888         blackboxWriteUnsignedVB(data->loggingResume.logIteration);
1889         blackboxWriteUnsignedVB(data->loggingResume.currentTime);
1890         break;
1891     case FLIGHT_LOG_EVENT_LOG_END:
1892         blackboxWriteString("End of log");
1893         blackboxWrite(0);
1894         break;
1895     default:
1896         break;
1897     }
1898 }
1899
1900 /* 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 */
1901 static void blackboxCheckAndLogArmingBeep(void)
1902 {
1903     // Use != so that we can still detect a change if the counter wraps
1904     if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) {
1905         blackboxLastArmingBeep = getArmingBeepTimeMicros();
1906         flightLogEvent_syncBeep_t eventData;
1907         eventData.time = blackboxLastArmingBeep;
1908         blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *)&eventData);
1909     }
1910 }
1911
1912 /* monitor the flight mode event status and trigger an event record if the state changes */
1913 static void blackboxCheckAndLogFlightMode(void)
1914 {
1915     // Use != so that we can still detect a change if the counter wraps
1916     if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) {
1917         flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags
1918         eventData.lastFlags = blackboxLastFlightModeFlags;
1919         memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags));
1920         memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags));
1921         blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData);
1922     }
1923 }
1924
1925 STATIC_UNIT_TESTED bool blackboxShouldLogPFrame(void)
1926 {
1927     return blackboxPFrameIndex == 0 && blackboxPInterval != 0;
1928 }
1929
1930 STATIC_UNIT_TESTED bool blackboxShouldLogIFrame(void)
1931 {
1932     return blackboxLoopIndex == 0;
1933 }
1934
1935 /*
1936  * If the GPS home point has been updated, or every 128 I-frames (~10 seconds), write the
1937  * GPS home position.
1938  *
1939  * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
1940  * still be interpreted correctly.
1941  */
1942 #ifdef USE_GPS
1943 STATIC_UNIT_TESTED bool blackboxShouldLogGpsHomeFrame(void)
1944 {
1945     if ((GPS_home_llh.lat != gpsHistory.GPS_home.lat
1946          || GPS_home_llh.lon != gpsHistory.GPS_home.lon
1947          || (blackboxPFrameIndex == blackboxIInterval / 2 && blackboxIFrameIndex % 128 == 0))
1948         && isFieldEnabled(FIELD_SELECT(GPS))) {
1949         return true;
1950     }
1951     return false;
1952 }
1953 #endif // GPS
1954
1955 // Called once every FC loop in order to keep track of how many FC loop iterations have passed
1956 STATIC_UNIT_TESTED void blackboxAdvanceIterationTimers(void)
1957 {
1958     ++blackboxSlowFrameIterationTimer;
1959     ++blackboxIteration;
1960
1961     if (++blackboxLoopIndex >= blackboxIInterval) {
1962         blackboxLoopIndex = 0;
1963         blackboxIFrameIndex++;
1964         blackboxPFrameIndex = 0;
1965     } else if (++blackboxPFrameIndex >= blackboxPInterval) {
1966         blackboxPFrameIndex = 0;
1967     }
1968 }
1969
1970 // Called once every FC loop in order to log the current state
1971 STATIC_UNIT_TESTED void blackboxLogIteration(timeUs_t currentTimeUs)
1972 {
1973     // Write a keyframe every blackboxIInterval frames so we can resynchronise upon missing frames
1974     if (blackboxShouldLogIFrame()) {
1975         /*
1976          * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
1977          * an additional item to write at the same time). Unless we're *only* logging "I" frames, then we have no choice.
1978          */
1979         if (blackboxIsOnlyLoggingIntraframes()) {
1980             writeSlowFrameIfNeeded();
1981         }
1982
1983         loadMainState(currentTimeUs);
1984         writeIntraframe();
1985     } else {
1986         blackboxCheckAndLogArmingBeep();
1987         blackboxCheckAndLogFlightMode(); // Check for FlightMode status change event
1988
1989         if (blackboxShouldLogPFrame()) {
1990             /*
1991              * We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
1992              * So only log slow frames during loop iterations where we log a main frame.
1993              */
1994             writeSlowFrameIfNeeded();
1995
1996             loadMainState(currentTimeUs);
1997             writeInterframe();
1998         }
1999 #ifdef USE_GPS
2000         if (featureIsEnabled(FEATURE_GPS) && isFieldEnabled(FIELD_SELECT(GPS))) {
2001             if (blackboxShouldLogGpsHomeFrame()) {
2002                 writeGPSHomeFrame();
2003                 writeGPSFrame(currentTimeUs);
2004             } else if (gpsSol.numSat != gpsHistory.GPS_numSat
2005                        || gpsSol.llh.lat != gpsHistory.GPS_coord.lat
2006                        || gpsSol.llh.lon != gpsHistory.GPS_coord.lon) {
2007                 //We could check for velocity changes as well but I doubt it changes independent of position
2008                 writeGPSFrame(currentTimeUs);
2009             }
2010         }
2011 #endif
2012     }
2013
2014     //Flush every iteration so that our runtime variance is minimized
2015     blackboxDeviceFlush();
2016 }
2017
2018 /**
2019  * Call each flight loop iteration to perform blackbox logging.
2020  */
2021 void blackboxUpdate(timeUs_t currentTimeUs)
2022 {
2023     static BlackboxState cacheFlushNextState;
2024
2025     switch (blackboxState) {
2026     case BLACKBOX_STATE_STOPPED:
2027         if (ARMING_FLAG(ARMED)) {
2028             blackboxOpen();
2029             blackboxStart();
2030         }
2031 #ifdef USE_FLASHFS
2032         if (IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) {
2033             blackboxSetState(BLACKBOX_STATE_START_ERASE);
2034         }
2035 #endif
2036         break;
2037     case BLACKBOX_STATE_PREPARE_LOG_FILE:
2038         if (blackboxDeviceBeginLog()) {
2039             blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
2040         }
2041         break;
2042     case BLACKBOX_STATE_SEND_HEADER:
2043         blackboxReplenishHeaderBudget();
2044         //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
2045
2046         /*
2047          * Once the UART has had time to init, transmit the header in chunks so we don't overflow its transmit
2048          * buffer, overflow the OpenLog's buffer, or keep the main loop busy for too long.
2049          */
2050         if (millis() > xmitState.u.startTime + 100) {
2051             if (blackboxDeviceReserveBufferSpace(BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION) == BLACKBOX_RESERVE_SUCCESS) {
2052                 for (int i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
2053                     blackboxWrite(blackboxHeader[xmitState.headerIndex]);
2054                     blackboxHeaderBudget--;
2055                 }
2056                 if (blackboxHeader[xmitState.headerIndex] == '\0') {
2057                     blackboxSetState(BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER);
2058                 }
2059             }
2060         }
2061         break;
2062     case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
2063         blackboxReplenishHeaderBudget();
2064         //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
2065         if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAYLEN(blackboxMainFields),
2066                 &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
2067 #ifdef USE_GPS
2068             if (featureIsEnabled(FEATURE_GPS) && isFieldEnabled(FIELD_SELECT(GPS))) {
2069                 blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADER);
2070             } else
2071 #endif
2072                 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
2073         }
2074         break;
2075 #ifdef USE_GPS
2076     case BLACKBOX_STATE_SEND_GPS_H_HEADER:
2077         blackboxReplenishHeaderBudget();
2078         //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
2079         if (!sendFieldDefinition('H', 0, blackboxGpsHFields, blackboxGpsHFields + 1, ARRAYLEN(blackboxGpsHFields),
2080                 NULL, NULL) && isFieldEnabled(FIELD_SELECT(GPS))) {
2081             blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADER);
2082         }
2083         break;
2084     case BLACKBOX_STATE_SEND_GPS_G_HEADER:
2085         blackboxReplenishHeaderBudget();
2086         //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
2087         if (!sendFieldDefinition('G', 0, blackboxGpsGFields, blackboxGpsGFields + 1, ARRAYLEN(blackboxGpsGFields),
2088                 &blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition) && isFieldEnabled(FIELD_SELECT(GPS))) {
2089             blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
2090         }
2091         break;
2092 #endif
2093     case BLACKBOX_STATE_SEND_SLOW_HEADER:
2094         blackboxReplenishHeaderBudget();
2095         //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
2096         if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAYLEN(blackboxSlowFields),
2097                 NULL, NULL)) {
2098             cacheFlushNextState = BLACKBOX_STATE_SEND_SYSINFO;
2099             blackboxSetState(BLACKBOX_STATE_CACHE_FLUSH);
2100         }
2101         break;
2102     case BLACKBOX_STATE_SEND_SYSINFO:
2103         blackboxReplenishHeaderBudget();
2104         //On entry of this state, xmitState.headerIndex is 0
2105
2106         //Keep writing chunks of the system info headers until it returns true to signal completion
2107         if (blackboxWriteSysinfo()) {
2108             /*
2109              * Wait for header buffers to drain completely before data logging begins to ensure reliable header delivery
2110              * (overflowing circular buffers causes all data to be discarded, so the first few logged iterations
2111              * could wipe out the end of the header if we weren't careful)
2112              */
2113             cacheFlushNextState = BLACKBOX_STATE_RUNNING;
2114             blackboxSetState(BLACKBOX_STATE_CACHE_FLUSH);
2115         }
2116         break;
2117     case BLACKBOX_STATE_CACHE_FLUSH:
2118         // Flush the cache and wait until all possible entries have been written to the media
2119         if (blackboxDeviceFlushForceComplete()) {
2120             blackboxSetState(cacheFlushNextState);
2121         }
2122         break;
2123     case BLACKBOX_STATE_PAUSED:
2124         // Only allow resume to occur during an I-frame iteration, so that we have an "I" base to work from
2125         if (IS_RC_MODE_ACTIVE(BOXBLACKBOX) && blackboxShouldLogIFrame()) {
2126             // Write a log entry so the decoder is aware that our large time/iteration skip is intended
2127             flightLogEvent_loggingResume_t resume;
2128
2129             resume.logIteration = blackboxIteration;
2130             resume.currentTime = currentTimeUs;
2131
2132             blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
2133             blackboxSetState(BLACKBOX_STATE_RUNNING);
2134
2135             blackboxLogIteration(currentTimeUs);
2136         }
2137         // Keep the logging timers ticking so our log iteration continues to advance
2138         blackboxAdvanceIterationTimers();
2139         break;
2140     case BLACKBOX_STATE_RUNNING:
2141         // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
2142         // Prevent the Pausing of the log on the mode switch if in Motor Test Mode
2143         if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX) && !startedLoggingInTestMode) {
2144             blackboxSetState(BLACKBOX_STATE_PAUSED);
2145         } else {
2146             blackboxLogIteration(currentTimeUs);
2147         }
2148         blackboxAdvanceIterationTimers();
2149         break;
2150     case BLACKBOX_STATE_SHUTTING_DOWN:
2151         //On entry of this state, startTime is set
2152         /*
2153          * Wait for the log we've transmitted to make its way to the logger before we release the serial port,
2154          * since releasing the port clears the Tx buffer.
2155          *
2156          * Don't wait longer than it could possibly take if something funky happens.
2157          */
2158         if (blackboxDeviceEndLog(blackboxLoggedAnyFrames) && (millis() > xmitState.u.startTime + BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS || blackboxDeviceFlushForce())) {
2159             blackboxDeviceClose();
2160             blackboxSetState(BLACKBOX_STATE_STOPPED);
2161         }
2162         break;
2163 #ifdef USE_FLASHFS
2164     case BLACKBOX_STATE_START_ERASE:
2165         blackboxEraseAll();
2166         blackboxSetState(BLACKBOX_STATE_ERASING);
2167         beeper(BEEPER_BLACKBOX_ERASE);
2168         break;
2169     case BLACKBOX_STATE_ERASING:
2170         if (isBlackboxErased()) {
2171             //Done erasing
2172             blackboxSetState(BLACKBOX_STATE_ERASED);
2173             beeper(BEEPER_BLACKBOX_ERASE);
2174         }
2175         break;
2176     case BLACKBOX_STATE_ERASED:
2177         if (!IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) {
2178             blackboxSetState(BLACKBOX_STATE_STOPPED);
2179         }
2180     break;
2181 #endif
2182     default:
2183         break;
2184     }
2185
2186     // Did we run out of room on the device? Stop!
2187     if (isBlackboxDeviceFull()) {
2188 #ifdef USE_FLASHFS
2189         if (blackboxState != BLACKBOX_STATE_ERASING
2190             && blackboxState != BLACKBOX_STATE_START_ERASE
2191             && blackboxState != BLACKBOX_STATE_ERASED)
2192 #endif
2193         {
2194             blackboxSetState(BLACKBOX_STATE_STOPPED);
2195             // ensure we reset the test mode flag if we stop due to full memory card
2196             if (startedLoggingInTestMode) {
2197                 startedLoggingInTestMode = false;
2198             }
2199         }
2200     } else { // Only log in test mode if there is room!
2201         switch (blackboxConfig()->mode) {
2202         case BLACKBOX_MODE_MOTOR_TEST:
2203             // Handle Motor Test Mode
2204             if (inMotorTestMode()) {
2205                 if (blackboxState==BLACKBOX_STATE_STOPPED) {
2206                     startInTestMode();
2207                 }
2208             } else {
2209                 if (blackboxState!=BLACKBOX_STATE_STOPPED) {
2210                     stopInTestMode();
2211                 }
2212             }
2213
2214             break;
2215         case BLACKBOX_MODE_ALWAYS_ON:
2216             if (blackboxState==BLACKBOX_STATE_STOPPED) {
2217                 startInTestMode();
2218             }
2219
2220             break;
2221         case BLACKBOX_MODE_NORMAL:
2222         default:
2223
2224             break;
2225         }
2226     }
2227 }
2228
2229 int blackboxCalculatePDenom(int rateNum, int rateDenom)
2230 {
2231     return blackboxIInterval * rateNum / rateDenom;
2232 }
2233
2234 uint8_t blackboxGetRateDenom(void)
2235 {
2236     return blackboxPInterval;
2237
2238 }
2239
2240 uint16_t blackboxGetPRatio(void)
2241 {
2242     return blackboxIInterval / blackboxPInterval;
2243 }
2244
2245 uint8_t blackboxCalculateSampleRate(uint16_t pRatio)
2246 {
2247     return llog2(32000 / (targetPidLooptime * pRatio));
2248 }
2249
2250 /**
2251  * Call during system startup to initialize the blackbox.
2252  */
2253 void blackboxInit(void)
2254 {
2255     blackboxResetIterationTimers();
2256
2257     // an I-frame is written every 32ms
2258     // blackboxUpdate() is run in synchronisation with the PID loop
2259     // targetPidLooptime is 1000 for 1kHz loop, 500 for 2kHz loop etc, targetPidLooptime is rounded for short looptimes
2260     blackboxIInterval = (uint16_t)(32 * 1000 / targetPidLooptime);
2261
2262     blackboxPInterval = 1 << blackboxConfig()->sample_rate;
2263     if (blackboxPInterval > blackboxIInterval) {
2264         blackboxPInterval = 0; // log only I frames if logging frequency is too low
2265     }
2266
2267     if (blackboxConfig()->device) {
2268         blackboxSetState(BLACKBOX_STATE_STOPPED);
2269     } else {
2270         blackboxSetState(BLACKBOX_STATE_DISABLED);
2271     }
2272     blackboxSInterval = blackboxIInterval * 256; // S-frame is written every 256*32 = 8192ms, approx every 8 seconds
2273
2274     blackboxHighResolutionScale = blackboxConfig()->high_resolution ? 10.0f : 1.0f;
2275 }
2276 #endif