diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index cbce7b57da..31650ae01e 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1,1262 +1,1262 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight. If not, see .
- */
-
-#include
-#include
-#include
-
-#include "platform.h"
-#include "version.h"
-
-#include "common/maths.h"
-#include "common/axis.h"
-#include "common/color.h"
-
-#include "drivers/gpio.h"
-#include "drivers/sensor.h"
-#include "drivers/system.h"
-#include "drivers/serial.h"
-#include "drivers/compass.h"
-#include "drivers/timer.h"
-#include "drivers/pwm_rx.h"
-#include "drivers/accgyro.h"
-#include "drivers/light_led.h"
-#include "drivers/sound_beeper.h"
-
-#include "common/printf.h"
-
-#include "flight/flight.h"
-#include "sensors/sensors.h"
-#include "sensors/boardalignment.h"
-#include "sensors/sonar.h"
-#include "sensors/compass.h"
-#include "sensors/acceleration.h"
-#include "sensors/barometer.h"
-#include "sensors/gyro.h"
-#include "sensors/battery.h"
-#include "io/beeper.h"
-#include "io/display.h"
-#include "io/escservo.h"
-#include "rx/rx.h"
-#include "io/rc_controls.h"
-#include "flight/mixer.h"
-#include "flight/altitudehold.h"
-#include "flight/failsafe.h"
-#include "flight/imu.h"
-#include "flight/autotune.h"
-#include "flight/navigation.h"
-#include "io/gimbal.h"
-#include "io/gps.h"
-#include "io/ledstrip.h"
-#include "io/serial.h"
-#include "io/serial_cli.h"
-#include "io/serial_msp.h"
-#include "io/statusindicator.h"
-#include "rx/msp.h"
-#include "telemetry/telemetry.h"
-
-#include "config/runtime_config.h"
-#include "config/config.h"
-#include "config/config_profile.h"
-#include "config/config_master.h"
-
-#include "blackbox_fielddefs.h"
-#include "blackbox.h"
-
-#define BLACKBOX_BAUDRATE 115200
-#define BLACKBOX_INITIAL_PORT_MODE MODE_TX
-#define BLACKBOX_I_INTERVAL 32
-
-#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
-
-// Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
-#define STR_HELPER(x) #x
-#define STR(x) STR_HELPER(x)
-
-#define CONCAT_HELPER(x,y) x ## y
-#define CONCAT(x,y) CONCAT_HELPER(x, y)
-
-#define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
-#define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)
-#define CONDITION(x) CONCAT(FLIGHT_LOG_FIELD_CONDITION_, x)
-#define UNSIGNED FLIGHT_LOG_FIELD_UNSIGNED
-#define SIGNED FLIGHT_LOG_FIELD_SIGNED
-
-static const char blackboxHeader[] =
- "H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
- "H Blackbox version:1\n"
- "H Data version:2\n"
- "H I interval:" STR(BLACKBOX_I_INTERVAL) "\n";
-
-static const char* const blackboxMainHeaderNames[] = {
- "I name",
- "I signed",
- "I predictor",
- "I encoding",
- "P predictor",
- "P encoding"
-};
-
-static const char* const blackboxGPSGHeaderNames[] = {
- "G name",
- "G signed",
- "G predictor",
- "G encoding"
-};
-
-static const char* const blackboxGPSHHeaderNames[] = {
- "H name",
- "H signed",
- "H predictor",
- "H encoding"
-};
-
-/* All field definition structs should look like this (but with longer arrs): */
-typedef struct blackboxFieldDefinition_t {
- const char *name;
- uint8_t arr[1];
-} blackboxFieldDefinition_t;
-
-typedef struct blackboxMainFieldDefinition_t {
- const char *name;
- uint8_t isSigned;
- uint8_t Ipredict;
- uint8_t Iencode;
- uint8_t Ppredict;
- uint8_t Pencode;
- uint8_t condition; // Decide whether this field should appear in the log
-} blackboxMainFieldDefinition_t;
-
-typedef struct blackboxGPSFieldDefinition_t {
- const char *name;
- uint8_t isSigned;
- uint8_t predict;
- uint8_t encode;
-} blackboxGPSFieldDefinition_t;
-
-/**
- * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
- * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
- * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
- * the encoding we've promised here).
- */
-static const blackboxMainFieldDefinition_t blackboxMainFields[] = {
- /* loopIteration doesn't appear in P frames since it always increments */
- {"loopIteration", UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC), .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)},
- /* Time advances pretty steadily so the P-frame prediction is a straight line */
- {"time", UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(STRAIGHT_LINE), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
- {"axisP[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
- {"axisP[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
- {"axisP[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
- /* I terms get special packed encoding in P frames: */
- {"axisI[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
- {"axisI[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
- {"axisI[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
- {"axisD[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
- {"axisD[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
- {"axisD[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
- /* rcCommands are encoded together as a group in P-frames: */
- {"rcCommand[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
- {"rcCommand[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
- {"rcCommand[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
- /* Throttle is always in the range [minthrottle..maxthrottle]: */
- {"rcCommand[3]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
-
- {"vbatLatest", UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT},
-#ifdef MAG
- {"magADC[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
- {"magADC[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
- {"magADC[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
-#endif
-#ifdef BARO
- {"BaroAlt", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
-#endif
-
- /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
- {"gyroData[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
- {"gyroData[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
- {"gyroData[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
- {"accSmooth[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
- {"accSmooth[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
- {"accSmooth[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
- /* 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): */
- {"motor[0]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
- /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
- {"motor[1]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_2)},
- {"motor[2]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_3)},
- {"motor[3]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_4)},
- {"motor[4]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_5)},
- {"motor[5]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_6)},
- {"motor[6]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)},
- {"motor[7]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)},
- {"servo[5]", UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)}
-};
-
-// GPS position/vel frame
-static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = {
- {"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
- {"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)},
- {"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)},
- {"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
- {"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}
-};
-
-// GPS home frame
-static const blackboxGPSFieldDefinition_t blackboxGpsHFields[] = {
- {"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
- {"GPS_home[1]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}
-};
-
-typedef enum BlackboxState {
- BLACKBOX_STATE_DISABLED = 0,
- BLACKBOX_STATE_STOPPED,
- BLACKBOX_STATE_SEND_HEADER,
- BLACKBOX_STATE_SEND_FIELDINFO,
- BLACKBOX_STATE_SEND_GPS_H_HEADERS,
- BLACKBOX_STATE_SEND_GPS_G_HEADERS,
- BLACKBOX_STATE_SEND_SYSINFO,
- BLACKBOX_STATE_PRERUN,
- BLACKBOX_STATE_RUNNING
-} BlackboxState;
-
-typedef struct gpsState_t {
- int32_t GPS_home[2], GPS_coord[2];
- uint8_t GPS_numSat;
-} gpsState_t;
-
-//From mixer.c:
-extern uint8_t motorCount;
-
-//From mw.c:
-extern uint32_t currentTime;
-
-static const int SERIAL_CHUNK_SIZE = 16;
-
-static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
-
-static uint32_t startTime;
-static unsigned int headerXmitIndex;
-static int fieldXmitIndex;
-
-static uint32_t blackboxIteration;
-static uint32_t blackboxPFrameIndex, blackboxIFrameIndex;
-
-static serialPort_t *blackboxPort;
-static portMode_t previousPortMode;
-static uint32_t previousBaudRate;
-
-/*
- * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
- * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs
- * to encode:
- */
-static uint16_t vbatReference;
-static gpsState_t gpsHistory;
-
-// Keep a history of length 2, plus a buffer for MW to store the new values into
-static blackboxValues_t blackboxHistoryRing[3];
-
-// These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
-static blackboxValues_t* blackboxHistory[3];
-
-static void blackboxWrite(uint8_t value)
-{
- serialWrite(blackboxPort, value);
-}
-
-static void _putc(void *p, char c)
-{
- (void)p;
- serialWrite(blackboxPort, c);
-}
-
-//printf() to the blackbox serial port with no blocking shenanigans (so it's caller's responsibility to not write too fast!)
-static void blackboxPrintf(char *fmt, ...)
-{
- va_list va;
- va_start(va, fmt);
- tfp_format(NULL, _putc, fmt, va);
- va_end(va);
-}
-
-// Print the null-terminated string 's' to the serial port and return the number of bytes written
-static int blackboxPrint(const char *s)
-{
- const char *pos = s;
-
- while (*pos) {
- serialWrite(blackboxPort, *pos);
- pos++;
- }
-
- return pos - s;
-}
-
-/**
- * Write an unsigned integer to the blackbox serial port using variable byte encoding.
- */
-static void writeUnsignedVB(uint32_t value)
-{
- //While this isn't the final byte (we can only write 7 bits at a time)
- while (value > 127) {
- blackboxWrite((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow"
- value >>= 7;
- }
- blackboxWrite(value);
-}
-
-/**
- * Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding.
- */
-static void writeSignedVB(int32_t value)
-{
- //ZigZag encode to make the value always positive
- writeUnsignedVB((uint32_t)((value << 1) ^ (value >> 31)));
-}
-
-/**
- * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits
- */
-static void writeTag2_3S32(int32_t *values) {
- static const int NUM_FIELDS = 3;
-
- //Need to be enums rather than const ints if we want to switch on them (due to being C)
- enum {
- BITS_2 = 0,
- BITS_4 = 1,
- BITS_6 = 2,
- BITS_32 = 3
- };
-
- enum {
- BYTES_1 = 0,
- BYTES_2 = 1,
- BYTES_3 = 2,
- BYTES_4 = 3
- };
-
- int x;
- int selector = BITS_2, selector2;
-
- /*
- * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes
- * below:
- *
- * Selector possibilities
- *
- * 2 bits per field ss11 2233,
- * 4 bits per field ss00 1111 2222 3333
- * 6 bits per field ss11 1111 0022 2222 0033 3333
- * 32 bits per field sstt tttt followed by fields of various byte counts
- */
- for (x = 0; x < NUM_FIELDS; x++) {
- //Require more than 6 bits?
- if (values[x] >= 32 || values[x] < -32) {
- selector = BITS_32;
- break;
- }
-
- //Require more than 4 bits?
- if (values[x] >= 8 || values[x] < -8) {
- if (selector < BITS_6)
- selector = BITS_6;
- } else if (values[x] >= 2 || values[x] < -2) { //Require more than 2 bits?
- if (selector < BITS_4)
- selector = BITS_4;
- }
- }
-
- switch (selector) {
- case BITS_2:
- blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03));
- break;
- case BITS_4:
- blackboxWrite((selector << 6) | (values[0] & 0x0F));
- blackboxWrite((values[1] << 4) | (values[2] & 0x0F));
- break;
- case BITS_6:
- blackboxWrite((selector << 6) | (values[0] & 0x3F));
- blackboxWrite((uint8_t)values[1]);
- blackboxWrite((uint8_t)values[2]);
- break;
- case BITS_32:
- /*
- * Do another round to compute a selector for each field, assuming that they are at least 8 bits each
- *
- * Selector2 field possibilities
- * 0 - 8 bits
- * 1 - 16 bits
- * 2 - 24 bits
- * 3 - 32 bits
- */
- selector2 = 0;
-
- //Encode in reverse order so the first field is in the low bits:
- for (x = NUM_FIELDS - 1; x >= 0; x--) {
- selector2 <<= 2;
-
- if (values[x] < 128 && values[x] >= -128)
- selector2 |= BYTES_1;
- else if (values[x] < 32768 && values[x] >= -32768)
- selector2 |= BYTES_2;
- else if (values[x] < 8388608 && values[x] >= -8388608)
- selector2 |= BYTES_3;
- else
- selector2 |= BYTES_4;
- }
-
- //Write the selectors
- blackboxWrite((selector << 6) | selector2);
-
- //And now the values according to the selectors we picked for them
- for (x = 0; x < NUM_FIELDS; x++, selector2 >>= 2) {
- switch (selector2 & 0x03) {
- case BYTES_1:
- blackboxWrite(values[x]);
- break;
- case BYTES_2:
- blackboxWrite(values[x]);
- blackboxWrite(values[x] >> 8);
- break;
- case BYTES_3:
- blackboxWrite(values[x]);
- blackboxWrite(values[x] >> 8);
- blackboxWrite(values[x] >> 16);
- break;
- case BYTES_4:
- blackboxWrite(values[x]);
- blackboxWrite(values[x] >> 8);
- blackboxWrite(values[x] >> 16);
- blackboxWrite(values[x] >> 24);
- break;
- }
- }
- break;
- }
-}
-
-/**
- * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits.
- */
-static void writeTag8_4S16(int32_t *values) {
-
- //Need to be enums rather than const ints if we want to switch on them (due to being C)
- enum {
- FIELD_ZERO = 0,
- FIELD_4BIT = 1,
- FIELD_8BIT = 2,
- FIELD_16BIT = 3
- };
-
- uint8_t selector, buffer;
- int nibbleIndex;
- int x;
-
- selector = 0;
- //Encode in reverse order so the first field is in the low bits:
- for (x = 3; x >= 0; x--) {
- selector <<= 2;
-
- if (values[x] == 0)
- selector |= FIELD_ZERO;
- else if (values[x] < 8 && values[x] >= -8)
- selector |= FIELD_4BIT;
- else if (values[x] < 128 && values[x] >= -128)
- selector |= FIELD_8BIT;
- else
- selector |= FIELD_16BIT;
- }
-
- blackboxWrite(selector);
-
- nibbleIndex = 0;
- buffer = 0;
- for (x = 0; x < 4; x++, selector >>= 2) {
- switch (selector & 0x03) {
- case FIELD_ZERO:
- //No-op
- break;
- case FIELD_4BIT:
- if (nibbleIndex == 0) {
- //We fill high-bits first
- buffer = values[x] << 4;
- nibbleIndex = 1;
- } else {
- blackboxWrite(buffer | (values[x] & 0x0F));
- nibbleIndex = 0;
- }
- break;
- case FIELD_8BIT:
- if (nibbleIndex == 0)
- blackboxWrite(values[x]);
- else {
- //Write the high bits of the value first (mask to avoid sign extension)
- blackboxWrite(buffer | ((values[x] >> 4) & 0x0F));
- //Now put the leftover low bits into the top of the next buffer entry
- buffer = values[x] << 4;
- }
- break;
- case FIELD_16BIT:
- if (nibbleIndex == 0) {
- //Write high byte first
- blackboxWrite(values[x] >> 8);
- blackboxWrite(values[x]);
- } else {
- //First write the highest 4 bits
- blackboxWrite(buffer | ((values[x] >> 12) & 0x0F));
- // Then the middle 8
- blackboxWrite(values[x] >> 4);
- //Only the smallest 4 bits are still left to write
- buffer = values[x] << 4;
- }
- break;
- }
- }
- //Anything left over to write?
- if (nibbleIndex == 1)
- blackboxWrite(buffer);
-}
-
-/**
- * Write `valueCount` fields from `values` to the Blackbox using signed variable byte encoding. A 1-byte header is
- * written first which specifies which fields are non-zero (so this encoding is compact when most fields are zero).
- *
- * valueCount must be 8 or less.
- */
-static void writeTag8_8SVB(int32_t *values, int valueCount)
-{
- uint8_t header;
- int i;
-
- if (valueCount > 0) {
- //If we're only writing one field then we can skip the header
- if (valueCount == 1) {
- writeSignedVB(values[0]);
- } else {
- //First write a one-byte header that marks which fields are non-zero
- header = 0;
-
- // First field should be in low bits of header
- for (i = valueCount - 1; i >= 0; i--) {
- header <<= 1;
-
- if (values[i] != 0)
- header |= 0x01;
- }
-
- blackboxWrite(header);
-
- for (i = 0; i < valueCount; i++)
- if (values[i] != 0)
- writeSignedVB(values[i]);
- }
- }
-}
-
-static bool testBlackboxCondition(FlightLogFieldCondition condition)
-{
- switch (condition) {
- case FLIGHT_LOG_FIELD_CONDITION_ALWAYS:
- return true;
-
- case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1:
- case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2:
- case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3:
- case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4:
- case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5:
- case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6:
- case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7:
- case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8:
- return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
- case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
- return masterConfig.mixerMode == MIXER_TRI;
-
- case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0:
- case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_1:
- case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_2:
- return currentProfile->pidProfile.P8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0] != 0;
-
- case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0:
- case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_1:
- case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_2:
- return currentProfile->pidProfile.I8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0] != 0;
-
- case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
- case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
- case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
- return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
-
- case FLIGHT_LOG_FIELD_CONDITION_MAG:
-#ifdef MAG
- return sensors(SENSOR_MAG);
-#else
- return false;
-#endif
-
- case FLIGHT_LOG_FIELD_CONDITION_BARO:
-#ifdef BARO
- return sensors(SENSOR_BARO);
-#else
- return false;
-#endif
-
- case FLIGHT_LOG_FIELD_CONDITION_VBAT:
- return feature(FEATURE_VBAT);
-
- case FLIGHT_LOG_FIELD_CONDITION_NEVER:
- return false;
- default:
- return false;
- }
-}
-
-static void blackboxSetState(BlackboxState newState)
-{
- //Perform initial setup required for the new state
- switch (newState) {
- case BLACKBOX_STATE_SEND_HEADER:
- startTime = millis();
- headerXmitIndex = 0;
- break;
- case BLACKBOX_STATE_SEND_FIELDINFO:
- case BLACKBOX_STATE_SEND_GPS_G_HEADERS:
- case BLACKBOX_STATE_SEND_GPS_H_HEADERS:
- headerXmitIndex = 0;
- fieldXmitIndex = -1;
- break;
- case BLACKBOX_STATE_SEND_SYSINFO:
- headerXmitIndex = 0;
- break;
- case BLACKBOX_STATE_RUNNING:
- blackboxIteration = 0;
- blackboxPFrameIndex = 0;
- blackboxIFrameIndex = 0;
- break;
- default:
- ;
- }
- blackboxState = newState;
-}
-
-static void writeIntraframe(void)
-{
- blackboxValues_t *blackboxCurrent = blackboxHistory[0];
- int x;
-
- blackboxWrite('I');
-
- writeUnsignedVB(blackboxIteration);
- writeUnsignedVB(blackboxCurrent->time);
-
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- writeSignedVB(blackboxCurrent->axisPID_P[x]);
-
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- writeSignedVB(blackboxCurrent->axisPID_I[x]);
-
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x))
- writeSignedVB(blackboxCurrent->axisPID_D[x]);
-
- for (x = 0; x < 3; x++)
- writeSignedVB(blackboxCurrent->rcCommand[x]);
-
- writeUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle]
-
- if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
- /*
- * Our voltage is expected to decrease over the course of the flight, so store our difference from
- * the reference:
- *
- * Write 14 bits even if the number is negative (which would otherwise result in 32 bits)
- */
- writeUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF);
- }
-
-#ifdef MAG
- if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- writeSignedVB(blackboxCurrent->magADC[x]);
- }
-#endif
-
-#ifdef BARO
- if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO))
- writeSignedVB(blackboxCurrent->BaroAlt);
-#endif
-
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- writeSignedVB(blackboxCurrent->gyroData[x]);
-
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- writeSignedVB(blackboxCurrent->accSmooth[x]);
-
- //Motors can be below minthrottle when disarmed, but that doesn't happen much
- writeUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle);
-
- //Motors tend to be similar to each other
- for (x = 1; x < motorCount; x++)
- writeSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
-
- if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER))
- writeSignedVB(blackboxHistory[0]->servo[5] - 1500);
-
- //Rotate our history buffers:
-
- //The current state becomes the new "before" state
- blackboxHistory[1] = blackboxHistory[0];
- //And since we have no other history, we also use it for the "before, before" state
- blackboxHistory[2] = blackboxHistory[0];
- //And advance the current state over to a blank space ready to be filled
- blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
-}
-
-static void writeInterframe(void)
-{
- int x;
- int32_t deltas[5];
-
- blackboxValues_t *blackboxCurrent = blackboxHistory[0];
- blackboxValues_t *blackboxLast = blackboxHistory[1];
-
- blackboxWrite('P');
-
- //No need to store iteration count since its delta is always 1
-
- /*
- * Since the difference between the difference between successive times will be nearly zero, use
- * second-order differences.
- */
- writeSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
-
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- writeSignedVB(blackboxCurrent->axisPID_P[x] - blackboxLast->axisPID_P[x]);
-
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- deltas[x] = blackboxCurrent->axisPID_I[x] - blackboxLast->axisPID_I[x];
-
- /*
- * The PID I field changes very slowly, most of the time +-2, so use an encoding
- * that can pack all three fields into one byte in that situation.
- */
- writeTag2_3S32(deltas);
-
- /*
- * The PID D term is frequently set to zero for yaw, which makes the result from the calculation
- * always zero. So don't bother recording D results when PID D terms are zero.
- */
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x))
- writeSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]);
-
- /*
- * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
- * can pack multiple values per byte:
- */
- for (x = 0; x < 4; x++)
- deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x];
-
- writeTag8_4S16(deltas);
-
- //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, MAG, BARO
- int optionalFieldCount = 0;
-
- if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
- deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->vbatLatest - blackboxLast->vbatLatest;
- }
-
-#ifdef MAG
- if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- deltas[optionalFieldCount++] = blackboxCurrent->magADC[x] - blackboxLast->magADC[x];
- }
-#endif
-
-#ifdef BARO
- if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO))
- deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt;
-#endif
- writeTag8_8SVB(deltas, optionalFieldCount);
-
- //Since gyros, accs and motors are noisy, base the prediction on the average of the history:
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- writeSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2);
-
- for (x = 0; x < XYZ_AXIS_COUNT; x++)
- writeSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[x]) / 2);
-
- for (x = 0; x < motorCount; x++)
- writeSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2);
-
- if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER))
- writeSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]);
-
- //Rotate our history buffers
- blackboxHistory[2] = blackboxHistory[1];
- blackboxHistory[1] = blackboxHistory[0];
- blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
-}
-
-static int gcd(int num, int denom)
-{
- if (denom == 0)
- return num;
- return gcd(denom, num % denom);
-}
-
-static void validateBlackboxConfig()
-{
- int div;
-
- if (masterConfig.blackbox_rate_num == 0 || masterConfig.blackbox_rate_denom == 0
- || masterConfig.blackbox_rate_num >= masterConfig.blackbox_rate_denom) {
- masterConfig.blackbox_rate_num = 1;
- masterConfig.blackbox_rate_denom = 1;
- } else {
- div = gcd(masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
-
- masterConfig.blackbox_rate_num /= div;
- masterConfig.blackbox_rate_denom /= div;
- }
-}
-
-static void configureBlackboxPort(void)
-{
- blackboxPort = findOpenSerialPort(FUNCTION_BLACKBOX);
- if (blackboxPort) {
- previousPortMode = blackboxPort->mode;
- previousBaudRate = blackboxPort->baudRate;
-
- serialSetBaudRate(blackboxPort, BLACKBOX_BAUDRATE);
- serialSetMode(blackboxPort, BLACKBOX_INITIAL_PORT_MODE);
- beginSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX);
- } else {
- blackboxPort = openSerialPort(FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
-
- if (blackboxPort) {
- previousPortMode = blackboxPort->mode;
- previousBaudRate = blackboxPort->baudRate;
- }
- }
-}
-
-static void releaseBlackboxPort(void)
-{
- serialSetMode(blackboxPort, previousPortMode);
- serialSetBaudRate(blackboxPort, previousBaudRate);
-
- endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX);
-}
-
-void startBlackbox(void)
-{
- if (blackboxState == BLACKBOX_STATE_STOPPED) {
- validateBlackboxConfig();
-
- configureBlackboxPort();
-
- if (!blackboxPort) {
- blackboxSetState(BLACKBOX_STATE_DISABLED);
- return;
- }
-
- memset(&gpsHistory, 0, sizeof(gpsHistory));
-
- blackboxHistory[0] = &blackboxHistoryRing[0];
- blackboxHistory[1] = &blackboxHistoryRing[1];
- blackboxHistory[2] = &blackboxHistoryRing[2];
-
- vbatReference = vbatLatest;
-
- //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
-
- blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
- }
-}
-
-void finishBlackbox(void)
-{
- if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED) {
- blackboxSetState(BLACKBOX_STATE_STOPPED);
-
- releaseBlackboxPort();
- }
-}
-
-static void writeGPSHomeFrame()
-{
- blackboxWrite('H');
-
- writeSignedVB(GPS_home[0]);
- writeSignedVB(GPS_home[1]);
- //TODO it'd be great if we could grab the GPS current time and write that too
-
- gpsHistory.GPS_home[0] = GPS_home[0];
- gpsHistory.GPS_home[1] = GPS_home[1];
-}
-
-static void writeGPSFrame()
-{
- blackboxWrite('G');
-
- writeUnsignedVB(GPS_numSat);
- writeSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]);
- writeSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]);
- writeUnsignedVB(GPS_altitude);
- writeUnsignedVB(GPS_speed);
-
- gpsHistory.GPS_numSat = GPS_numSat;
- gpsHistory.GPS_coord[0] = GPS_coord[0];
- gpsHistory.GPS_coord[1] = GPS_coord[1];
-}
-
-/**
- * Fill the current state of the blackbox using values read from the flight controller
- */
-static void loadBlackboxState(void)
-{
- blackboxValues_t *blackboxCurrent = blackboxHistory[0];
- int i;
-
- blackboxCurrent->time = currentTime;
-
- for (i = 0; i < XYZ_AXIS_COUNT; i++)
- blackboxCurrent->axisPID_P[i] = axisPID_P[i];
- for (i = 0; i < XYZ_AXIS_COUNT; i++)
- blackboxCurrent->axisPID_I[i] = axisPID_I[i];
- for (i = 0; i < XYZ_AXIS_COUNT; i++)
- blackboxCurrent->axisPID_D[i] = axisPID_D[i];
-
- for (i = 0; i < 4; i++)
- blackboxCurrent->rcCommand[i] = rcCommand[i];
-
- for (i = 0; i < XYZ_AXIS_COUNT; i++)
- blackboxCurrent->gyroData[i] = gyroData[i];
-
- for (i = 0; i < XYZ_AXIS_COUNT; i++)
- blackboxCurrent->accSmooth[i] = accSmooth[i];
-
- for (i = 0; i < motorCount; i++)
- blackboxCurrent->motor[i] = motor[i];
-
- blackboxCurrent->vbatLatest = vbatLatest;
-
-#ifdef MAG
- for (i = 0; i < XYZ_AXIS_COUNT; i++)
- blackboxCurrent->magADC[i] = magADC[i];
-#endif
-
-#ifdef BARO
- blackboxCurrent->BaroAlt = BaroAlt;
-#endif
-
- //Tail servo for tricopters
- blackboxCurrent->servo[5] = servo[5];
-}
-
-/**
- * Transmit the header information for the given field definitions. Transmitted header lines look like:
- *
- * H Field I name:a,b,c
- * H Field I predictor:0,1,2
- *
- * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
- * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
- *
- * Set headerXmitIndex to 0 and fieldXmitIndex to -1 before calling for the first time.
- *
- * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the
- * fieldDefinition and secondCondition arrays.
- *
- * Returns true if there is still header left to transmit (so call again to continue transmission).
- */
-static bool sendFieldDefinition(const char * const *headerNames, unsigned int headerCount, const void *fieldDefinitions,
- const void *secondFieldDefinition, int fieldCount, const uint8_t *conditions, const uint8_t *secondCondition)
-{
- const blackboxFieldDefinition_t *def;
- int charsWritten;
- static bool needComma = false;
- size_t definitionStride = (char*) secondFieldDefinition - (char*) fieldDefinitions;
- size_t conditionsStride = (char*) secondCondition - (char*) conditions;
-
- /*
- * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit
- * the whole header.
- */
- if (fieldXmitIndex == -1) {
- if (headerXmitIndex >= headerCount)
- return false; //Someone probably called us again after we had already completed transmission
-
- charsWritten = blackboxPrint("H Field ");
- charsWritten += blackboxPrint(headerNames[headerXmitIndex]);
- charsWritten += blackboxPrint(":");
-
- fieldXmitIndex++;
- needComma = false;
- } else
- charsWritten = 0;
-
- for (; fieldXmitIndex < fieldCount && charsWritten < SERIAL_CHUNK_SIZE; fieldXmitIndex++) {
- def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * fieldXmitIndex);
-
- if (!conditions || testBlackboxCondition(conditions[conditionsStride * fieldXmitIndex])) {
- if (needComma) {
- blackboxWrite(',');
- charsWritten++;
- } else
- needComma = true;
-
- // The first header is a field name
- if (headerXmitIndex == 0) {
- charsWritten += blackboxPrint(def->name);
- } else {
- //The other headers are integers
- if (def->arr[headerXmitIndex - 1] >= 10) {
- blackboxWrite(def->arr[headerXmitIndex - 1] / 10 + '0');
- blackboxWrite(def->arr[headerXmitIndex - 1] % 10 + '0');
- charsWritten += 2;
- } else {
- blackboxWrite(def->arr[headerXmitIndex - 1] + '0');
- charsWritten++;
- }
- }
- }
- }
-
- // Did we complete this line?
- if (fieldXmitIndex == fieldCount) {
- blackboxWrite('\n');
- headerXmitIndex++;
- fieldXmitIndex = -1;
- }
-
- return headerXmitIndex < headerCount;
-}
-
-/**
- * Transmit a portion of the system information headers. Begin with a xmitIndex of 0. Returns the next xmitIndex to
- * call with, or -1 if transmission is complete.
- */
-static int blackboxWriteSysinfo(int xmitIndex)
-{
- union floatConvert_t {
- float f;
- uint32_t u;
- } floatConvert;
-
- switch (xmitIndex) {
- case 0:
- blackboxPrintf("H Firmware type:Cleanflight\n");
- break;
- case 1:
- // Pause to allow more time for previous to transmit (it exceeds our chunk size)
- break;
- case 2:
- blackboxPrintf("H Firmware revision:%s\n", shortGitRevision);
- break;
- case 3:
- // Pause to allow more time for previous to transmit
- break;
- case 4:
- blackboxPrintf("H Firmware date:%s %s\n", buildDate, buildTime);
- break;
- case 5:
- // Pause to allow more time for previous to transmit
- break;
- case 6:
- blackboxPrintf("H P interval:%d/%d\n", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
- break;
- case 7:
- blackboxPrintf("H rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8);
- break;
- case 8:
- blackboxPrintf("H minthrottle:%d\n", masterConfig.escAndServoConfig.minthrottle);
- break;
- case 9:
- blackboxPrintf("H maxthrottle:%d\n", masterConfig.escAndServoConfig.maxthrottle);
- break;
- case 10:
- floatConvert.f = gyro.scale;
- blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u);
- break;
- case 11:
- blackboxPrintf("H acc_1G:%u\n", acc_1G);
- break;
- case 12:
- blackboxPrintf("H vbatscale:%u\n", masterConfig.batteryConfig.vbatscale);
- break;
- case 13:
- blackboxPrintf("H vbatcellvoltage:%u,%u,%u\n", masterConfig.batteryConfig.vbatmincellvoltage,
- masterConfig.batteryConfig.vbatwarningcellvoltage, masterConfig.batteryConfig.vbatmaxcellvoltage);
- break;
- case 14:
- //Pause
- break;
- case 15:
- blackboxPrintf("H vbatref:%u\n", vbatReference);
- break;
- case 16:
- // One more pause for good luck
- break;
- default:
- return -1;
- }
-
- return xmitIndex + 1;
-}
-
-// Beep the buzzer and write the current time to the log as a synchronization point
-static void blackboxPlaySyncBeep()
-{
- uint32_t now = micros();
-
- /*
- * The regular beep routines aren't going to work for us, because they queue up the beep to be executed later.
- * Our beep is timing sensitive, so start beeping now without setting the beeperIsOn flag.
- */
- BEEP_ON;
-
- // Have the regular beeper code turn off the beep for us eventually, since that's not timing-sensitive
- queueConfirmationBeep(1);
-
- blackboxWrite('E');
- blackboxWrite(FLIGHT_LOG_EVENT_SYNC_BEEP);
-
- writeUnsignedVB(now);
-}
-
-void handleBlackbox(void)
-{
- int i, result;
-
- switch (blackboxState) {
- case BLACKBOX_STATE_SEND_HEADER:
- //On entry of this state, headerXmitIndex is 0 and startTime is intialised
-
- /*
- * Once the UART has had time to init, transmit the header in chunks so we don't overflow our transmit
- * buffer.
- */
- if (millis() > startTime + 100) {
- for (i = 0; i < SERIAL_CHUNK_SIZE && blackboxHeader[headerXmitIndex] != '\0'; i++, headerXmitIndex++)
- blackboxWrite(blackboxHeader[headerXmitIndex]);
-
- if (blackboxHeader[headerXmitIndex] == '\0')
- blackboxSetState(BLACKBOX_STATE_SEND_FIELDINFO);
- }
- break;
- case BLACKBOX_STATE_SEND_FIELDINFO:
- //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1
- if (!sendFieldDefinition(blackboxMainHeaderNames, ARRAY_LENGTH(blackboxMainHeaderNames), blackboxMainFields, blackboxMainFields + 1,
- ARRAY_LENGTH(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
- if (feature(FEATURE_GPS))
- blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADERS);
- else
- blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
- }
- break;
- case BLACKBOX_STATE_SEND_GPS_H_HEADERS:
- //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1
- if (!sendFieldDefinition(blackboxGPSHHeaderNames, ARRAY_LENGTH(blackboxGPSHHeaderNames), blackboxGpsHFields, blackboxGpsHFields + 1,
- ARRAY_LENGTH(blackboxGpsHFields), NULL, 0)) {
- blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADERS);
- }
- break;
- case BLACKBOX_STATE_SEND_GPS_G_HEADERS:
- //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1
- if (!sendFieldDefinition(blackboxGPSGHeaderNames, ARRAY_LENGTH(blackboxGPSGHeaderNames), blackboxGpsGFields, blackboxGpsGFields + 1,
- ARRAY_LENGTH(blackboxGpsGFields), NULL, 0)) {
- blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
- }
- break;
- case BLACKBOX_STATE_SEND_SYSINFO:
- //On entry of this state, headerXmitIndex is 0
- result = blackboxWriteSysinfo(headerXmitIndex);
-
- if (result == -1)
- blackboxSetState(BLACKBOX_STATE_PRERUN);
- else
- headerXmitIndex = result;
- break;
- case BLACKBOX_STATE_PRERUN:
- blackboxPlaySyncBeep();
-
- blackboxSetState(BLACKBOX_STATE_RUNNING);
- break;
- case BLACKBOX_STATE_RUNNING:
- // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
-
- // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
- if (blackboxPFrameIndex == 0) {
- // Copy current system values into the blackbox
- loadBlackboxState();
- writeIntraframe();
- } else {
- if ((blackboxPFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num) {
- loadBlackboxState();
- writeInterframe();
- }
-
- if (feature(FEATURE_GPS)) {
- /*
- * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the
- * GPS home position.
- *
- * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
- * still be interpreted correctly.
- */
- if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
- || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
-
- writeGPSHomeFrame();
- writeGPSFrame();
- } else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
- || GPS_coord[1] != gpsHistory.GPS_coord[1]) {
- //We could check for velocity changes as well but I doubt it changes independent of position
- writeGPSFrame();
- }
- }
- }
-
- blackboxIteration++;
- blackboxPFrameIndex++;
-
- if (blackboxPFrameIndex == BLACKBOX_I_INTERVAL) {
- blackboxPFrameIndex = 0;
- blackboxIFrameIndex++;
- }
- break;
- default:
- break;
- }
-}
-
-static bool canUseBlackboxWithCurrentConfiguration(void)
-{
- if (!feature(FEATURE_BLACKBOX))
- return false;
-
- return true;
-}
-
-void initBlackbox(void)
-{
- if (canUseBlackboxWithCurrentConfiguration())
- blackboxSetState(BLACKBOX_STATE_STOPPED);
- else
- blackboxSetState(BLACKBOX_STATE_DISABLED);
-}
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+#include "version.h"
+
+#include "common/maths.h"
+#include "common/axis.h"
+#include "common/color.h"
+
+#include "drivers/gpio.h"
+#include "drivers/sensor.h"
+#include "drivers/system.h"
+#include "drivers/serial.h"
+#include "drivers/compass.h"
+#include "drivers/timer.h"
+#include "drivers/pwm_rx.h"
+#include "drivers/accgyro.h"
+#include "drivers/light_led.h"
+#include "drivers/sound_beeper.h"
+
+#include "common/printf.h"
+
+#include "flight/flight.h"
+#include "sensors/sensors.h"
+#include "sensors/boardalignment.h"
+#include "sensors/sonar.h"
+#include "sensors/compass.h"
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/gyro.h"
+#include "sensors/battery.h"
+#include "io/beeper.h"
+#include "io/display.h"
+#include "io/escservo.h"
+#include "rx/rx.h"
+#include "io/rc_controls.h"
+#include "flight/mixer.h"
+#include "flight/altitudehold.h"
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/autotune.h"
+#include "flight/navigation.h"
+#include "io/gimbal.h"
+#include "io/gps.h"
+#include "io/ledstrip.h"
+#include "io/serial.h"
+#include "io/serial_cli.h"
+#include "io/serial_msp.h"
+#include "io/statusindicator.h"
+#include "rx/msp.h"
+#include "telemetry/telemetry.h"
+
+#include "config/runtime_config.h"
+#include "config/config.h"
+#include "config/config_profile.h"
+#include "config/config_master.h"
+
+#include "blackbox_fielddefs.h"
+#include "blackbox.h"
+
+#define BLACKBOX_BAUDRATE 115200
+#define BLACKBOX_INITIAL_PORT_MODE MODE_TX
+#define BLACKBOX_I_INTERVAL 32
+
+#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
+
+// Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter:
+#define STR_HELPER(x) #x
+#define STR(x) STR_HELPER(x)
+
+#define CONCAT_HELPER(x,y) x ## y
+#define CONCAT(x,y) CONCAT_HELPER(x, y)
+
+#define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)
+#define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)
+#define CONDITION(x) CONCAT(FLIGHT_LOG_FIELD_CONDITION_, x)
+#define UNSIGNED FLIGHT_LOG_FIELD_UNSIGNED
+#define SIGNED FLIGHT_LOG_FIELD_SIGNED
+
+static const char blackboxHeader[] =
+ "H Product:Blackbox flight data recorder by Nicholas Sherlock\n"
+ "H Blackbox version:1\n"
+ "H Data version:2\n"
+ "H I interval:" STR(BLACKBOX_I_INTERVAL) "\n";
+
+static const char* const blackboxMainHeaderNames[] = {
+ "I name",
+ "I signed",
+ "I predictor",
+ "I encoding",
+ "P predictor",
+ "P encoding"
+};
+
+static const char* const blackboxGPSGHeaderNames[] = {
+ "G name",
+ "G signed",
+ "G predictor",
+ "G encoding"
+};
+
+static const char* const blackboxGPSHHeaderNames[] = {
+ "H name",
+ "H signed",
+ "H predictor",
+ "H encoding"
+};
+
+/* All field definition structs should look like this (but with longer arrs): */
+typedef struct blackboxFieldDefinition_t {
+ const char *name;
+ uint8_t arr[1];
+} blackboxFieldDefinition_t;
+
+typedef struct blackboxMainFieldDefinition_t {
+ const char *name;
+ uint8_t isSigned;
+ uint8_t Ipredict;
+ uint8_t Iencode;
+ uint8_t Ppredict;
+ uint8_t Pencode;
+ uint8_t condition; // Decide whether this field should appear in the log
+} blackboxMainFieldDefinition_t;
+
+typedef struct blackboxGPSFieldDefinition_t {
+ const char *name;
+ uint8_t isSigned;
+ uint8_t predict;
+ uint8_t encode;
+} blackboxGPSFieldDefinition_t;
+
+/**
+ * Description of the blackbox fields we are writing in our main intra (I) and inter (P) frames. This description is
+ * written into the flight log header so the log can be properly interpreted (but these definitions don't actually cause
+ * the encoding to happen, we have to encode the flight log ourselves in write{Inter|Intra}frame() in a way that matches
+ * the encoding we've promised here).
+ */
+static const blackboxMainFieldDefinition_t blackboxMainFields[] = {
+ /* loopIteration doesn't appear in P frames since it always increments */
+ {"loopIteration", UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(INC), .Pencode = FLIGHT_LOG_FIELD_ENCODING_NULL, CONDITION(ALWAYS)},
+ /* Time advances pretty steadily so the P-frame prediction is a straight line */
+ {"time", UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(STRAIGHT_LINE), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+ {"axisP[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+ {"axisP[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+ {"axisP[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+ /* I terms get special packed encoding in P frames: */
+ {"axisI[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
+ {"axisI[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
+ {"axisI[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG2_3S32), CONDITION(ALWAYS)},
+ {"axisD[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
+ {"axisD[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
+ {"axisD[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
+ /* rcCommands are encoded together as a group in P-frames: */
+ {"rcCommand[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
+ {"rcCommand[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
+ {"rcCommand[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
+ /* Throttle is always in the range [minthrottle..maxthrottle]: */
+ {"rcCommand[3]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
+
+ {"vbatLatest", UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT},
+#ifdef MAG
+ {"magADC[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
+ {"magADC[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
+ {"magADC[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_MAG},
+#endif
+#ifdef BARO
+ {"BaroAlt", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
+#endif
+
+ /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
+ {"gyroData[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+ {"gyroData[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+ {"gyroData[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+ {"accSmooth[0]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+ {"accSmooth[1]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+ {"accSmooth[2]", SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+ /* 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): */
+ {"motor[0]", UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
+ /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
+ {"motor[1]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_2)},
+ {"motor[2]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_3)},
+ {"motor[3]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_4)},
+ {"motor[4]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_5)},
+ {"motor[5]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_6)},
+ {"motor[6]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_7)},
+ {"motor[7]", UNSIGNED, .Ipredict = PREDICT(MOTOR_0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_8)},
+ {"servo[5]", UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)}
+};
+
+// GPS position/vel frame
+static const blackboxGPSFieldDefinition_t blackboxGpsGFields[] = {
+ {"GPS_numSat", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
+ {"GPS_coord[0]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)},
+ {"GPS_coord[1]", SIGNED, PREDICT(HOME_COORD), ENCODING(SIGNED_VB)},
+ {"GPS_altitude", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)},
+ {"GPS_speed", UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}
+};
+
+// GPS home frame
+static const blackboxGPSFieldDefinition_t blackboxGpsHFields[] = {
+ {"GPS_home[0]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)},
+ {"GPS_home[1]", SIGNED, PREDICT(0), ENCODING(SIGNED_VB)}
+};
+
+typedef enum BlackboxState {
+ BLACKBOX_STATE_DISABLED = 0,
+ BLACKBOX_STATE_STOPPED,
+ BLACKBOX_STATE_SEND_HEADER,
+ BLACKBOX_STATE_SEND_FIELDINFO,
+ BLACKBOX_STATE_SEND_GPS_H_HEADERS,
+ BLACKBOX_STATE_SEND_GPS_G_HEADERS,
+ BLACKBOX_STATE_SEND_SYSINFO,
+ BLACKBOX_STATE_PRERUN,
+ BLACKBOX_STATE_RUNNING
+} BlackboxState;
+
+typedef struct gpsState_t {
+ int32_t GPS_home[2], GPS_coord[2];
+ uint8_t GPS_numSat;
+} gpsState_t;
+
+//From mixer.c:
+extern uint8_t motorCount;
+
+//From mw.c:
+extern uint32_t currentTime;
+
+static const int SERIAL_CHUNK_SIZE = 16;
+
+static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
+
+static uint32_t startTime;
+static unsigned int headerXmitIndex;
+static int fieldXmitIndex;
+
+static uint32_t blackboxIteration;
+static uint32_t blackboxPFrameIndex, blackboxIFrameIndex;
+
+static serialPort_t *blackboxPort;
+static portMode_t previousPortMode;
+static uint32_t previousBaudRate;
+
+/*
+ * We store voltages in I-frames relative to this, which was the voltage when the blackbox was activated.
+ * This helps out since the voltage is only expected to fall from that point and we can reduce our diffs
+ * to encode:
+ */
+static uint16_t vbatReference;
+static gpsState_t gpsHistory;
+
+// Keep a history of length 2, plus a buffer for MW to store the new values into
+static blackboxValues_t blackboxHistoryRing[3];
+
+// These point into blackboxHistoryRing, use them to know where to store history of a given age (0, 1 or 2 generations old)
+static blackboxValues_t* blackboxHistory[3];
+
+static void blackboxWrite(uint8_t value)
+{
+ serialWrite(blackboxPort, value);
+}
+
+static void _putc(void *p, char c)
+{
+ (void)p;
+ serialWrite(blackboxPort, c);
+}
+
+//printf() to the blackbox serial port with no blocking shenanigans (so it's caller's responsibility to not write too fast!)
+static void blackboxPrintf(char *fmt, ...)
+{
+ va_list va;
+ va_start(va, fmt);
+ tfp_format(NULL, _putc, fmt, va);
+ va_end(va);
+}
+
+// Print the null-terminated string 's' to the serial port and return the number of bytes written
+static int blackboxPrint(const char *s)
+{
+ const char *pos = s;
+
+ while (*pos) {
+ serialWrite(blackboxPort, *pos);
+ pos++;
+ }
+
+ return pos - s;
+}
+
+/**
+ * Write an unsigned integer to the blackbox serial port using variable byte encoding.
+ */
+static void writeUnsignedVB(uint32_t value)
+{
+ //While this isn't the final byte (we can only write 7 bits at a time)
+ while (value > 127) {
+ blackboxWrite((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow"
+ value >>= 7;
+ }
+ blackboxWrite(value);
+}
+
+/**
+ * Write a signed integer to the blackbox serial port using ZigZig and variable byte encoding.
+ */
+static void writeSignedVB(int32_t value)
+{
+ //ZigZag encode to make the value always positive
+ writeUnsignedVB((uint32_t)((value << 1) ^ (value >> 31)));
+}
+
+/**
+ * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits
+ */
+static void writeTag2_3S32(int32_t *values) {
+ static const int NUM_FIELDS = 3;
+
+ //Need to be enums rather than const ints if we want to switch on them (due to being C)
+ enum {
+ BITS_2 = 0,
+ BITS_4 = 1,
+ BITS_6 = 2,
+ BITS_32 = 3
+ };
+
+ enum {
+ BYTES_1 = 0,
+ BYTES_2 = 1,
+ BYTES_3 = 2,
+ BYTES_4 = 3
+ };
+
+ int x;
+ int selector = BITS_2, selector2;
+
+ /*
+ * Find out how many bits the largest value requires to encode, and use it to choose one of the packing schemes
+ * below:
+ *
+ * Selector possibilities
+ *
+ * 2 bits per field ss11 2233,
+ * 4 bits per field ss00 1111 2222 3333
+ * 6 bits per field ss11 1111 0022 2222 0033 3333
+ * 32 bits per field sstt tttt followed by fields of various byte counts
+ */
+ for (x = 0; x < NUM_FIELDS; x++) {
+ //Require more than 6 bits?
+ if (values[x] >= 32 || values[x] < -32) {
+ selector = BITS_32;
+ break;
+ }
+
+ //Require more than 4 bits?
+ if (values[x] >= 8 || values[x] < -8) {
+ if (selector < BITS_6)
+ selector = BITS_6;
+ } else if (values[x] >= 2 || values[x] < -2) { //Require more than 2 bits?
+ if (selector < BITS_4)
+ selector = BITS_4;
+ }
+ }
+
+ switch (selector) {
+ case BITS_2:
+ blackboxWrite((selector << 6) | ((values[0] & 0x03) << 4) | ((values[1] & 0x03) << 2) | (values[2] & 0x03));
+ break;
+ case BITS_4:
+ blackboxWrite((selector << 6) | (values[0] & 0x0F));
+ blackboxWrite((values[1] << 4) | (values[2] & 0x0F));
+ break;
+ case BITS_6:
+ blackboxWrite((selector << 6) | (values[0] & 0x3F));
+ blackboxWrite((uint8_t)values[1]);
+ blackboxWrite((uint8_t)values[2]);
+ break;
+ case BITS_32:
+ /*
+ * Do another round to compute a selector for each field, assuming that they are at least 8 bits each
+ *
+ * Selector2 field possibilities
+ * 0 - 8 bits
+ * 1 - 16 bits
+ * 2 - 24 bits
+ * 3 - 32 bits
+ */
+ selector2 = 0;
+
+ //Encode in reverse order so the first field is in the low bits:
+ for (x = NUM_FIELDS - 1; x >= 0; x--) {
+ selector2 <<= 2;
+
+ if (values[x] < 128 && values[x] >= -128)
+ selector2 |= BYTES_1;
+ else if (values[x] < 32768 && values[x] >= -32768)
+ selector2 |= BYTES_2;
+ else if (values[x] < 8388608 && values[x] >= -8388608)
+ selector2 |= BYTES_3;
+ else
+ selector2 |= BYTES_4;
+ }
+
+ //Write the selectors
+ blackboxWrite((selector << 6) | selector2);
+
+ //And now the values according to the selectors we picked for them
+ for (x = 0; x < NUM_FIELDS; x++, selector2 >>= 2) {
+ switch (selector2 & 0x03) {
+ case BYTES_1:
+ blackboxWrite(values[x]);
+ break;
+ case BYTES_2:
+ blackboxWrite(values[x]);
+ blackboxWrite(values[x] >> 8);
+ break;
+ case BYTES_3:
+ blackboxWrite(values[x]);
+ blackboxWrite(values[x] >> 8);
+ blackboxWrite(values[x] >> 16);
+ break;
+ case BYTES_4:
+ blackboxWrite(values[x]);
+ blackboxWrite(values[x] >> 8);
+ blackboxWrite(values[x] >> 16);
+ blackboxWrite(values[x] >> 24);
+ break;
+ }
+ }
+ break;
+ }
+}
+
+/**
+ * Write an 8-bit selector followed by four signed fields of size 0, 4, 8 or 16 bits.
+ */
+static void writeTag8_4S16(int32_t *values) {
+
+ //Need to be enums rather than const ints if we want to switch on them (due to being C)
+ enum {
+ FIELD_ZERO = 0,
+ FIELD_4BIT = 1,
+ FIELD_8BIT = 2,
+ FIELD_16BIT = 3
+ };
+
+ uint8_t selector, buffer;
+ int nibbleIndex;
+ int x;
+
+ selector = 0;
+ //Encode in reverse order so the first field is in the low bits:
+ for (x = 3; x >= 0; x--) {
+ selector <<= 2;
+
+ if (values[x] == 0)
+ selector |= FIELD_ZERO;
+ else if (values[x] < 8 && values[x] >= -8)
+ selector |= FIELD_4BIT;
+ else if (values[x] < 128 && values[x] >= -128)
+ selector |= FIELD_8BIT;
+ else
+ selector |= FIELD_16BIT;
+ }
+
+ blackboxWrite(selector);
+
+ nibbleIndex = 0;
+ buffer = 0;
+ for (x = 0; x < 4; x++, selector >>= 2) {
+ switch (selector & 0x03) {
+ case FIELD_ZERO:
+ //No-op
+ break;
+ case FIELD_4BIT:
+ if (nibbleIndex == 0) {
+ //We fill high-bits first
+ buffer = values[x] << 4;
+ nibbleIndex = 1;
+ } else {
+ blackboxWrite(buffer | (values[x] & 0x0F));
+ nibbleIndex = 0;
+ }
+ break;
+ case FIELD_8BIT:
+ if (nibbleIndex == 0)
+ blackboxWrite(values[x]);
+ else {
+ //Write the high bits of the value first (mask to avoid sign extension)
+ blackboxWrite(buffer | ((values[x] >> 4) & 0x0F));
+ //Now put the leftover low bits into the top of the next buffer entry
+ buffer = values[x] << 4;
+ }
+ break;
+ case FIELD_16BIT:
+ if (nibbleIndex == 0) {
+ //Write high byte first
+ blackboxWrite(values[x] >> 8);
+ blackboxWrite(values[x]);
+ } else {
+ //First write the highest 4 bits
+ blackboxWrite(buffer | ((values[x] >> 12) & 0x0F));
+ // Then the middle 8
+ blackboxWrite(values[x] >> 4);
+ //Only the smallest 4 bits are still left to write
+ buffer = values[x] << 4;
+ }
+ break;
+ }
+ }
+ //Anything left over to write?
+ if (nibbleIndex == 1)
+ blackboxWrite(buffer);
+}
+
+/**
+ * Write `valueCount` fields from `values` to the Blackbox using signed variable byte encoding. A 1-byte header is
+ * written first which specifies which fields are non-zero (so this encoding is compact when most fields are zero).
+ *
+ * valueCount must be 8 or less.
+ */
+static void writeTag8_8SVB(int32_t *values, int valueCount)
+{
+ uint8_t header;
+ int i;
+
+ if (valueCount > 0) {
+ //If we're only writing one field then we can skip the header
+ if (valueCount == 1) {
+ writeSignedVB(values[0]);
+ } else {
+ //First write a one-byte header that marks which fields are non-zero
+ header = 0;
+
+ // First field should be in low bits of header
+ for (i = valueCount - 1; i >= 0; i--) {
+ header <<= 1;
+
+ if (values[i] != 0)
+ header |= 0x01;
+ }
+
+ blackboxWrite(header);
+
+ for (i = 0; i < valueCount; i++)
+ if (values[i] != 0)
+ writeSignedVB(values[i]);
+ }
+ }
+}
+
+static bool testBlackboxCondition(FlightLogFieldCondition condition)
+{
+ switch (condition) {
+ case FLIGHT_LOG_FIELD_CONDITION_ALWAYS:
+ return true;
+
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7:
+ case FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8:
+ return motorCount >= condition - FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1 + 1;
+ case FLIGHT_LOG_FIELD_CONDITION_TRICOPTER:
+ return masterConfig.mixerMode == MIXER_TRI;
+
+ case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0:
+ case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_1:
+ case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_2:
+ return currentProfile->pidProfile.P8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0] != 0;
+
+ case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0:
+ case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_1:
+ case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_2:
+ return currentProfile->pidProfile.I8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0] != 0;
+
+ case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
+ case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
+ case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
+ return currentProfile->pidProfile.D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
+
+ case FLIGHT_LOG_FIELD_CONDITION_MAG:
+#ifdef MAG
+ return sensors(SENSOR_MAG);
+#else
+ return false;
+#endif
+
+ case FLIGHT_LOG_FIELD_CONDITION_BARO:
+#ifdef BARO
+ return sensors(SENSOR_BARO);
+#else
+ return false;
+#endif
+
+ case FLIGHT_LOG_FIELD_CONDITION_VBAT:
+ return feature(FEATURE_VBAT);
+
+ case FLIGHT_LOG_FIELD_CONDITION_NEVER:
+ return false;
+ default:
+ return false;
+ }
+}
+
+static void blackboxSetState(BlackboxState newState)
+{
+ //Perform initial setup required for the new state
+ switch (newState) {
+ case BLACKBOX_STATE_SEND_HEADER:
+ startTime = millis();
+ headerXmitIndex = 0;
+ break;
+ case BLACKBOX_STATE_SEND_FIELDINFO:
+ case BLACKBOX_STATE_SEND_GPS_G_HEADERS:
+ case BLACKBOX_STATE_SEND_GPS_H_HEADERS:
+ headerXmitIndex = 0;
+ fieldXmitIndex = -1;
+ break;
+ case BLACKBOX_STATE_SEND_SYSINFO:
+ headerXmitIndex = 0;
+ break;
+ case BLACKBOX_STATE_RUNNING:
+ blackboxIteration = 0;
+ blackboxPFrameIndex = 0;
+ blackboxIFrameIndex = 0;
+ break;
+ default:
+ ;
+ }
+ blackboxState = newState;
+}
+
+static void writeIntraframe(void)
+{
+ blackboxValues_t *blackboxCurrent = blackboxHistory[0];
+ int x;
+
+ blackboxWrite('I');
+
+ writeUnsignedVB(blackboxIteration);
+ writeUnsignedVB(blackboxCurrent->time);
+
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ writeSignedVB(blackboxCurrent->axisPID_P[x]);
+
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ writeSignedVB(blackboxCurrent->axisPID_I[x]);
+
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x))
+ writeSignedVB(blackboxCurrent->axisPID_D[x]);
+
+ for (x = 0; x < 3; x++)
+ writeSignedVB(blackboxCurrent->rcCommand[x]);
+
+ writeUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle]
+
+ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
+ /*
+ * Our voltage is expected to decrease over the course of the flight, so store our difference from
+ * the reference:
+ *
+ * Write 14 bits even if the number is negative (which would otherwise result in 32 bits)
+ */
+ writeUnsignedVB((vbatReference - blackboxCurrent->vbatLatest) & 0x3FFF);
+ }
+
+#ifdef MAG
+ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ writeSignedVB(blackboxCurrent->magADC[x]);
+ }
+#endif
+
+#ifdef BARO
+ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO))
+ writeSignedVB(blackboxCurrent->BaroAlt);
+#endif
+
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ writeSignedVB(blackboxCurrent->gyroData[x]);
+
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ writeSignedVB(blackboxCurrent->accSmooth[x]);
+
+ //Motors can be below minthrottle when disarmed, but that doesn't happen much
+ writeUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle);
+
+ //Motors tend to be similar to each other
+ for (x = 1; x < motorCount; x++)
+ writeSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
+
+ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER))
+ writeSignedVB(blackboxHistory[0]->servo[5] - 1500);
+
+ //Rotate our history buffers:
+
+ //The current state becomes the new "before" state
+ blackboxHistory[1] = blackboxHistory[0];
+ //And since we have no other history, we also use it for the "before, before" state
+ blackboxHistory[2] = blackboxHistory[0];
+ //And advance the current state over to a blank space ready to be filled
+ blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
+}
+
+static void writeInterframe(void)
+{
+ int x;
+ int32_t deltas[5];
+
+ blackboxValues_t *blackboxCurrent = blackboxHistory[0];
+ blackboxValues_t *blackboxLast = blackboxHistory[1];
+
+ blackboxWrite('P');
+
+ //No need to store iteration count since its delta is always 1
+
+ /*
+ * Since the difference between the difference between successive times will be nearly zero, use
+ * second-order differences.
+ */
+ writeSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
+
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ writeSignedVB(blackboxCurrent->axisPID_P[x] - blackboxLast->axisPID_P[x]);
+
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ deltas[x] = blackboxCurrent->axisPID_I[x] - blackboxLast->axisPID_I[x];
+
+ /*
+ * The PID I field changes very slowly, most of the time +-2, so use an encoding
+ * that can pack all three fields into one byte in that situation.
+ */
+ writeTag2_3S32(deltas);
+
+ /*
+ * The PID D term is frequently set to zero for yaw, which makes the result from the calculation
+ * always zero. So don't bother recording D results when PID D terms are zero.
+ */
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x))
+ writeSignedVB(blackboxCurrent->axisPID_D[x] - blackboxLast->axisPID_D[x]);
+
+ /*
+ * RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
+ * can pack multiple values per byte:
+ */
+ for (x = 0; x < 4; x++)
+ deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x];
+
+ writeTag8_4S16(deltas);
+
+ //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, MAG, BARO
+ int optionalFieldCount = 0;
+
+ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
+ deltas[optionalFieldCount++] = (int32_t) blackboxCurrent->vbatLatest - blackboxLast->vbatLatest;
+ }
+
+#ifdef MAG
+ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ deltas[optionalFieldCount++] = blackboxCurrent->magADC[x] - blackboxLast->magADC[x];
+ }
+#endif
+
+#ifdef BARO
+ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_BARO))
+ deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt;
+#endif
+ writeTag8_8SVB(deltas, optionalFieldCount);
+
+ //Since gyros, accs and motors are noisy, base the prediction on the average of the history:
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ writeSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2);
+
+ for (x = 0; x < XYZ_AXIS_COUNT; x++)
+ writeSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[x]) / 2);
+
+ for (x = 0; x < motorCount; x++)
+ writeSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2);
+
+ if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER))
+ writeSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]);
+
+ //Rotate our history buffers
+ blackboxHistory[2] = blackboxHistory[1];
+ blackboxHistory[1] = blackboxHistory[0];
+ blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
+}
+
+static int gcd(int num, int denom)
+{
+ if (denom == 0)
+ return num;
+ return gcd(denom, num % denom);
+}
+
+static void validateBlackboxConfig()
+{
+ int div;
+
+ if (masterConfig.blackbox_rate_num == 0 || masterConfig.blackbox_rate_denom == 0
+ || masterConfig.blackbox_rate_num >= masterConfig.blackbox_rate_denom) {
+ masterConfig.blackbox_rate_num = 1;
+ masterConfig.blackbox_rate_denom = 1;
+ } else {
+ div = gcd(masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
+
+ masterConfig.blackbox_rate_num /= div;
+ masterConfig.blackbox_rate_denom /= div;
+ }
+}
+
+static void configureBlackboxPort(void)
+{
+ blackboxPort = findOpenSerialPort(FUNCTION_BLACKBOX);
+ if (blackboxPort) {
+ previousPortMode = blackboxPort->mode;
+ previousBaudRate = blackboxPort->baudRate;
+
+ serialSetBaudRate(blackboxPort, BLACKBOX_BAUDRATE);
+ serialSetMode(blackboxPort, BLACKBOX_INITIAL_PORT_MODE);
+ beginSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX);
+ } else {
+ blackboxPort = openSerialPort(FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
+
+ if (blackboxPort) {
+ previousPortMode = blackboxPort->mode;
+ previousBaudRate = blackboxPort->baudRate;
+ }
+ }
+}
+
+static void releaseBlackboxPort(void)
+{
+ serialSetMode(blackboxPort, previousPortMode);
+ serialSetBaudRate(blackboxPort, previousBaudRate);
+
+ endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX);
+}
+
+void startBlackbox(void)
+{
+ if (blackboxState == BLACKBOX_STATE_STOPPED) {
+ validateBlackboxConfig();
+
+ configureBlackboxPort();
+
+ if (!blackboxPort) {
+ blackboxSetState(BLACKBOX_STATE_DISABLED);
+ return;
+ }
+
+ memset(&gpsHistory, 0, sizeof(gpsHistory));
+
+ blackboxHistory[0] = &blackboxHistoryRing[0];
+ blackboxHistory[1] = &blackboxHistoryRing[1];
+ blackboxHistory[2] = &blackboxHistoryRing[2];
+
+ vbatReference = vbatLatest;
+
+ //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it
+
+ blackboxSetState(BLACKBOX_STATE_SEND_HEADER);
+ }
+}
+
+void finishBlackbox(void)
+{
+ if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED) {
+ blackboxSetState(BLACKBOX_STATE_STOPPED);
+
+ releaseBlackboxPort();
+ }
+}
+
+static void writeGPSHomeFrame()
+{
+ blackboxWrite('H');
+
+ writeSignedVB(GPS_home[0]);
+ writeSignedVB(GPS_home[1]);
+ //TODO it'd be great if we could grab the GPS current time and write that too
+
+ gpsHistory.GPS_home[0] = GPS_home[0];
+ gpsHistory.GPS_home[1] = GPS_home[1];
+}
+
+static void writeGPSFrame()
+{
+ blackboxWrite('G');
+
+ writeUnsignedVB(GPS_numSat);
+ writeSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]);
+ writeSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]);
+ writeUnsignedVB(GPS_altitude);
+ writeUnsignedVB(GPS_speed);
+
+ gpsHistory.GPS_numSat = GPS_numSat;
+ gpsHistory.GPS_coord[0] = GPS_coord[0];
+ gpsHistory.GPS_coord[1] = GPS_coord[1];
+}
+
+/**
+ * Fill the current state of the blackbox using values read from the flight controller
+ */
+static void loadBlackboxState(void)
+{
+ blackboxValues_t *blackboxCurrent = blackboxHistory[0];
+ int i;
+
+ blackboxCurrent->time = currentTime;
+
+ for (i = 0; i < XYZ_AXIS_COUNT; i++)
+ blackboxCurrent->axisPID_P[i] = axisPID_P[i];
+ for (i = 0; i < XYZ_AXIS_COUNT; i++)
+ blackboxCurrent->axisPID_I[i] = axisPID_I[i];
+ for (i = 0; i < XYZ_AXIS_COUNT; i++)
+ blackboxCurrent->axisPID_D[i] = axisPID_D[i];
+
+ for (i = 0; i < 4; i++)
+ blackboxCurrent->rcCommand[i] = rcCommand[i];
+
+ for (i = 0; i < XYZ_AXIS_COUNT; i++)
+ blackboxCurrent->gyroData[i] = gyroData[i];
+
+ for (i = 0; i < XYZ_AXIS_COUNT; i++)
+ blackboxCurrent->accSmooth[i] = accSmooth[i];
+
+ for (i = 0; i < motorCount; i++)
+ blackboxCurrent->motor[i] = motor[i];
+
+ blackboxCurrent->vbatLatest = vbatLatest;
+
+#ifdef MAG
+ for (i = 0; i < XYZ_AXIS_COUNT; i++)
+ blackboxCurrent->magADC[i] = magADC[i];
+#endif
+
+#ifdef BARO
+ blackboxCurrent->BaroAlt = BaroAlt;
+#endif
+
+ //Tail servo for tricopters
+ blackboxCurrent->servo[5] = servo[5];
+}
+
+/**
+ * Transmit the header information for the given field definitions. Transmitted header lines look like:
+ *
+ * H Field I name:a,b,c
+ * H Field I predictor:0,1,2
+ *
+ * Provide an array 'conditions' of FlightLogFieldCondition enums if you want these conditions to decide whether a field
+ * should be included or not. Otherwise provide NULL for this parameter and NULL for secondCondition.
+ *
+ * Set headerXmitIndex to 0 and fieldXmitIndex to -1 before calling for the first time.
+ *
+ * secondFieldDefinition and secondCondition element pointers need to be provided in order to compute the stride of the
+ * fieldDefinition and secondCondition arrays.
+ *
+ * Returns true if there is still header left to transmit (so call again to continue transmission).
+ */
+static bool sendFieldDefinition(const char * const *headerNames, unsigned int headerCount, const void *fieldDefinitions,
+ const void *secondFieldDefinition, int fieldCount, const uint8_t *conditions, const uint8_t *secondCondition)
+{
+ const blackboxFieldDefinition_t *def;
+ int charsWritten;
+ static bool needComma = false;
+ size_t definitionStride = (char*) secondFieldDefinition - (char*) fieldDefinitions;
+ size_t conditionsStride = (char*) secondCondition - (char*) conditions;
+
+ /*
+ * We're chunking up the header data so we don't exceed our datarate. So we'll be called multiple times to transmit
+ * the whole header.
+ */
+ if (fieldXmitIndex == -1) {
+ if (headerXmitIndex >= headerCount)
+ return false; //Someone probably called us again after we had already completed transmission
+
+ charsWritten = blackboxPrint("H Field ");
+ charsWritten += blackboxPrint(headerNames[headerXmitIndex]);
+ charsWritten += blackboxPrint(":");
+
+ fieldXmitIndex++;
+ needComma = false;
+ } else
+ charsWritten = 0;
+
+ for (; fieldXmitIndex < fieldCount && charsWritten < SERIAL_CHUNK_SIZE; fieldXmitIndex++) {
+ def = (const blackboxFieldDefinition_t*) ((const char*)fieldDefinitions + definitionStride * fieldXmitIndex);
+
+ if (!conditions || testBlackboxCondition(conditions[conditionsStride * fieldXmitIndex])) {
+ if (needComma) {
+ blackboxWrite(',');
+ charsWritten++;
+ } else
+ needComma = true;
+
+ // The first header is a field name
+ if (headerXmitIndex == 0) {
+ charsWritten += blackboxPrint(def->name);
+ } else {
+ //The other headers are integers
+ if (def->arr[headerXmitIndex - 1] >= 10) {
+ blackboxWrite(def->arr[headerXmitIndex - 1] / 10 + '0');
+ blackboxWrite(def->arr[headerXmitIndex - 1] % 10 + '0');
+ charsWritten += 2;
+ } else {
+ blackboxWrite(def->arr[headerXmitIndex - 1] + '0');
+ charsWritten++;
+ }
+ }
+ }
+ }
+
+ // Did we complete this line?
+ if (fieldXmitIndex == fieldCount) {
+ blackboxWrite('\n');
+ headerXmitIndex++;
+ fieldXmitIndex = -1;
+ }
+
+ return headerXmitIndex < headerCount;
+}
+
+/**
+ * Transmit a portion of the system information headers. Begin with a xmitIndex of 0. Returns the next xmitIndex to
+ * call with, or -1 if transmission is complete.
+ */
+static int blackboxWriteSysinfo(int xmitIndex)
+{
+ union floatConvert_t {
+ float f;
+ uint32_t u;
+ } floatConvert;
+
+ switch (xmitIndex) {
+ case 0:
+ blackboxPrintf("H Firmware type:Cleanflight\n");
+ break;
+ case 1:
+ // Pause to allow more time for previous to transmit (it exceeds our chunk size)
+ break;
+ case 2:
+ blackboxPrintf("H Firmware revision:%s\n", shortGitRevision);
+ break;
+ case 3:
+ // Pause to allow more time for previous to transmit
+ break;
+ case 4:
+ blackboxPrintf("H Firmware date:%s %s\n", buildDate, buildTime);
+ break;
+ case 5:
+ // Pause to allow more time for previous to transmit
+ break;
+ case 6:
+ blackboxPrintf("H P interval:%d/%d\n", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
+ break;
+ case 7:
+ blackboxPrintf("H rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8);
+ break;
+ case 8:
+ blackboxPrintf("H minthrottle:%d\n", masterConfig.escAndServoConfig.minthrottle);
+ break;
+ case 9:
+ blackboxPrintf("H maxthrottle:%d\n", masterConfig.escAndServoConfig.maxthrottle);
+ break;
+ case 10:
+ floatConvert.f = gyro.scale;
+ blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u);
+ break;
+ case 11:
+ blackboxPrintf("H acc_1G:%u\n", acc_1G);
+ break;
+ case 12:
+ blackboxPrintf("H vbatscale:%u\n", masterConfig.batteryConfig.vbatscale);
+ break;
+ case 13:
+ blackboxPrintf("H vbatcellvoltage:%u,%u,%u\n", masterConfig.batteryConfig.vbatmincellvoltage,
+ masterConfig.batteryConfig.vbatwarningcellvoltage, masterConfig.batteryConfig.vbatmaxcellvoltage);
+ break;
+ case 14:
+ //Pause
+ break;
+ case 15:
+ blackboxPrintf("H vbatref:%u\n", vbatReference);
+ break;
+ case 16:
+ // One more pause for good luck
+ break;
+ default:
+ return -1;
+ }
+
+ return xmitIndex + 1;
+}
+
+// Beep the buzzer and write the current time to the log as a synchronization point
+static void blackboxPlaySyncBeep()
+{
+ uint32_t now = micros();
+
+ /*
+ * The regular beep routines aren't going to work for us, because they queue up the beep to be executed later.
+ * Our beep is timing sensitive, so start beeping now without setting the beeperIsOn flag.
+ */
+ BEEP_ON;
+
+ // Have the regular beeper code turn off the beep for us eventually, since that's not timing-sensitive
+ queueConfirmationBeep(1);
+
+ blackboxWrite('E');
+ blackboxWrite(FLIGHT_LOG_EVENT_SYNC_BEEP);
+
+ writeUnsignedVB(now);
+}
+
+void handleBlackbox(void)
+{
+ int i, result;
+
+ switch (blackboxState) {
+ case BLACKBOX_STATE_SEND_HEADER:
+ //On entry of this state, headerXmitIndex is 0 and startTime is intialised
+
+ /*
+ * Once the UART has had time to init, transmit the header in chunks so we don't overflow our transmit
+ * buffer.
+ */
+ if (millis() > startTime + 100) {
+ for (i = 0; i < SERIAL_CHUNK_SIZE && blackboxHeader[headerXmitIndex] != '\0'; i++, headerXmitIndex++)
+ blackboxWrite(blackboxHeader[headerXmitIndex]);
+
+ if (blackboxHeader[headerXmitIndex] == '\0')
+ blackboxSetState(BLACKBOX_STATE_SEND_FIELDINFO);
+ }
+ break;
+ case BLACKBOX_STATE_SEND_FIELDINFO:
+ //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1
+ if (!sendFieldDefinition(blackboxMainHeaderNames, ARRAY_LENGTH(blackboxMainHeaderNames), blackboxMainFields, blackboxMainFields + 1,
+ ARRAY_LENGTH(blackboxMainFields), &blackboxMainFields[0].condition, &blackboxMainFields[1].condition)) {
+ if (feature(FEATURE_GPS))
+ blackboxSetState(BLACKBOX_STATE_SEND_GPS_H_HEADERS);
+ else
+ blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
+ }
+ break;
+ case BLACKBOX_STATE_SEND_GPS_H_HEADERS:
+ //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1
+ if (!sendFieldDefinition(blackboxGPSHHeaderNames, ARRAY_LENGTH(blackboxGPSHHeaderNames), blackboxGpsHFields, blackboxGpsHFields + 1,
+ ARRAY_LENGTH(blackboxGpsHFields), NULL, 0)) {
+ blackboxSetState(BLACKBOX_STATE_SEND_GPS_G_HEADERS);
+ }
+ break;
+ case BLACKBOX_STATE_SEND_GPS_G_HEADERS:
+ //On entry of this state, headerXmitIndex is 0 and fieldXmitIndex is -1
+ if (!sendFieldDefinition(blackboxGPSGHeaderNames, ARRAY_LENGTH(blackboxGPSGHeaderNames), blackboxGpsGFields, blackboxGpsGFields + 1,
+ ARRAY_LENGTH(blackboxGpsGFields), NULL, 0)) {
+ blackboxSetState(BLACKBOX_STATE_SEND_SYSINFO);
+ }
+ break;
+ case BLACKBOX_STATE_SEND_SYSINFO:
+ //On entry of this state, headerXmitIndex is 0
+ result = blackboxWriteSysinfo(headerXmitIndex);
+
+ if (result == -1)
+ blackboxSetState(BLACKBOX_STATE_PRERUN);
+ else
+ headerXmitIndex = result;
+ break;
+ case BLACKBOX_STATE_PRERUN:
+ blackboxPlaySyncBeep();
+
+ blackboxSetState(BLACKBOX_STATE_RUNNING);
+ break;
+ case BLACKBOX_STATE_RUNNING:
+ // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
+
+ // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
+ if (blackboxPFrameIndex == 0) {
+ // Copy current system values into the blackbox
+ loadBlackboxState();
+ writeIntraframe();
+ } else {
+ if ((blackboxPFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num) {
+ loadBlackboxState();
+ writeInterframe();
+ }
+
+ if (feature(FEATURE_GPS)) {
+ /*
+ * If the GPS home point has been updated, or every 128 intraframes (~10 seconds), write the
+ * GPS home position.
+ *
+ * We write it periodically so that if one Home Frame goes missing, the GPS coordinates can
+ * still be interpreted correctly.
+ */
+ if (GPS_home[0] != gpsHistory.GPS_home[0] || GPS_home[1] != gpsHistory.GPS_home[1]
+ || (blackboxPFrameIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIFrameIndex % 128 == 0)) {
+
+ writeGPSHomeFrame();
+ writeGPSFrame();
+ } else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
+ || GPS_coord[1] != gpsHistory.GPS_coord[1]) {
+ //We could check for velocity changes as well but I doubt it changes independent of position
+ writeGPSFrame();
+ }
+ }
+ }
+
+ blackboxIteration++;
+ blackboxPFrameIndex++;
+
+ if (blackboxPFrameIndex == BLACKBOX_I_INTERVAL) {
+ blackboxPFrameIndex = 0;
+ blackboxIFrameIndex++;
+ }
+ break;
+ default:
+ break;
+ }
+}
+
+static bool canUseBlackboxWithCurrentConfiguration(void)
+{
+ if (!feature(FEATURE_BLACKBOX))
+ return false;
+
+ return true;
+}
+
+void initBlackbox(void)
+{
+ if (canUseBlackboxWithCurrentConfiguration())
+ blackboxSetState(BLACKBOX_STATE_STOPPED);
+ else
+ blackboxSetState(BLACKBOX_STATE_DISABLED);
+}
diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h
index 36b006c0ad..5d30671bbf 100644
--- a/src/main/blackbox/blackbox.h
+++ b/src/main/blackbox/blackbox.h
@@ -1,47 +1,47 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight. If not, see .
- */
-
-#pragma once
-
-#include "common/axis.h"
-#include
-
-typedef struct blackboxValues_t {
- uint32_t time;
-
- int32_t axisPID_P[XYZ_AXIS_COUNT], axisPID_I[XYZ_AXIS_COUNT], axisPID_D[XYZ_AXIS_COUNT];
-
- int16_t rcCommand[4];
- int16_t gyroData[XYZ_AXIS_COUNT];
- int16_t accSmooth[XYZ_AXIS_COUNT];
- int16_t motor[MAX_SUPPORTED_MOTORS];
- int16_t servo[MAX_SUPPORTED_SERVOS];
-
- uint16_t vbatLatest;
-
-#ifdef BARO
- int32_t BaroAlt;
-#endif
-#ifdef MAG
- int16_t magADC[XYZ_AXIS_COUNT];
-#endif
-} blackboxValues_t;
-
-void initBlackbox(void);
-void handleBlackbox(void);
-void startBlackbox(void);
-void finishBlackbox(void);
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+#include "common/axis.h"
+#include
+
+typedef struct blackboxValues_t {
+ uint32_t time;
+
+ int32_t axisPID_P[XYZ_AXIS_COUNT], axisPID_I[XYZ_AXIS_COUNT], axisPID_D[XYZ_AXIS_COUNT];
+
+ int16_t rcCommand[4];
+ int16_t gyroData[XYZ_AXIS_COUNT];
+ int16_t accSmooth[XYZ_AXIS_COUNT];
+ int16_t motor[MAX_SUPPORTED_MOTORS];
+ int16_t servo[MAX_SUPPORTED_SERVOS];
+
+ uint16_t vbatLatest;
+
+#ifdef BARO
+ int32_t BaroAlt;
+#endif
+#ifdef MAG
+ int16_t magADC[XYZ_AXIS_COUNT];
+#endif
+} blackboxValues_t;
+
+void initBlackbox(void);
+void handleBlackbox(void);
+void startBlackbox(void);
+void finishBlackbox(void);
diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h
index 68a75439f7..87db8ecbbb 100644
--- a/src/main/blackbox/blackbox_fielddefs.h
+++ b/src/main/blackbox/blackbox_fielddefs.h
@@ -1,100 +1,100 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight. If not, see .
- */
-
-#pragma once
-
-typedef enum FlightLogFieldCondition {
- FLIGHT_LOG_FIELD_CONDITION_ALWAYS = 0,
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1,
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2,
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3,
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4,
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5,
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6,
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7,
- FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8,
- FLIGHT_LOG_FIELD_CONDITION_TRICOPTER,
-
- FLIGHT_LOG_FIELD_CONDITION_MAG = 20,
- FLIGHT_LOG_FIELD_CONDITION_BARO,
- FLIGHT_LOG_FIELD_CONDITION_VBAT,
-
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0 = 40,
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_1,
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_2,
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0,
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_1,
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_2,
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0,
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1,
- FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2,
-
- FLIGHT_LOG_FIELD_CONDITION_NEVER = 255,
-} FlightLogFieldCondition;
-
-typedef enum FlightLogFieldPredictor {
- //No prediction:
- FLIGHT_LOG_FIELD_PREDICTOR_0 = 0,
-
- //Predict that the field is the same as last frame:
- FLIGHT_LOG_FIELD_PREDICTOR_PREVIOUS = 1,
-
- //Predict that the slope between this field and the previous item is the same as that between the past two history items:
- FLIGHT_LOG_FIELD_PREDICTOR_STRAIGHT_LINE = 2,
-
- //Predict that this field is the same as the average of the last two history items:
- FLIGHT_LOG_FIELD_PREDICTOR_AVERAGE_2 = 3,
-
- //Predict that this field is minthrottle
- FLIGHT_LOG_FIELD_PREDICTOR_MINTHROTTLE = 4,
-
- //Predict that this field is the same as motor 0
- FLIGHT_LOG_FIELD_PREDICTOR_MOTOR_0 = 5,
-
- //This field always increments
- FLIGHT_LOG_FIELD_PREDICTOR_INC = 6,
-
- //Predict this GPS co-ordinate is the GPS home co-ordinate (or no prediction if that coordinate is not set)
- FLIGHT_LOG_FIELD_PREDICTOR_HOME_COORD = 7,
-
- //Predict 1500
- FLIGHT_LOG_FIELD_PREDICTOR_1500 = 8,
-
- //Predict vbatref, the reference ADC level stored in the header
- FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9
-
-} FlightLogFieldPredictor;
-
-typedef enum FlightLogFieldEncoding {
- FLIGHT_LOG_FIELD_ENCODING_SIGNED_VB = 0, // Signed variable-byte
- FLIGHT_LOG_FIELD_ENCODING_UNSIGNED_VB = 1, // Unsigned variable-byte
- FLIGHT_LOG_FIELD_ENCODING_NEG_14BIT = 3, // Unsigned variable-byte but we negate the value before storing, value is 14 bits
- FLIGHT_LOG_FIELD_ENCODING_TAG8_8SVB = 6,
- FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 = 7,
- FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 = 8,
- FLIGHT_LOG_FIELD_ENCODING_NULL = 9 // Nothing is written to the file, take value to be zero
-} FlightLogFieldEncoding;
-
-typedef enum FlightLogFieldSign {
- FLIGHT_LOG_FIELD_UNSIGNED = 0,
- FLIGHT_LOG_FIELD_SIGNED = 1
-} FlightLogFieldSign;
-
-typedef enum FlightLogEvent {
- FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
- FLIGHT_LOG_EVENT_LOG_END = 255
-} FlightLogEvent;
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+typedef enum FlightLogFieldCondition {
+ FLIGHT_LOG_FIELD_CONDITION_ALWAYS = 0,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_1,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_2,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_3,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_4,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_5,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_6,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_7,
+ FLIGHT_LOG_FIELD_CONDITION_AT_LEAST_MOTORS_8,
+ FLIGHT_LOG_FIELD_CONDITION_TRICOPTER,
+
+ FLIGHT_LOG_FIELD_CONDITION_MAG = 20,
+ FLIGHT_LOG_FIELD_CONDITION_BARO,
+ FLIGHT_LOG_FIELD_CONDITION_VBAT,
+
+ FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_0 = 40,
+ FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_1,
+ FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_P_2,
+ FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_0,
+ FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_1,
+ FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_I_2,
+ FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0,
+ FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1,
+ FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2,
+
+ FLIGHT_LOG_FIELD_CONDITION_NEVER = 255,
+} FlightLogFieldCondition;
+
+typedef enum FlightLogFieldPredictor {
+ //No prediction:
+ FLIGHT_LOG_FIELD_PREDICTOR_0 = 0,
+
+ //Predict that the field is the same as last frame:
+ FLIGHT_LOG_FIELD_PREDICTOR_PREVIOUS = 1,
+
+ //Predict that the slope between this field and the previous item is the same as that between the past two history items:
+ FLIGHT_LOG_FIELD_PREDICTOR_STRAIGHT_LINE = 2,
+
+ //Predict that this field is the same as the average of the last two history items:
+ FLIGHT_LOG_FIELD_PREDICTOR_AVERAGE_2 = 3,
+
+ //Predict that this field is minthrottle
+ FLIGHT_LOG_FIELD_PREDICTOR_MINTHROTTLE = 4,
+
+ //Predict that this field is the same as motor 0
+ FLIGHT_LOG_FIELD_PREDICTOR_MOTOR_0 = 5,
+
+ //This field always increments
+ FLIGHT_LOG_FIELD_PREDICTOR_INC = 6,
+
+ //Predict this GPS co-ordinate is the GPS home co-ordinate (or no prediction if that coordinate is not set)
+ FLIGHT_LOG_FIELD_PREDICTOR_HOME_COORD = 7,
+
+ //Predict 1500
+ FLIGHT_LOG_FIELD_PREDICTOR_1500 = 8,
+
+ //Predict vbatref, the reference ADC level stored in the header
+ FLIGHT_LOG_FIELD_PREDICTOR_VBATREF = 9
+
+} FlightLogFieldPredictor;
+
+typedef enum FlightLogFieldEncoding {
+ FLIGHT_LOG_FIELD_ENCODING_SIGNED_VB = 0, // Signed variable-byte
+ FLIGHT_LOG_FIELD_ENCODING_UNSIGNED_VB = 1, // Unsigned variable-byte
+ FLIGHT_LOG_FIELD_ENCODING_NEG_14BIT = 3, // Unsigned variable-byte but we negate the value before storing, value is 14 bits
+ FLIGHT_LOG_FIELD_ENCODING_TAG8_8SVB = 6,
+ FLIGHT_LOG_FIELD_ENCODING_TAG2_3S32 = 7,
+ FLIGHT_LOG_FIELD_ENCODING_TAG8_4S16 = 8,
+ FLIGHT_LOG_FIELD_ENCODING_NULL = 9 // Nothing is written to the file, take value to be zero
+} FlightLogFieldEncoding;
+
+typedef enum FlightLogFieldSign {
+ FLIGHT_LOG_FIELD_UNSIGNED = 0,
+ FLIGHT_LOG_FIELD_SIGNED = 1
+} FlightLogFieldSign;
+
+typedef enum FlightLogEvent {
+ FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
+ FLIGHT_LOG_EVENT_LOG_END = 255
+} FlightLogEvent;