src/main/blackbox/blackbox.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <string.h>
  20 #include <math.h>
  21
  22 #include "platform.h"
  23 #include "version.h"
  24
  25 #ifdef BLACKBOX
  26
  27 #include "common/maths.h"
  28 #include "common/axis.h"
  29 #include "common/color.h"
  30 #include "common/encoding.h"
  31 #include "common/utils.h"
  32
  33 #include "drivers/gpio.h"
  34 #include "drivers/sensor.h"
  35 #include "drivers/system.h"
  36 #include "drivers/serial.h"
  37 #include "drivers/compass.h"
  38 #include "drivers/timer.h"
  39 #include "drivers/pwm_rx.h"
  40 #include "drivers/accgyro.h"
  41 #include "drivers/light_led.h"
  42
  43 #include "sensors/sensors.h"
  44 #include "sensors/boardalignment.h"
  45 #include "sensors/sonar.h"
  46 #include "sensors/compass.h"
  47 #include "sensors/acceleration.h"
  48 #include "sensors/barometer.h"
  49 #include "sensors/gyro.h"
  50 #include "sensors/battery.h"
  51
  52 #include "io/beeper.h"
  53 #include "io/display.h"
  54 #include "io/escservo.h"
  55 #include "io/rc_controls.h"
  56 #include "io/gimbal.h"
  57 #include "io/gps.h"
  58 #include "io/ledstrip.h"
  59 #include "io/serial.h"
  60 #include "io/serial_cli.h"
  61 #include "io/serial_msp.h"
  62 #include "io/statusindicator.h"
  63
  64 #include "rx/rx.h"
  65 #include "rx/msp.h"
  66
  67 #include "telemetry/telemetry.h"
  68
  69 #include "flight/mixer.h"
  70 #include "flight/failsafe.h"
  71 #include "flight/imu.h"
  72 #include "flight/navigation_rewrite.h"
  73
  74 #include "config/runtime_config.h"
  75 #include "config/config.h"
  76 #include "config/config_profile.h"
  77 #include "config/config_master.h"
  78
  79 #include "blackbox.h"
  80 #include "blackbox_io.h"
  81
  82 #define BLACKBOX_I_INTERVAL 32
  83 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
  84 #define SLOW_FRAME_INTERVAL 4096
  85
  86 #define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
  87
  88 #define STATIC_ASSERT(condition, name ) \
  89     typedef char assert_failed_ ## name [(condition) ? 1 : -1 ]
  90
  91 // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
  92
  93 #define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
  94 #define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)
  95 #define CONDITION(x) CONCAT(FLIGHT_LOG_FIELD_CONDITION_, x)
  96 #define UNSIGNED FLIGHT_LOG_FIELD_UNSIGNED
  97 #define SIGNED FLIGHT_LOG_FIELD_SIGNED
  98
  99 static const char blackboxHeader[] =
 100     "H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
 101     "H Data version:2\n"
 102     "H I interval:" STR(BLACKBOX_I_INTERVAL) "\n";
 103
 104 static const char* const blackboxFieldHeaderNames[] = {
 105     "name",
 106     "signed",
 107     "predictor",
 108     "encoding",
 109     "predictor",
 110     "encoding"
 111 };
 112
 113 /* All field definition structs should look like this (but with longer arrs): */
 114 typedef struct blackboxFieldDefinition_s {
 115     const char *name;
 116     // If the field name has a number to be included in square brackets [1] afterwards, set it here, or -1 for no brackets:
 117     int8_t fieldNameIndex;
 118
 119     // Each member of this array will be the value to print for this field for the given header index
 120     uint8_t arr[1];
 121 } blackboxFieldDefinition_t;
 122
 123 #define BLACKBOX_DELTA_FIELD_HEADER_COUNT       ARRAY_LENGTH(blackboxFieldHeaderNames)
 124 #define BLACKBOX_SIMPLE_FIELD_HEADER_COUNT      (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
 125 #define BLACKBOX_CONDITIONAL_FIELD_HEADER_COUNT (BLACKBOX_DELTA_FIELD_HEADER_COUNT - 2)
 126
 127 typedef struct blackboxSimpleFieldDefinition_s {
 128     const char *name;
 129     int8_t fieldNameIndex;
 130
 131     uint8_t isSigned;
 132     uint8_t predict;
 133     uint8_t encode;
 134 } blackboxSimpleFieldDefinition_t;
 135
 136 typedef struct blackboxConditionalFieldDefinition_s {
 137     const char *name;
 138     int8_t fieldNameIndex;
 139
 140     uint8_t isSigned;
 141     uint8_t predict;
 142     uint8_t encode;
 143     uint8_t condition; // Decide whether this field should appear in the log
 144 } blackboxConditionalFieldDefinition_t;
 145
 146 typedef struct blackboxDeltaFieldDefinition_s {
 147     const char *name;
 148     int8_t fieldNameIndex;
 149
 150     uint8_t isSigned;
 151     uint8_t Ipredict;
 152     uint8_t Iencode;
 153     uint8_t Ppredict;
 154     uint8_t Pencode;
 155     uint8_t condition; // Decide whether this field should appear in the log
 156 } blackboxDeltaFieldDefinition_t;
 157
 158 /**
 159  * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
 160  * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
 161  * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
 162  * the encoding we've promised here).
 163  */
 164 static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
 165     /* loopIteration doesn't appear in P frames since it always increments */
 166     {"loopIteration",-1, UNSIGNED, .Ipredict = PREDICT(0),     .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC),           .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)},
 167     /* Time advances pretty steadily so the P-frame prediction is a straight line */
 168     {"time",       -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(STRAIGHT_LINE), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 169     {"axisRate",    0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 170     {"axisRate",    1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 171     {"axisRate",    2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 172     {"axisP",       0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 173     {"axisP",       1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 174     {"axisP",       2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 175     /* I terms get special packed encoding in P frames: */
 176     {"axisI",       0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
 177     {"axisI",       1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
 178     {"axisI",       2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
 179     {"axisD",       0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
 180     {"axisD",       1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
 181     {"axisD",       2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
 182     /* rcCommands are encoded together as a group in P-frames: */
 183     {"rcCommand",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
 184     {"rcCommand",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
 185     {"rcCommand",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
 186     /* Throttle is always in the range [minthrottle..maxthrottle]: */
 187     {"rcCommand",   3, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),  .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
 188
 189     {"vbatLatest",    -1, UNSIGNED, .Ipredict = PREDICT(VBATREF),  .Iencode = ENCODING(NEG_14BIT),   .Ppredict = PREDICT(PREVIOUS),  .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT},
 190     {"amperageLatest",-1, UNSIGNED, .Ipredict = PREDICT(0),        .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),  .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC},
 191
 192 #ifdef MAG
 193     {"magADC",      0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
 194     {"magADC",      1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
 195     {"magADC",      2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
 196 #endif
 197 #ifdef BARO
 198     {"BaroAlt",    -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
 199 #endif
 200 #ifdef SONAR
 201     {"sonarRaw",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_SONAR},
 202 #endif
 203     {"rssi",       -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_RSSI},
 204
 205     /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
 206     {"gyroADC",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 207     {"gyroADC",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 208     {"gyroADC",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 209     {"accSmooth",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 210     {"accSmooth",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 211     {"accSmooth",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 212     {"attitude",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 213     {"attitude",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 214     {"attitude",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 215     /* 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): */
 216     {"motor",      0, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
 217     /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
 218     {"motor",      1, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_2)},
 219     {"motor",      2, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_3)},
 220     {"motor",      3, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_4)},
 221     {"motor",      4, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_5)},
 222     {"motor",      5, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_6)},
 223     {"motor",      6, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)},
 224     {"motor",      7, UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)},
 225
 226     /* Tricopter tail servo */
 227     {"servo",      5, UNSIGNED, .Ipredict = PREDICT(1500),    .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)},
 228
 229 #ifdef NAV_BLACKBOX
 230     {"navState",  -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 231     {"navFlags",  -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 232     {"navPos",     0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 233     {"navPos",     1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 234     {"navPos",     2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 235     {"navVel",     0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 236     {"navVel",     1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 237     {"navVel",     2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 238     {"navTgtVel",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 239     {"navTgtVel",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 240     {"navTgtVel",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 241     {"navTgtPos",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 242     {"navTgtPos",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 243     {"navTgtPos",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 244     {"navSurf",    0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 245     {"navTgtSurf", 0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 246     {"navDebug",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 247     {"navDebug",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 248     {"navDebug",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 249     {"navDebug",   3, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
 250 #endif
 251 };
 252
 253 #ifdef GPS
 254 // GPS position/vel frame
 255 static const blackboxConditionalFieldDefinition_t blackboxGpsGFields[] = {
 256     {"time",              -1, UNSIGNED, PREDICT(LAST_MAIN_FRAME_TIME), ENCODING(UNSIGNED_VB), CONDITION(NOT_LOGGING_EVERY_FRAME)},
 257     {"GPS_fixType",       -1, UNSIGNED, PREDICT(0),          ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
 258     {"GPS_numSat",        -1, UNSIGNED, PREDICT(0),          ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
 259     {"GPS_coord",          0, SIGNED,   PREDICT(HOME_COORD), ENCODING(SIGNED_VB),   CONDITION(ALWAYS)},
 260     {"GPS_coord",          1, SIGNED,   PREDICT(HOME_COORD), ENCODING(SIGNED_VB),   CONDITION(ALWAYS)},
 261     {"GPS_altitude",      -1, UNSIGNED, PREDICT(0),          ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
 262     {"GPS_speed",         -1, UNSIGNED, PREDICT(0),          ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
 263     {"GPS_ground_course", -1, UNSIGNED, PREDICT(0),          ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
 264     {"GPS_hdop",          -1, UNSIGNED, PREDICT(0),          ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
 265     {"GPS_eph",           -1, UNSIGNED, PREDICT(0),          ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)},
 266     {"GPS_epv",           -1, UNSIGNED, PREDICT(0),          ENCODING(UNSIGNED_VB), CONDITION(ALWAYS)}
 267 };
 268
 269 // GPS home frame
 270 static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = {
 271     {"GPS_home",           0, SIGNED,   PREDICT(0),          ENCODING(SIGNED_VB)},
 272     {"GPS_home",           1, SIGNED,   PREDICT(0),          ENCODING(SIGNED_VB)}
 273 };
 274 #endif
 275
 276 // Rarely-updated fields
 277 static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = {
 278     {"flightModeFlags",       -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
 279     {"stateFlags",            -1, UNSIGNED, PREDICT(0),      ENCODING(UNSIGNED_VB)},
 280
 281     {"failsafePhase",         -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)},
 282     {"rxSignalReceived",      -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)},
 283     {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0),      ENCODING(TAG2_3S32)}
 284 };
 285
 286 typedef enum BlackboxState {
 287     BLACKBOX_STATE_DISABLED = 0,
 288     BLACKBOX_STATE_STOPPED,
 289     BLACKBOX_STATE_SEND_HEADER,
 290     BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER,
 291     BLACKBOX_STATE_SEND_GPS_H_HEADER,
 292     BLACKBOX_STATE_SEND_GPS_G_HEADER,
 293     BLACKBOX_STATE_SEND_SLOW_HEADER,
 294     BLACKBOX_STATE_SEND_SYSINFO,
 295     BLACKBOX_STATE_PAUSED,
 296     BLACKBOX_STATE_RUNNING,
 297     BLACKBOX_STATE_SHUTTING_DOWN
 298 } BlackboxState;
 299
 300 #define BLACKBOX_FIRST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_HEADER
 301 #define BLACKBOX_LAST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_SYSINFO
 302
 303 typedef struct blackboxMainState_s {
 304     uint32_t time;
 305
 306     int32_t axisPID_P[XYZ_AXIS_COUNT], axisPID_I[XYZ_AXIS_COUNT], axisPID_D[XYZ_AXIS_COUNT], axisPID_Setpoint[XYZ_AXIS_COUNT];
 307
 308     int16_t rcCommand[4];
 309     int16_t gyroADC[XYZ_AXIS_COUNT];
 310     int16_t accADC[XYZ_AXIS_COUNT];
 311     int16_t attitude[XYZ_AXIS_COUNT];
 312     int16_t motor[MAX_SUPPORTED_MOTORS];
 313     int16_t servo[MAX_SUPPORTED_SERVOS];
 314
 315     uint16_t vbatLatest;
 316     uint16_t amperageLatest;
 317
 318 #ifdef BARO
 319     int32_t BaroAlt;
 320 #endif
 321 #ifdef MAG
 322     int16_t magADC[XYZ_AXIS_COUNT];
 323 #endif
 324 #ifdef SONAR
 325     int32_t sonarRaw;
 326 #endif
 327     uint16_t rssi;
 328 #ifdef NAV_BLACKBOX
 329     int16_t navState;
 330     uint16_t navFlags;
 331     int32_t navPos[XYZ_AXIS_COUNT];
 332     int16_t navRealVel[XYZ_AXIS_COUNT];
 333     int16_t navTargetVel[XYZ_AXIS_COUNT];
 334     int16_t navTargetPos[XYZ_AXIS_COUNT];
 335     int16_t navHeading;
 336     int16_t navTargetHeading;
 337     int16_t navSurface;
 338     int16_t navTargetSurface;
 339     int16_t navDebug[4];
 340 #endif
 341 } blackboxMainState_t;
 342
 343 typedef struct blackboxGpsState_s {
 344     int32_t GPS_home[2], GPS_coord[2];
 345     uint8_t GPS_numSat;
 346 } blackboxGpsState_t;
 347
 348 // This data is updated really infrequently:
 349 typedef struct blackboxSlowState_s {
 350     uint16_t flightModeFlags;
 351     uint8_t stateFlags;
 352     uint8_t failsafePhase;
 353     bool rxSignalReceived;
 354     bool rxFlightChannelsValid;
 355 } __attribute__((__packed__)) blackboxSlowState_t; // We pack this struct so that padding doesn't interfere with memcmp()
 356
 357 //From mixer.c:
 358 extern uint8_t motorCount;
 359
 360 //From mw.c:
 361 extern uint32_t currentTime;
 362
 363 //From rx.c:
 364 extern uint16_t rssi;
 365
 366 static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
 367
 368 static uint32_t blackboxLastArmingBeep = 0;
 369
 370 static struct {
 371     uint32_t headerIndex;
 372
 373     /* Since these fields are used during different blackbox states (never simultaneously) we can
 374      * overlap them to save on RAM
 375      */
 376     union {
 377         int fieldIndex;
 378         uint32_t startTime;
 379     } u;
 380 } xmitState;
 381
 382 // Cache for FLIGHT_LOG_FIELD_CONDITION_* test results:
 383 static uint32_t blackboxConditionCache;
 384
 385 STATIC_ASSERT((sizeof(blackboxConditionCache) * 8) >= FLIGHT_LOG_FIELD_CONDITION_NEVER, too_many_flight_log_conditions);
 386
 387 static uint32_t blackboxIteration;
 388 static uint16_t blackboxPFrameIndex, blackboxIFrameIndex;
 389 static uint16_t blackboxSlowFrameIterationTimer;
 390
 391 /*
 392  * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
 393  * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs
 394  * to encode:
 395  */
 396 static uint16_t vbatReference;
 397
 398 static blackboxGpsState_t gpsHistory;
 399 static blackboxSlowState_t slowHistory;
 400
 401 // Keep a history of length 2, plus a buffer for MW to store the new values into
 402 static blackboxMainState_t blackboxHistoryRing[3];
 403
 404 // These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
 405 static blackboxMainState_t* blackboxHistory[3];
 406
 407 static bool blackboxModeActivationConditionPresent = false;
 408
 409 static bool blackboxIsOnlyLoggingIntraframes() {
 410     return masterConfig.blackbox_rate_num == 1 && masterConfig.blackbox_rate_denom == 32;
 411 }
 412
 413 static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
 414 {
 415     switch (condition) {
 416         case FLIGHT_LOG_FIELD_CONDITION_ALWAYS:
 417             return true;
 418
 419         case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1:
 420         case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2:
 421         case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3:
 422         case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4:
 423         case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5:
 424         case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6:
 425         case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7:
 426         case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8:
 427             return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
 428
 429         case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
 430             return masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI;
 431
 432         case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
 433         case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
 434         case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
 435             return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
 436
 437         case FLIGHT_LOG_FIELD_CONDITION_MAG:
 438 #ifdef MAG
 439             return sensors(SENSOR_MAG);
 440 #else
 441             return false;
 442 #endif
 443
 444         case FLIGHT_LOG_FIELD_CONDITION_BARO:
 445 #ifdef BARO
 446             return sensors(SENSOR_BARO);
 447 #else
 448             return false;
 449 #endif
 450
 451         case FLIGHT_LOG_FIELD_CONDITION_VBAT:
 452             return feature(FEATURE_VBAT);
 453
 454         case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC:
 455             return feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
 456
 457         case FLIGHT_LOG_FIELD_CONDITION_SONAR:
 458 #ifdef SONAR
 459             return feature(FEATURE_SONAR);
 460 #else
 461             return false;
 462 #endif
 463
 464         case FLIGHT_LOG_FIELD_CONDITION_RSSI:
 465             return masterConfig.rxConfig.rssi_channel > 0 || feature(FEATURE_RSSI_ADC);
 466
 467         case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
 468             return masterConfig.blackbox_rate_num < masterConfig.blackbox_rate_denom;
 469
 470         case FLIGHT_LOG_FIELD_CONDITION_NEVER:
 471             return false;
 472         default:
 473             return false;
 474     }
 475 }
 476
 477 static void blackboxBuildConditionCache()
 478 {
 479     FlightLogFieldCondition cond;
 480
 481     blackboxConditionCache = 0;
 482
 483     for (cond = FLIGHT_LOG_FIELD_CONDITION_FIRST; cond <= FLIGHT_LOG_FIELD_CONDITION_LAST; cond++) {
 484         if (testBlackboxConditionUncached(cond)) {
 485             blackboxConditionCache |= 1 << cond;
 486         }
 487     }
 488 }
 489
 490 static bool testBlackboxCondition(FlightLogFieldCondition condition)
 491 {
 492     return (blackboxConditionCache & (1 << condition)) != 0;
 493 }
 494
 495 static void blackboxSetState(BlackboxState newState)
 496 {
 497     //Perform initial setup required for the new state
 498     switch (newState) {
 499         case BLACKBOX_STATE_SEND_HEADER:
 500             blackboxHeaderBudget = 0;
 501             xmitState.headerIndex = 0;
 502             xmitState.u.startTime = millis();
 503         break;
 504         case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
 505         case BLACKBOX_STATE_SEND_GPS_G_HEADER:
 506         case BLACKBOX_STATE_SEND_GPS_H_HEADER:
 507         case BLACKBOX_STATE_SEND_SLOW_HEADER:
 508             xmitState.headerIndex = 0;
 509             xmitState.u.fieldIndex = -1;
 510         break;
 511         case BLACKBOX_STATE_SEND_SYSINFO:
 512             xmitState.headerIndex = 0;
 513         break;
 514         case BLACKBOX_STATE_RUNNING:
 515             blackboxSlowFrameIterationTimer = SLOW_FRAME_INTERVAL; //Force a slow frame to be written on the first iteration
 516         break;
 517         case BLACKBOX_STATE_SHUTTING_DOWN:
 518             xmitState.u.startTime = millis();
 519             blackboxDeviceFlush();
 520         break;
 521         default:
 522             ;
 523     }
 524     blackboxState = newState;
 525 }
 526
 527 static void writeIntraframe(void)
 528 {
 529     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
 530     int x;
 531
 532     blackboxWrite('I');
 533
 534     blackboxWriteUnsignedVB(blackboxIteration);
 535     blackboxWriteUnsignedVB(blackboxCurrent->time);
 536
 537     blackboxWriteSignedVBArray(blackboxCurrent->axisPID_Setpoint, XYZ_AXIS_COUNT);
 538     blackboxWriteSignedVBArray(blackboxCurrent->axisPID_P, XYZ_AXIS_COUNT);
 539     blackboxWriteSignedVBArray(blackboxCurrent->axisPID_I, XYZ_AXIS_COUNT);
 540
 541     // Don't bother writing the current D term if the corresponding PID setting is zero
 542     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
 543         if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) {
 544             blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]);
 545         }
 546     }
 547
 548     // Write roll, pitch and yaw first:
 549     blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3);
 550
 551     /*
 552      * Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
 553      * Throttle lies in range [minthrottle..maxthrottle]:
 554      */
 555     blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - masterConfig.escAndServoConfig.minthrottle);
 556
 557     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
 558         /*
 559          * Our voltage is expected to decrease over the course of the flight, so store our difference from
 560          * the reference:
 561          *
 562          * Write 14 bits even if the number is negative (which would otherwise result in 32 bits)
 563          */
 564         blackboxWriteUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF);
 565     }
 566
 567     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC)) {
 568         // 12bit value directly from ADC
 569         blackboxWriteUnsignedVB(blackboxCurrent->amperageLatest);
 570     }
 571
 572 #ifdef MAG
 573         if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
 574             blackboxWriteSigned16VBArray(blackboxCurrent->magADC, XYZ_AXIS_COUNT);
 575         }
 576 #endif
 577
 578 #ifdef BARO
 579         if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) {
 580             blackboxWriteSignedVB(blackboxCurrent->BaroAlt);
 581         }
 582 #endif
 583
 584 #ifdef SONAR
 585         if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
 586             blackboxWriteSignedVB(blackboxCurrent->sonarRaw);
 587         }
 588 #endif
 589
 590     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RSSI)) {
 591         blackboxWriteUnsignedVB(blackboxCurrent->rssi);
 592     }
 593
 594     blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
 595     blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
 596     blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT);
 597
 598     //Motors can be below minthrottle when disarmed, but that doesn't happen much
 599     blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle);
 600
 601     //Motors tend to be similar to each other so use the first motor's value as a predictor of the others
 602     for (x = 1; x < motorCount; x++) {
 603         blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
 604     }
 605
 606     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) {
 607         //Assume the tail spends most of its time around the center
 608         blackboxWriteSignedVB(blackboxCurrent->servo[5] - 1500);
 609     }
 610
 611 #ifdef NAV_BLACKBOX
 612     blackboxWriteSignedVB(blackboxCurrent->navState);
 613
 614     blackboxWriteSignedVB(blackboxCurrent->navFlags);
 615
 616     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
 617         blackboxWriteSignedVB(blackboxCurrent->navPos[x]);
 618     }
 619
 620     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
 621         blackboxWriteSignedVB(blackboxCurrent->navRealVel[x]);
 622     }
 623
 624     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
 625         blackboxWriteSignedVB(blackboxCurrent->navTargetVel[x]);
 626     }
 627
 628     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
 629         blackboxWriteSignedVB(blackboxCurrent->navTargetPos[x]);
 630     }
 631
 632     blackboxWriteSignedVB(blackboxCurrent->navSurface);
 633     blackboxWriteSignedVB(blackboxCurrent->navTargetSurface);
 634
 635     for (x = 0; x < 4; x++) {
 636         blackboxWriteSignedVB(blackboxCurrent->navDebug[x]);
 637     }
 638 #endif
 639
 640     //Rotate our history buffers:
 641
 642     //The current state becomes the new "before" state
 643     blackboxHistory[1] = blackboxHistory[0];
 644     //And since we have no other history, we also use it for the "before, before" state
 645     blackboxHistory[2] = blackboxHistory[0];
 646     //And advance the current state over to a blank space ready to be filled
 647     blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
 648 }
 649
 650 static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHistory, int count)
 651 {
 652     int16_t *curr  = (int16_t*) ((char*) (blackboxHistory[0]) + arrOffsetInHistory);
 653     int16_t *prev1 = (int16_t*) ((char*) (blackboxHistory[1]) + arrOffsetInHistory);
 654     int16_t *prev2 = (int16_t*) ((char*) (blackboxHistory[2]) + arrOffsetInHistory);
 655
 656     for (int i = 0; i < count; i++) {
 657         // Predictor is the average of the previous two history states
 658         int32_t predictor = (prev1[i] + prev2[i]) / 2;
 659
 660         blackboxWriteSignedVB(curr[i] - predictor);
 661     }
 662 }
 663
 664 static void writeInterframe(void)
 665 {
 666     int x;
 667     int32_t deltas[8];
 668
 669     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
 670     blackboxMainState_t *blackboxLast = blackboxHistory[1];
 671
 672     blackboxWrite('P');
 673
 674     //No need to store iteration count since its delta is always 1
 675
 676     /*
 677      * Since the difference between the difference between successive times will be nearly zero (due to consistent
 678      * looptime spacing), use second-order differences.
 679      */
 680     blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
 681
 682     arraySubInt32(deltas, blackboxCurrent->axisPID_Setpoint, blackboxLast->axisPID_Setpoint, XYZ_AXIS_COUNT);
 683     blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
 684
 685     arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
 686     blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
 687
 688     /*
 689      * The PID I field changes very slowly, most of the time +-2, so use an encoding
 690      * that can pack all three fields into one byte in that situation.
 691      */
 692     arraySubInt32(deltas, blackboxCurrent->axisPID_I, blackboxLast->axisPID_I, XYZ_AXIS_COUNT);
 693     blackboxWriteTag2_3S32(deltas);
 694
 695     /*
 696      * The PID D term is frequently set to zero for yaw, which makes the result from the calculation
 697      * always zero. So don't bother recording D results when PID D terms are zero.
 698      */
 699     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
 700         if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) {
 701             blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]);
 702         }
 703     }
 704
 705     /*
 706      * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
 707      * can pack multiple values per byte:
 708      */
 709     for (x = 0; x < 4; x++) {
 710         deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x];
 711     }
 712
 713     blackboxWriteTag8_4S16(deltas);
 714
 715     //Check for sensors that are updated periodically (so deltas are normally zero)
 716     int optionalFieldCount = 0;
 717
 718     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
 719         deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->vbatLatest - blackboxLast->vbatLatest;
 720     }
 721
 722     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC)) {
 723         deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->amperageLatest - blackboxLast->amperageLatest;
 724     }
 725
 726 #ifdef MAG
 727     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
 728         for (x = 0; x < XYZ_AXIS_COUNT; x++) {
 729             deltas[optionalFieldCount++] = blackboxCurrent->magADC[x] - blackboxLast->magADC[x];
 730         }
 731     }
 732 #endif
 733
 734 #ifdef BARO
 735     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO)) {
 736         deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt;
 737     }
 738 #endif
 739
 740 #ifdef SONAR
 741     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
 742         deltas[optionalFieldCount++] = blackboxCurrent->sonarRaw - blackboxLast->sonarRaw;
 743     }
 744 #endif
 745
 746     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RSSI)) {
 747         deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->rssi - blackboxLast->rssi;
 748     }
 749
 750     blackboxWriteTag8_8SVB(deltas, optionalFieldCount);
 751
 752     //Since gyros, accs and motors are noisy, base their predictions on the average of the history:
 753     blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC),   XYZ_AXIS_COUNT);
 754     blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
 755     blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
 756     blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor),     motorCount);
 757
 758     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) {
 759         blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]);
 760     }
 761
 762 #ifdef NAV_BLACKBOX
 763     blackboxWriteSignedVB(blackboxCurrent->navState - blackboxLast->navState);
 764
 765     blackboxWriteSignedVB(blackboxCurrent->navFlags - blackboxLast->navFlags);
 766
 767     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
 768         blackboxWriteSignedVB(blackboxCurrent->navPos[x] - blackboxLast->navPos[x]);
 769     }
 770
 771     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
 772         blackboxWriteSignedVB(blackboxHistory[0]->navRealVel[x] - (blackboxHistory[1]->navRealVel[x] + blackboxHistory[2]->navRealVel[x]) / 2);
 773     }
 774
 775     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
 776         blackboxWriteSignedVB(blackboxHistory[0]->navTargetVel[x] - (blackboxHistory[1]->navTargetVel[x] + blackboxHistory[2]->navTargetVel[x]) / 2);
 777     }
 778
 779     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
 780         blackboxWriteSignedVB(blackboxHistory[0]->navTargetPos[x] - (blackboxHistory[1]->navTargetPos[x] + blackboxHistory[2]->navTargetPos[x]) / 2);
 781     }
 782
 783     blackboxWriteSignedVB(blackboxCurrent->navSurface - blackboxLast->navSurface);
 784     blackboxWriteSignedVB(blackboxCurrent->navTargetSurface - blackboxLast->navTargetSurface);
 785
 786     for (x = 0; x < 4; x++) {
 787         blackboxWriteSignedVB(blackboxCurrent->navDebug[x] - blackboxLast->navDebug[x]);
 788     }
 789 #endif
 790
 791     //Rotate our history buffers
 792     blackboxHistory[2] = blackboxHistory[1];
 793     blackboxHistory[1] = blackboxHistory[0];
 794     blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
 795 }
 796
 797 /* Write the contents of the global "slowHistory" to the log as an "S" frame. Because this data is logged so
 798  * infrequently, delta updates are not reasonable, so we log independent frames. */
 799 static void writeSlowFrame(void)
 800 {
 801     int32_t values[3];
 802
 803     blackboxWrite('S');
 804
 805     blackboxWriteUnsignedVB(slowHistory.flightModeFlags);
 806     blackboxWriteUnsignedVB(slowHistory.stateFlags);
 807
 808     /*
 809      * Most of the time these three values will be able to pack into one byte for us:
 810      */
 811     values[0] = slowHistory.failsafePhase;
 812     values[1] = slowHistory.rxSignalReceived ? 1 : 0;
 813     values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0;
 814     blackboxWriteTag2_3S32(values);
 815
 816     blackboxSlowFrameIterationTimer = 0;
 817 }
 818
 819 /**
 820  * Load rarely-changing values from the FC into the given structure
 821  */
 822 static void loadSlowState(blackboxSlowState_t *slow)
 823 {
 824     slow->flightModeFlags = flightModeFlags;
 825     slow->stateFlags = stateFlags;
 826     slow->failsafePhase = failsafePhase();
 827     slow->rxSignalReceived = rxIsReceivingSignal();
 828     slow->rxFlightChannelsValid = rxAreFlightChannelsValid();
 829 }
 830
 831 /**
 832  * If the data in the slow frame has changed, log a slow frame.
 833  *
 834  * If allowPeriodicWrite is true, the frame is also logged if it has been more than SLOW_FRAME_INTERVAL logging iterations
 835  * since the field was last logged.
 836  */
 837 static void writeSlowFrameIfNeeded(bool allowPeriodicWrite)
 838 {
 839     // Write the slow frame peridocially so it can be recovered if we ever lose sync
 840     bool shouldWrite = allowPeriodicWrite && blackboxSlowFrameIterationTimer >= SLOW_FRAME_INTERVAL;
 841
 842     if (shouldWrite) {
 843         loadSlowState(&slowHistory);
 844     } else {
 845         blackboxSlowState_t newSlowState;
 846
 847         loadSlowState(&newSlowState);
 848
 849         // Only write a slow frame if it was different from the previous state
 850         if (memcmp(&newSlowState, &slowHistory, sizeof(slowHistory)) != 0) {
 851             // Use the new state as our new history
 852             memcpy(&slowHistory, &newSlowState, sizeof(slowHistory));
 853             shouldWrite = true;
 854         }
 855     }
 856
 857     if (shouldWrite) {
 858         writeSlowFrame();
 859     }
 860 }
 861
 862 static int gcd(int num, int denom)
 863 {
 864     if (denom == 0) {
 865         return num;
 866     }
 867
 868     return gcd(denom, num % denom);
 869 }
 870
 871 static void validateBlackboxConfig()
 872 {
 873     int div;
 874
 875     if (masterConfig.blackbox_rate_num == 0 || masterConfig.blackbox_rate_denom == 0
 876             || masterConfig.blackbox_rate_num >= masterConfig.blackbox_rate_denom) {
 877         masterConfig.blackbox_rate_num = 1;
 878         masterConfig.blackbox_rate_denom = 1;
 879     } else {
 880         /* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
 881          * itself more frequently)
 882          */
 883         div = gcd(masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
 884
 885         masterConfig.blackbox_rate_num /= div;
 886         masterConfig.blackbox_rate_denom /= div;
 887     }
 888
 889     if (masterConfig.blackbox_device >= BLACKBOX_DEVICE_END) {
 890         masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL;
 891     }
 892 }
 893
 894 /**
 895  * Start Blackbox logging if it is not already running. Intended to be called upon arming.
 896  */
 897 void startBlackbox(void)
 898 {
 899     if (blackboxState == BLACKBOX_STATE_STOPPED) {
 900         validateBlackboxConfig();
 901
 902         if (!blackboxDeviceOpen()) {
 903             blackboxSetState(BLACKBOX_STATE_DISABLED);
 904             return;
 905         }
 906
 907         memset(&gpsHistory, 0, sizeof(gpsHistory));
 908
 909         blackboxHistory[0] = &blackboxHistoryRing[0];
 910         blackboxHistory[1] = &blackboxHistoryRing[1];
 911         blackboxHistory[2] = &blackboxHistoryRing[2];
 912
 913         vbatReference = vbatLatestADC;
 914
 915         //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
 916
 917         /*
 918          * We use conditional tests to decide whether or not certain fields should be logged. Since our headers
 919          * must always agree with the logged data, the results of these tests must not change during logging. So
 920          * cache those now.
 921          */
 922         blackboxBuildConditionCache();
 923
 924         blackboxModeActivationConditionPresent = isModeActivationConditionPresent(currentProfile->modeActivationConditions, BOXBLACKBOX);
 925
 926         blackboxIteration = 0;
 927         blackboxPFrameIndex = 0;
 928         blackboxIFrameIndex = 0;
 929
 930         /*
 931          * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
 932          * it finally plays the beep for this arming event.
 933          */
 934         blackboxLastArmingBeep = getArmingBeepTimeMicros();
 935
 936         blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
 937     }
 938 }
 939
 940 /**
 941  * Begin Blackbox shutdown.
 942  */
 943 void finishBlackbox(void)
 944 {
 945     if (blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_PAUSED) {
 946         blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END, NULL);
 947
 948         blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN);
 949     } else if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED
 950             && blackboxState != BLACKBOX_STATE_SHUTTING_DOWN) {
 951         /*
 952          * We're shutting down in the middle of transmitting headers, so we can't log a "log completed" event.
 953          * Just give the port back and stop immediately.
 954          */
 955         blackboxDeviceClose();
 956         blackboxSetState(BLACKBOX_STATE_STOPPED);
 957     }
 958 }
 959
 960 #ifdef GPS
 961 static void writeGPSHomeFrame()
 962 {
 963     blackboxWrite('H');
 964
 965     blackboxWriteSignedVB(GPS_home.lat);
 966     blackboxWriteSignedVB(GPS_home.lon);
 967     //TODO it'd be great if we could grab the GPS current time and write that too
 968
 969     gpsHistory.GPS_home[0] = GPS_home.lat;
 970     gpsHistory.GPS_home[1] = GPS_home.lon;
 971 }
 972
 973 static void writeGPSFrame()
 974 {
 975     blackboxWrite('G');
 976
 977     /*
 978      * If we're logging every frame, then a GPS frame always appears just after a frame with the
 979      * currentTime timestamp in the log, so the reader can just use that timestamp for the GPS frame.
 980      *
 981      * If we're not logging every frame, we need to store the time of this GPS frame.
 982      */
 983     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME)) {
 984         // Predict the time of the last frame in the main log
 985         blackboxWriteUnsignedVB(currentTime - blackboxHistory[1]->time);
 986     }
 987
 988     blackboxWriteUnsignedVB(gpsSol.fixType);
 989     blackboxWriteUnsignedVB(gpsSol.numSat);
 990     blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[0]);
 991     blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[1]);
 992     blackboxWriteUnsignedVB(gpsSol.llh.alt / 100); // meters
 993     blackboxWriteUnsignedVB(gpsSol.groundSpeed);
 994     blackboxWriteUnsignedVB(gpsSol.groundCourse);
 995     blackboxWriteUnsignedVB(gpsSol.hdop);
 996     blackboxWriteUnsignedVB(gpsSol.eph);
 997     blackboxWriteUnsignedVB(gpsSol.epv);
 998
 999     gpsHistory.GPS_numSat = gpsSol.numSat;
1000     gpsHistory.GPS_coord[0] = gpsSol.llh.lat;
1001     gpsHistory.GPS_coord[1] = gpsSol.llh.lon;
1002 }
1003 #endif
1004
1005 /**
1006  * Fill the current state of the blackbox using values read from the flight controller
1007  */
1008 static void loadMainState(void)
1009 {
1010     blackboxMainState_t *blackboxCurrent = blackboxHistory[0];
1011     int i;
1012
1013     blackboxCurrent->time = currentTime;
1014
1015     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
1016         blackboxCurrent->axisPID_Setpoint[i] = axisPID_Setpoint[i];
1017     }
1018     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
1019         blackboxCurrent->axisPID_P[i] = axisPID_P[i];
1020     }
1021     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
1022         blackboxCurrent->axisPID_I[i] = axisPID_I[i];
1023     }
1024     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
1025         blackboxCurrent->axisPID_D[i] = axisPID_D[i];
1026     }
1027
1028     for (i = 0; i < 4; i++) {
1029         blackboxCurrent->rcCommand[i] = rcCommand[i];
1030     }
1031
1032     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
1033         blackboxCurrent->gyroADC[i] = gyroADC[i];
1034     }
1035
1036     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
1037         blackboxCurrent->accADC[i] = accADC[i];
1038     }
1039
1040     blackboxCurrent->attitude[0] = attitude.values.roll;
1041     blackboxCurrent->attitude[1] = attitude.values.pitch;
1042     blackboxCurrent->attitude[2] = attitude.values.yaw;
1043
1044     for (i = 0; i < motorCount; i++) {
1045         blackboxCurrent->motor[i] = motor[i];
1046     }
1047
1048     blackboxCurrent->vbatLatest = vbatLatestADC;
1049     blackboxCurrent->amperageLatest = amperageLatestADC;
1050
1051 #ifdef MAG
1052     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
1053         blackboxCurrent->magADC[i] = magADC[i];
1054     }
1055 #endif
1056
1057 #ifdef BARO
1058     blackboxCurrent->BaroAlt = BaroAlt;
1059 #endif
1060
1061 #ifdef SONAR
1062     // Store the raw sonar value without applying tilt correction
1063     blackboxCurrent->sonarRaw = sonarRead();
1064 #endif
1065
1066     blackboxCurrent->rssi = rssi;
1067
1068 #ifdef USE_SERVOS
1069     //Tail servo for tricopters
1070     blackboxCurrent->servo[5] = servo[5];
1071 #endif
1072
1073 #ifdef NAV_BLACKBOX
1074     blackboxCurrent->navState = navCurrentState;
1075     blackboxCurrent->navFlags = navFlags;
1076     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
1077         blackboxCurrent->navPos[i] = navLatestActualPosition[i];
1078         blackboxCurrent->navRealVel[i] = navActualVelocity[i];
1079         blackboxCurrent->navTargetVel[i] = navDesiredVelocity[i];
1080         blackboxCurrent->navTargetPos[i] = navTargetPosition[i];
1081     }
1082     blackboxCurrent->navSurface = navActualSurface;
1083     blackboxCurrent->navTargetSurface = navTargetSurface;
1084     for (i = 0; i < 4; i++) {
1085         blackboxCurrent->navDebug[i] = navDebug[i];
1086     }
1087 #endif
1088 }
1089
1090 /**
1091  * Transmit the header information for the given field definitions. Transmitted header lines look like:
1092  *
1093  * H Field I name:a,b,c
1094  * H Field I predictor:0,1,2
1095  *
1096  * For all header types, provide a "mainFrameChar" which is the name for the field and will be used to refer to it in the
1097  * header (e.g. P, I etc). For blackboxDeltaField_t fields, also provide deltaFrameChar, otherwise set this to zero.
1098  *
1099  * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
1100  * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
1101  *
1102  * Set xmitState.headerIndex to 0 and xmitState.u.fieldIndex to -1 before calling for the first time.
1103  *
1104  * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the
1105  * fieldDefinition and secondCondition arrays.
1106  *
1107  * Returns true if there is still header left to transmit (so call again to continue transmission).
1108  */
1109 static bool sendFieldDefinition(char mainFrameChar, char deltaFrameChar, const void *fieldDefinitions,
1110         const void *secondFieldDefinition, int fieldCount, const uint8_t *conditions, const uint8_t *secondCondition)
1111 {
1112     const blackboxFieldDefinition_t *def;
1113     unsigned int headerCount;
1114     static bool needComma = false;
1115     size_t definitionStride = (char*) secondFieldDefinition - (char*) fieldDefinitions;
1116     size_t conditionsStride = (char*) secondCondition - (char*) conditions;
1117
1118     if (deltaFrameChar) {
1119         headerCount = BLACKBOX_DELTA_FIELD_HEADER_COUNT;
1120     } else {
1121         headerCount = BLACKBOX_SIMPLE_FIELD_HEADER_COUNT;
1122     }
1123
1124     /*
1125      * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit
1126      * the whole header.
1127      */
1128
1129     // On our first call we need to print the name of the header and a colon
1130     if (xmitState.u.fieldIndex == -1) {
1131         if (xmitState.headerIndex >= headerCount) {
1132             return false; //Someone probably called us again after we had already completed transmission
1133         }
1134
1135         uint32_t charsToBeWritten = strlen("H Field x :") + strlen(blackboxFieldHeaderNames[xmitState.headerIndex]);
1136
1137         if (blackboxDeviceReserveBufferSpace(charsToBeWritten) != BLACKBOX_RESERVE_SUCCESS) {
1138             return true; // Try again later
1139         }
1140
1141         blackboxHeaderBudget -= blackboxPrintf("H Field %c %s:", xmitState.headerIndex >= BLACKBOX_SIMPLE_FIELD_HEADER_COUNT ? deltaFrameChar : mainFrameChar, blackboxFieldHeaderNames[xmitState.headerIndex]);
1142
1143         xmitState.u.fieldIndex++;
1144         needComma = false;
1145     }
1146
1147     // The longest we expect an integer to be as a string:
1148     const uint32_t LONGEST_INTEGER_STRLEN = 2;
1149
1150     for (; xmitState.u.fieldIndex < fieldCount; xmitState.u.fieldIndex++) {
1151         def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * xmitState.u.fieldIndex);
1152
1153         if (!conditions || testBlackboxCondition(conditions[conditionsStride * xmitState.u.fieldIndex])) {
1154             // First (over)estimate the length of the string we want to print
1155
1156             int32_t bytesToWrite = 1; // Leading comma
1157
1158             // The first header is a field name
1159             if (xmitState.headerIndex == 0) {
1160                 bytesToWrite += strlen(def->name) + strlen("[]") + LONGEST_INTEGER_STRLEN;
1161             } else {
1162                 //The other headers are integers
1163                 bytesToWrite += LONGEST_INTEGER_STRLEN;
1164             }
1165
1166             // Now perform the write if the buffer is large enough
1167             if (blackboxDeviceReserveBufferSpace(bytesToWrite) != BLACKBOX_RESERVE_SUCCESS) {
1168                 // Ran out of space!
1169                 return true;
1170             }
1171
1172             blackboxHeaderBudget -= bytesToWrite;
1173
1174             if (needComma) {
1175                 blackboxWrite(',');
1176             } else {
1177                 needComma = true;
1178             }
1179
1180             // The first header is a field name
1181             if (xmitState.headerIndex == 0) {
1182                 blackboxPrint(def->name);
1183
1184                 // Do we need to print an index in brackets after the name?
1185                 if (def->fieldNameIndex != -1) {
1186                     blackboxPrintf("[%d]", def->fieldNameIndex);
1187                 }
1188             } else {
1189                 //The other headers are integers
1190                 blackboxPrintf("%d", def->arr[xmitState.headerIndex - 1]);
1191             }
1192         }
1193     }
1194
1195     // Did we complete this line?
1196     if (xmitState.u.fieldIndex == fieldCount && blackboxDeviceReserveBufferSpace(1) == BLACKBOX_RESERVE_SUCCESS) {
1197         blackboxHeaderBudget--;
1198         blackboxWrite('\n');
1199         xmitState.headerIndex++;
1200         xmitState.u.fieldIndex = -1;
1201     }
1202
1203     return xmitState.headerIndex < headerCount;
1204 }
1205
1206 /**
1207  * Transmit a portion of the system information headers. Call the first time with xmitState.headerIndex == 0. Returns
1208  * true iff transmission is complete, otherwise call again later to continue transmission.
1209  */
1210 static bool blackboxWriteSysinfo()
1211 {
1212     // Make sure we have enough room in the buffer for our longest line (as of this writing, the "Firmware date" line)
1213     if (blackboxDeviceReserveBufferSpace(64) != BLACKBOX_RESERVE_SUCCESS) {
1214         return false;
1215     }
1216
1217     switch (xmitState.headerIndex) {
1218         case 0:
1219             blackboxPrintfHeaderLine("Firmware type:Cleanflight");
1220         break;
1221         case 1:
1222             blackboxPrintfHeaderLine("Firmware revision:%s", shortGitRevision);
1223         break;
1224         case 2:
1225             blackboxPrintfHeaderLine("Firmware date:%s %s", buildDate, buildTime);
1226         break;
1227         case 3:
1228             blackboxPrintfHeaderLine("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
1229         break;
1230         case 4:
1231             blackboxPrintfHeaderLine("rcRate:%d", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8);
1232         break;
1233         case 5:
1234             blackboxPrintfHeaderLine("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle);
1235         break;
1236         case 6:
1237             blackboxPrintfHeaderLine("maxthrottle:%d", masterConfig.escAndServoConfig.maxthrottle);
1238         break;
1239         case 7:
1240             blackboxPrintfHeaderLine("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale));
1241         break;
1242         case 8:
1243             blackboxPrintfHeaderLine("acc_1G:%u", acc.acc_1G);
1244         break;
1245         case 9:
1246             if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
1247                 blackboxPrintfHeaderLine("vbatscale:%u", masterConfig.batteryConfig.vbatscale);
1248             } else {
1249                 xmitState.headerIndex += 2; // Skip the next two vbat fields too
1250             }
1251         break;
1252         case 10:
1253             blackboxPrintfHeaderLine("vbatcellvoltage:%u,%u,%u", masterConfig.batteryConfig.vbatmincellvoltage,
1254                 masterConfig.batteryConfig.vbatwarningcellvoltage, masterConfig.batteryConfig.vbatmaxcellvoltage);
1255         break;
1256         case 11:
1257             blackboxPrintfHeaderLine("vbatref:%u", vbatReference);
1258         break;
1259         case 12:
1260             //Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too:
1261             if (feature(FEATURE_CURRENT_METER)) {
1262                 blackboxPrintfHeaderLine("currentMeter:%d,%d", masterConfig.batteryConfig.currentMeterOffset, masterConfig.batteryConfig.currentMeterScale);
1263             }
1264         break;
1265         default:
1266             return true;
1267     }
1268
1269     xmitState.headerIndex++;
1270     return false;
1271 }
1272
1273 /**
1274  * Write the given event to the log immediately
1275  */
1276 void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
1277 {
1278     // Only allow events to be logged after headers have been written
1279     if (!(blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_PAUSED)) {
1280         return;
1281     }
1282
1283     //Shared header for event frames
1284     blackboxWrite('E');
1285     blackboxWrite(event);
1286
1287     //Now serialize the data for this specific frame type
1288     switch (event) {
1289         case FLIGHT_LOG_EVENT_SYNC_BEEP:
1290             blackboxWriteUnsignedVB(data->syncBeep.time);
1291         break;
1292         case FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT:
1293             if (data->inflightAdjustment.floatFlag) {
1294                 blackboxWrite(data->inflightAdjustment.adjustmentFunction + FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT_FUNCTION_FLOAT_VALUE_FLAG);
1295                 blackboxWriteFloat(data->inflightAdjustment.newFloatValue);
1296             } else {
1297                 blackboxWrite(data->inflightAdjustment.adjustmentFunction);
1298                 blackboxWriteSignedVB(data->inflightAdjustment.newValue);
1299             }
1300         case FLIGHT_LOG_EVENT_LOGGING_RESUME:
1301             blackboxWriteUnsignedVB(data->loggingResume.logIteration);
1302             blackboxWriteUnsignedVB(data->loggingResume.currentTime);
1303         break;
1304         case FLIGHT_LOG_EVENT_LOG_END:
1305             blackboxPrint("End of log");
1306             blackboxWrite(0);
1307         break;
1308     }
1309 }
1310
1311 /* 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 */
1312 static void blackboxCheckAndLogArmingBeep()
1313 {
1314     flightLogEvent_syncBeep_t eventData;
1315
1316     // Use != so that we can still detect a change if the counter wraps
1317     if (getArmingBeepTimeMicros() != blackboxLastArmingBeep) {
1318         blackboxLastArmingBeep = getArmingBeepTimeMicros();
1319
1320         eventData.time = blackboxLastArmingBeep;
1321
1322         blackboxLogEvent(FLIGHT_LOG_EVENT_SYNC_BEEP, (flightLogEventData_t *) &eventData);
1323     }
1324 }
1325
1326 /*
1327  * Use the user's num/denom settings to decide if the P-frame of the given index should be logged, allowing the user to control
1328  * the portion of logged loop iterations.
1329  */
1330 static bool blackboxShouldLogPFrame(uint32_t pFrameIndex)
1331 {
1332     /* Adding a magic shift of "masterConfig.blackbox_rate_num - 1" in here creates a better spread of
1333      * recorded / skipped frames when the I frame's position is considered:
1334      */
1335     return (pFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num;
1336 }
1337
1338 static bool blackboxShouldLogIFrame() {
1339     return blackboxPFrameIndex == 0;
1340 }
1341
1342 // Called once every FC loop in order to keep track of how many FC loop iterations have passed
1343 static void blackboxAdvanceIterationTimers()
1344 {
1345     blackboxSlowFrameIterationTimer++;
1346     blackboxIteration++;
1347     blackboxPFrameIndex++;
1348
1349     if (blackboxPFrameIndex == BLACKBOX_I_INTERVAL) {
1350         blackboxPFrameIndex = 0;
1351         blackboxIFrameIndex++;
1352     }
1353 }
1354
1355 // Called once every FC loop in order to log the current state
1356 static void blackboxLogIteration()
1357 {
1358     // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
1359     if (blackboxShouldLogIFrame()) {
1360         /*
1361          * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
1362          * an additional item to write at the same time). Unless we're *only* logging "I" frames, then we have no choice.
1363          */
1364         writeSlowFrameIfNeeded(blackboxIsOnlyLoggingIntraframes());
1365
1366         loadMainState();
1367         writeIntraframe();
1368     } else {
1369         blackboxCheckAndLogArmingBeep();
1370
1371         if (blackboxShouldLogPFrame(blackboxPFrameIndex)) {
1372             /*
1373              * We assume that slow frames are only interesting in that they aid the interpretation of the main data stream.
1374              * So only log slow frames during loop iterations where we log a main frame.
1375              */
1376             writeSlowFrameIfNeeded(true);
1377
1378             loadMainState();
1379             writeInterframe();
1380         }
1381 #ifdef GPS
1382         if (feature(FEATURE_GPS)) {
1383             /*
1384              * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the
1385              * GPS home position.
1386              *
1387              * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
1388              * still be interpreted correctly.
1389              */
1390             if (GPS_home.lat != gpsHistory.GPS_home[0] || GPS_home.lon != gpsHistory.GPS_home[1]
1391                 || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
1392
1393                 writeGPSHomeFrame();
1394                 writeGPSFrame();
1395             } else if (gpsSol.numSat != gpsHistory.GPS_numSat || gpsSol.llh.lat != gpsHistory.GPS_coord[0]
1396                     || gpsSol.llh.lon != gpsHistory.GPS_coord[1]) {
1397                 //We could check for velocity changes as well but I doubt it changes independent of position
1398                 writeGPSFrame();
1399             }
1400         }
1401 #endif
1402     }
1403
1404     //Flush every iteration so that our runtime variance is minimized
1405     blackboxDeviceFlush();
1406 }
1407
1408 /**
1409  * Call each flight loop iteration to perform blackbox logging.
1410  */
1411 void handleBlackbox(void)
1412 {
1413     int i;
1414
1415     if (blackboxState >= BLACKBOX_FIRST_HEADER_SENDING_STATE && blackboxState <= BLACKBOX_LAST_HEADER_SENDING_STATE) {
1416         blackboxReplenishHeaderBudget();
1417     }
1418
1419     switch (blackboxState) {
1420         case BLACKBOX_STATE_SEND_HEADER:
1421             //On entry of this state, xmitState.headerIndex is 0 and startTime is intialised
1422
1423             /*
1424              * Once the UART has had time to init, transmit the header in chunks so we don't overflow its transmit
1425              * buffer, overflow the OpenLog's buffer, or keep the main loop busy for too long.
1426              */
1427             if (millis() > xmitState.u.startTime + 100) {
1428                 if (blackboxDeviceReserveBufferSpace(BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION) == BLACKBOX_RESERVE_SUCCESS) {
1429                     for (i = 0; i < BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION && blackboxHeader[xmitState.headerIndex] != '\0'; i++, xmitState.headerIndex++) {
1430                         blackboxWrite(blackboxHeader[xmitState.headerIndex]);
1431                         blackboxHeaderBudget--;
1432                     }
1433
1434                     if (blackboxHeader[xmitState.headerIndex] == '\0') {
1435                         blackboxSetState(BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER);
1436                     }
1437                 }
1438             }
1439         break;
1440         case BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER:
1441             //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1442             if (!sendFieldDefinition('I', 'P', blackboxMainFields, blackboxMainFields + 1, ARRAY_LENGTH(blackboxMainFields),
1443                     &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
1444 #ifdef GPS
1445                 if (feature(FEATURE_GPS)) {
1446                     blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADER);
1447                 } else
1448 #endif
1449                     blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
1450             }
1451         break;
1452 #ifdef GPS
1453         case BLACKBOX_STATE_SEND_GPS_H_HEADER:
1454             //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1455             if (!sendFieldDefinition('H', 0, blackboxGpsHFields, blackboxGpsHFields + 1, ARRAY_LENGTH(blackboxGpsHFields),
1456                     NULL, NULL)) {
1457                 blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADER);
1458             }
1459         break;
1460         case BLACKBOX_STATE_SEND_GPS_G_HEADER:
1461             //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1462             if (!sendFieldDefinition('G', 0, blackboxGpsGFields, blackboxGpsGFields + 1, ARRAY_LENGTH(blackboxGpsGFields),
1463                     &blackboxGpsGFields[0].condition, &blackboxGpsGFields[1].condition)) {
1464                 blackboxSetState(BLACKBOX_STATE_SEND_SLOW_HEADER);
1465             }
1466         break;
1467 #endif
1468         case BLACKBOX_STATE_SEND_SLOW_HEADER:
1469             //On entry of this state, xmitState.headerIndex is 0 and xmitState.u.fieldIndex is -1
1470             if (!sendFieldDefinition('S', 0, blackboxSlowFields, blackboxSlowFields + 1, ARRAY_LENGTH(blackboxSlowFields),
1471                     NULL, NULL)) {
1472                 blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
1473             }
1474         break;
1475         case BLACKBOX_STATE_SEND_SYSINFO:
1476             //On entry of this state, xmitState.headerIndex is 0
1477
1478             //Keep writing chunks of the system info headers until it returns true to signal completion
1479             if (blackboxWriteSysinfo()) {
1480
1481                 /*
1482                  * Wait for header buffers to drain completely before data logging begins to ensure reliable header delivery
1483                  * (overflowing circular buffers causes all data to be discarded, so the first few logged iterations
1484                  * could wipe out the end of the header if we weren't careful)
1485                  */
1486                 if (blackboxDeviceFlush()) {
1487                     blackboxSetState(BLACKBOX_STATE_RUNNING);
1488                 }
1489             }
1490         break;
1491         case BLACKBOX_STATE_PAUSED:
1492             // Only allow resume to occur during an I-frame iteration, so that we have an "I" base to work from
1493             if (IS_RC_MODE_ACTIVE(BOXBLACKBOX) && blackboxShouldLogIFrame()) {
1494                 // Write a log entry so the decoder is aware that our large time/iteration skip is intended
1495                 flightLogEvent_loggingResume_t resume;
1496
1497                 resume.logIteration = blackboxIteration;
1498                 resume.currentTime = currentTime;
1499
1500                 blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
1501                 blackboxSetState(BLACKBOX_STATE_RUNNING);
1502
1503                 blackboxLogIteration();
1504             }
1505
1506             // Keep the logging timers ticking so our log iteration continues to advance
1507             blackboxAdvanceIterationTimers();
1508         break;
1509         case BLACKBOX_STATE_RUNNING:
1510             // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
1511             if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
1512                 blackboxSetState(BLACKBOX_STATE_PAUSED);
1513             } else {
1514                 blackboxLogIteration();
1515             }
1516
1517             blackboxAdvanceIterationTimers();
1518         break;
1519         case BLACKBOX_STATE_SHUTTING_DOWN:
1520             //On entry of this state, startTime is set and a flush is performed
1521
1522             /*
1523              * Wait for the log we've transmitted to make its way to the logger before we release the serial port,
1524              * since releasing the port clears the Tx buffer.
1525              *
1526              * Don't wait longer than it could possibly take if something funky happens.
1527              */
1528             if (millis() > xmitState.u.startTime + BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS || blackboxDeviceFlush()) {
1529                 blackboxDeviceClose();
1530                 blackboxSetState(BLACKBOX_STATE_STOPPED);
1531             }
1532         break;
1533         default:
1534         break;
1535     }
1536
1537     // Did we run out of room on the device? Stop!
1538     if (isBlackboxDeviceFull()) {
1539         blackboxSetState(BLACKBOX_STATE_STOPPED);
1540     }
1541 }
1542
1543 static bool canUseBlackboxWithCurrentConfiguration(void)
1544 {
1545     return feature(FEATURE_BLACKBOX);
1546 }
1547
1548 /**
1549  * Call during system startup to initialize the blackbox.
1550  */
1551 void initBlackbox(void)
1552 {
1553     if (canUseBlackboxWithCurrentConfiguration()) {
1554         blackboxSetState(BLACKBOX_STATE_STOPPED);
1555     } else {
1556         blackboxSetState(BLACKBOX_STATE_DISABLED);
1557     }
1558 }
1559
1560 #endif