diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 87fc45a08f..baad91d3fc 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1,982 +1,982 @@ -/* - * 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/accgyro.h" -#include "drivers/light_led.h" - -#include "drivers/gpio.h" -#include "drivers/system.h" -#include "drivers/serial.h" -#include "drivers/timer.h" -#include "drivers/pwm_rx.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 - -// Some macros to make writing FLIGHT_LOG_FIELD_PREDICTOR_* 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) STR(CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)) -#define ENCODING(x) STR(CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)) - -static const char blackboxHeader[] = - "H Product:Blackbox flight data recorder by Nicholas Sherlock\n" - "H Blackbox version:1\n" - "H Data version:1\n" - "H I interval:" STR(BLACKBOX_I_INTERVAL) "\n"; - -/* These headers have info for all 8 motors on them, we'll trim the final fields off to match the number of motors in the mixer: */ -static const char * const blackboxHeaderFields[] = { - "H Field I name:" - "loopIteration,time," - "axisP[0],axisP[1],axisP[2]," - "axisI[0],axisI[1],axisI[2]," - "axisD[0],axisD[1],axisD[2]," - "rcCommand[0],rcCommand[1],rcCommand[2],rcCommand[3]," - "gyroData[0],gyroData[1],gyroData[2]," - "accSmooth[0],accSmooth[1],accSmooth[2]," - "motor[0],motor[1],motor[2],motor[3]," - "motor[4],motor[5],motor[6],motor[7]", - - "H Field I signed:" - /* loopIteration, time: */ - "0,0," - /* PIDs: */ - "1,1,1,1,1,1,1,1,1," - /* rcCommand[0..2] */ - "1,1,1," - /* rcCommand[3] (Throttle): */ - "0," - /* gyroData[0..2]: */ - "1,1,1," - /* accSmooth[0..2]: */ - "1,1,1," - /* Motor[0..7]: */ - "0,0,0,0,0,0,0,0", - - "H Field I predictor:" - /* loopIteration, time: */ - PREDICT(0) "," PREDICT(0) "," - /* PIDs: */ - PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," - PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," - PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," - /* rcCommand[0..2] */ - PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," - /* rcCommand[3] (Throttle): */ - PREDICT(MINTHROTTLE) "," - /* gyroData[0..2]: */ - PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," - /* accSmooth[0..2]: */ - PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," - /* Motor[0]: */ - PREDICT(MINTHROTTLE) "," - /* Motor[1..7]: */ - PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," - PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," - PREDICT(MOTOR_0), - - "H Field I encoding:" - /* loopIteration, time: */ - ENCODING(UNSIGNED_VB) "," ENCODING(UNSIGNED_VB) "," - /* PIDs: */ - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - /* rcCommand[0..2] */ - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - /* rcCommand[3] (Throttle): */ - ENCODING(UNSIGNED_VB) "," - /* gyroData[0..2]: */ - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - /* accSmooth[0..2]: */ - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - /* Motor[0]: */ - ENCODING(UNSIGNED_VB) "," - /* Motor[1..7]: */ - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - ENCODING(SIGNED_VB), - - //Motors and gyros predict an average of the last two measurements (to reduce the impact of noise): - "H Field P predictor:" - /* loopIteration, time: */ - PREDICT(INC) "," PREDICT(STRAIGHT_LINE) "," - /* PIDs: */ - PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," - PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," - PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," - /* rcCommand[0..2] */ - PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," - /* rcCommand[3] (Throttle): */ - PREDICT(PREVIOUS) "," - /* gyroData[0..2]: */ - PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," - /* accSmooth[0..2]: */ - PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," - /* Motor[0]: */ - PREDICT(AVERAGE_2) "," - /* Motor[1..7]: */ - PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," - PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," - PREDICT(AVERAGE_2), - - /* RC fields are encoded together as a group, everything else is signed since they're diffs: */ - "H Field P encoding:" - /* loopIteration, time: */ - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - /* PIDs: */ - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - ENCODING(TAG2_3S32) "," ENCODING(TAG2_3S32) "," ENCODING(TAG2_3S32) "," - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - /* rcCommand[0..3] */ - ENCODING(TAG8_4S16) "," ENCODING(TAG8_4S16) "," ENCODING(TAG8_4S16) "," - ENCODING(TAG8_4S16) "," - /* gyroData[0..2]: */ - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - /* accSmooth[0..2]: */ - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - /* Motor[0]: */ - ENCODING(SIGNED_VB) "," - /* Motor[1..7]: */ - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - ENCODING(SIGNED_VB) -}; - -/** - * Additional fields to tack on to those above for tricopters (to record tail servo position) - */ -static const char * const blackboxAdditionalFieldsTricopter[] = { - //Field I name - "servo[5]", - - //Field I signed - "0", - - //Field I predictor - PREDICT(1500), - - //Field I encoding: - ENCODING(SIGNED_VB), - - //Field P predictor: - PREDICT(PREVIOUS), - - //Field P encoding: - ENCODING(SIGNED_VB) -}; - -static const char blackboxGpsHeader[] = - "H Field G name:" - "GPS_numSat,GPS_coord[0],GPS_coord[1],GPS_altitude,GPS_speed\n" - "H Field G signed:" - "0,1,1,0,0\n" - "H Field G predictor:" - PREDICT(0) "," PREDICT(HOME_COORD) "," PREDICT(HOME_COORD) "," PREDICT(0) "," PREDICT(0) "\n" - "H Field G encoding:" - ENCODING(UNSIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," - ENCODING(UNSIGNED_VB) "," ENCODING(UNSIGNED_VB) "\n" - - "H Field H name:" - "GPS_home[0],GPS_home[1]\n" - "H Field H signed:" - "1,1\n" - "H Field H predictor:" - PREDICT(0) "," PREDICT(0) "\n" - "H Field H encoding:" - ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "\n"; - -typedef enum BlackboxState { - BLACKBOX_STATE_DISABLED = 0, - BLACKBOX_STATE_STOPPED, - BLACKBOX_STATE_SEND_HEADER, - BLACKBOX_STATE_SEND_FIELDINFO, - BLACKBOX_STATE_SEND_GPS_HEADERS, - BLACKBOX_STATE_SEND_SYSINFO, - 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 BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; -static uint32_t startTime; -static unsigned int headerXmitIndex; -static uint32_t blackboxIteration; - -static serialPort_t *blackboxPort; -static portMode_t previousPortMode; -static uint32_t previousBaudRate; - -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 int isTricopter() -{ - return masterConfig.mixerConfiguration == MULTITYPE_TRI; -} - -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); -} - -/** - * 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}; - enum { BITS_4 = 1}; - enum { BITS_6 = 2}; - enum { BITS_32 = 3}; - - enum { BYTES_1 = 0}; - enum { BYTES_2 = 1}; - enum { BYTES_3 = 2}; - enum { 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}; - enum { FIELD_4BIT = 1}; - enum { FIELD_8BIT = 2}; - enum { FIELD_16BIT = 3}; - - uint8_t selector; - int x; - - /* - * 4-bit fields can only be combined with their paired neighbor (there are two pairs), so choose a - * larger encoding if that's not possible. - */ - const uint8_t rcSelectorCleanup[16] = { - // Output selectors <- Input selectors - FIELD_ZERO << 2 | FIELD_ZERO, // zero, zero - FIELD_ZERO << 2 | FIELD_8BIT, // zero, 4-bit - FIELD_ZERO << 2 | FIELD_8BIT, // zero, 8-bit - FIELD_ZERO << 2 | FIELD_16BIT, // zero, 16-bit - FIELD_8BIT << 2 | FIELD_ZERO, // 4-bit, zero - FIELD_4BIT << 2 | FIELD_4BIT, // 4-bit, 4-bit - FIELD_8BIT << 2 | FIELD_8BIT, // 4-bit, 8-bit - FIELD_8BIT << 2 | FIELD_16BIT, // 4-bit, 16-bit - FIELD_8BIT << 2 | FIELD_ZERO, // 8-bit, zero - FIELD_8BIT << 2 | FIELD_8BIT, // 8-bit, 4-bit - FIELD_8BIT << 2 | FIELD_8BIT, // 8-bit, 8-bit - FIELD_8BIT << 2 | FIELD_16BIT, // 8-bit, 16-bit - FIELD_16BIT << 2 | FIELD_ZERO, // 16-bit, zero - FIELD_16BIT << 2 | FIELD_8BIT, // 16-bit, 4-bit - FIELD_16BIT << 2 | FIELD_8BIT, // 16-bit, 8-bit - FIELD_16BIT << 2 | FIELD_16BIT, // 16-bit, 16-bit - }; - - 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] <= 7 && values[x] >= -8) - selector |= FIELD_4BIT; - else if (values[x] <= 127 && values[x] >= -128) - selector |= FIELD_8BIT; - else - selector |= FIELD_16BIT; - } - - selector = rcSelectorCleanup[selector & 0x0F] | (rcSelectorCleanup[selector >> 4] << 4); - - blackboxWrite(selector); - - for (x = 0; x < 4; x++, selector >>= 2) { - switch (selector & 0x03) { - case FIELD_4BIT: - blackboxWrite((values[x] & 0x0F) | (values[x + 1] << 4)); - - //We write two selector fields: - x++; - selector >>= 2; - break; - case FIELD_8BIT: - blackboxWrite(values[x]); - break; - case FIELD_16BIT: - blackboxWrite(values[x]); - blackboxWrite(values[x] >> 8); - break; - } - } -} - -static void writeIntraframe(void) -{ - blackboxValues_t *blackboxCurrent = blackboxHistory[0]; - int x; - - blackboxWrite('I'); - - writeUnsignedVB(blackboxIteration); - writeUnsignedVB(blackboxCurrent->time); - - for (x = 0; x < 3; x++) - writeSignedVB(blackboxCurrent->axisP[x]); - - for (x = 0; x < 3; x++) - writeSignedVB(blackboxCurrent->axisI[x]); - - for (x = 0; x < 3; x++) - writeSignedVB(blackboxCurrent->axisD[x]); - - for (x = 0; x < 3; x++) - writeSignedVB(blackboxCurrent->rcCommand[x]); - - writeUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle] - - for (x = 0; x < 3; x++) - writeSignedVB(blackboxCurrent->gyroData[x]); - - for (x = 0; x < 3; 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 (isTricopter()) - 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[4]; - - 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 < 3; x++) - writeSignedVB(blackboxCurrent->axisP[x] - blackboxLast->axisP[x]); - - for (x = 0; x < 3; x++) - deltas[x] = blackboxCurrent->axisI[x] - blackboxLast->axisI[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); - - for (x = 0; x < 3; x++) - writeSignedVB(blackboxCurrent->axisD[x] - blackboxLast->axisD[x]); - - for (x = 0; x < 4; x++) - deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[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: - */ - writeTag8_4S16(deltas); - - //Since gyros, accs and motors are noisy, base the prediction on the average of the history: - for (x = 0; x < 3; x++) - writeSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2); - - for (x = 0; x < 3; 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 (isTricopter()) - 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) { - blackboxState = BLACKBOX_STATE_DISABLED; - return; - } - - startTime = millis(); - headerXmitIndex = 0; - blackboxIteration = 0; - blackboxState = BLACKBOX_STATE_SEND_HEADER; - - memset(&gpsHistory, 0, sizeof(gpsHistory)); - - //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it - - blackboxHistory[0] = &blackboxHistoryRing[0]; - blackboxHistory[1] = &blackboxHistoryRing[1]; - blackboxHistory[2] = &blackboxHistoryRing[2]; - } -} - -void finishBlackbox(void) -{ - if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED) { - blackboxState = 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 < 3; i++) - blackboxCurrent->axisP[i] = axisP[i]; - for (i = 0; i < 3; i++) - blackboxCurrent->axisI[i] = axisI[i]; - for (i = 0; i < 3; i++) - blackboxCurrent->axisD[i] = axisD[i]; - - for (i = 0; i < 4; i++) - blackboxCurrent->rcCommand[i] = rcCommand[i]; - - for (i = 0; i < 3; i++) - blackboxCurrent->gyroData[i] = gyroData[i]; - - for (i = 0; i < 3; i++) - blackboxCurrent->accSmooth[i] = accSmooth[i]; - - for (i = 0; i < motorCount; i++) - blackboxCurrent->motor[i] = motor[i]; - - //Tail servo for tricopters - blackboxCurrent->servo[5] = servo[5]; -} - -void handleBlackbox(void) -{ - int i; - const char *additionalHeader; - const int SERIAL_CHUNK_SIZE = 16; - static int charXmitIndex = 0; - int motorsToRemove, endIndex; - int blackboxIntercycleIndex, blackboxIntracycleIndex; - - union floatConvert_t { - float f; - uint32_t u; - } floatConvert; - - switch (blackboxState) { - case BLACKBOX_STATE_SEND_HEADER: - /* - * 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') { - blackboxState = BLACKBOX_STATE_SEND_FIELDINFO; - headerXmitIndex = 0; - charXmitIndex = 0; - } - } - break; - case BLACKBOX_STATE_SEND_FIELDINFO: - /* - * Send information about the fields we're recording to the log. - * - * Once again, we're chunking up the data so we don't exceed our datarate. This time we're trimming the - * excess field defs off the end of the header for motors we don't have. - */ - motorsToRemove = 8 - motorCount; - - if (headerXmitIndex < sizeof(blackboxHeaderFields) / sizeof(blackboxHeaderFields[0])){ - endIndex = strlen(blackboxHeaderFields[headerXmitIndex]) - (headerXmitIndex == 0 ? strlen(",motor[x]") : strlen(",x")) * motorsToRemove; - - for (i = charXmitIndex; i < charXmitIndex + SERIAL_CHUNK_SIZE && i < endIndex; i++) - blackboxWrite(blackboxHeaderFields[headerXmitIndex][i]); - - // Did we complete this line? - if (i == endIndex) { - if (isTricopter()) { - //Add fields to the end for the tail servo - blackboxWrite(','); - - for (additionalHeader = blackboxAdditionalFieldsTricopter[headerXmitIndex]; *additionalHeader; additionalHeader++) - blackboxWrite(*additionalHeader); - } - - blackboxWrite('\n'); - headerXmitIndex++; - charXmitIndex = 0; - } else { - charXmitIndex = i; - } - } else { - //Completed sending field information - - if (feature(FEATURE_GPS)) { - blackboxState = BLACKBOX_STATE_SEND_GPS_HEADERS; - } else { - blackboxState = BLACKBOX_STATE_SEND_SYSINFO; - } - headerXmitIndex = 0; - } - break; - case BLACKBOX_STATE_SEND_GPS_HEADERS: - for (i = 0; i < SERIAL_CHUNK_SIZE && blackboxGpsHeader[headerXmitIndex] != '\0'; i++, headerXmitIndex++) - blackboxWrite(blackboxGpsHeader[headerXmitIndex]); - - if (blackboxGpsHeader[headerXmitIndex] == '\0') { - blackboxState = BLACKBOX_STATE_SEND_SYSINFO; - headerXmitIndex = 0; - } - break; - case BLACKBOX_STATE_SEND_SYSINFO: - - switch (headerXmitIndex) { - 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 rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8); - break; - case 7: - blackboxPrintf("H minthrottle:%d\n", masterConfig.escAndServoConfig.minthrottle); - break; - case 8: - blackboxPrintf("H maxthrottle:%d\n", masterConfig.escAndServoConfig.maxthrottle); - break; - case 9: - floatConvert.f = gyro.scale; - blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u); - break; - case 10: - blackboxPrintf("H acc_1G:%u\n", acc_1G); - break; - case 11: - // One more pause for good luck - break; - default: - blackboxState = BLACKBOX_STATE_RUNNING; - } - - headerXmitIndex++; - break; - case BLACKBOX_STATE_RUNNING: - // Write a keyframe every 32 frames so we can resynchronise upon missing frames - blackboxIntercycleIndex = blackboxIteration % BLACKBOX_I_INTERVAL; - blackboxIntracycleIndex = blackboxIteration / BLACKBOX_I_INTERVAL; - - if (blackboxIntercycleIndex == 0) { - // Copy current system values into the blackbox - loadBlackboxState(); - writeIntraframe(); - } else { - if ((blackboxIntercycleIndex + 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] - || (blackboxIntercycleIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIntracycleIndex % 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++; - break; - default: - break; - } -} - -static bool canUseBlackboxWithCurrentConfiguration(void) -{ - if (!feature(FEATURE_BLACKBOX)) - return false; - - return true; -} - -void initBlackbox(void) -{ - if (canUseBlackboxWithCurrentConfiguration()) - blackboxState = BLACKBOX_STATE_STOPPED; - else - blackboxState = 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/accgyro.h" +#include "drivers/light_led.h" + +#include "drivers/gpio.h" +#include "drivers/system.h" +#include "drivers/serial.h" +#include "drivers/timer.h" +#include "drivers/pwm_rx.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 + +// Some macros to make writing FLIGHT_LOG_FIELD_PREDICTOR_* 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) STR(CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x)) +#define ENCODING(x) STR(CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x)) + +static const char blackboxHeader[] = + "H Product:Blackbox flight data recorder by Nicholas Sherlock\n" + "H Blackbox version:1\n" + "H Data version:1\n" + "H I interval:" STR(BLACKBOX_I_INTERVAL) "\n"; + +/* These headers have info for all 8 motors on them, we'll trim the final fields off to match the number of motors in the mixer: */ +static const char * const blackboxHeaderFields[] = { + "H Field I name:" + "loopIteration,time," + "axisP[0],axisP[1],axisP[2]," + "axisI[0],axisI[1],axisI[2]," + "axisD[0],axisD[1],axisD[2]," + "rcCommand[0],rcCommand[1],rcCommand[2],rcCommand[3]," + "gyroData[0],gyroData[1],gyroData[2]," + "accSmooth[0],accSmooth[1],accSmooth[2]," + "motor[0],motor[1],motor[2],motor[3]," + "motor[4],motor[5],motor[6],motor[7]", + + "H Field I signed:" + /* loopIteration, time: */ + "0,0," + /* PIDs: */ + "1,1,1,1,1,1,1,1,1," + /* rcCommand[0..2] */ + "1,1,1," + /* rcCommand[3] (Throttle): */ + "0," + /* gyroData[0..2]: */ + "1,1,1," + /* accSmooth[0..2]: */ + "1,1,1," + /* Motor[0..7]: */ + "0,0,0,0,0,0,0,0", + + "H Field I predictor:" + /* loopIteration, time: */ + PREDICT(0) "," PREDICT(0) "," + /* PIDs: */ + PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," + PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," + PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," + /* rcCommand[0..2] */ + PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," + /* rcCommand[3] (Throttle): */ + PREDICT(MINTHROTTLE) "," + /* gyroData[0..2]: */ + PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," + /* accSmooth[0..2]: */ + PREDICT(0) "," PREDICT(0) "," PREDICT(0) "," + /* Motor[0]: */ + PREDICT(MINTHROTTLE) "," + /* Motor[1..7]: */ + PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," + PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," PREDICT(MOTOR_0) "," + PREDICT(MOTOR_0), + + "H Field I encoding:" + /* loopIteration, time: */ + ENCODING(UNSIGNED_VB) "," ENCODING(UNSIGNED_VB) "," + /* PIDs: */ + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + /* rcCommand[0..2] */ + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + /* rcCommand[3] (Throttle): */ + ENCODING(UNSIGNED_VB) "," + /* gyroData[0..2]: */ + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + /* accSmooth[0..2]: */ + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + /* Motor[0]: */ + ENCODING(UNSIGNED_VB) "," + /* Motor[1..7]: */ + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + ENCODING(SIGNED_VB), + + //Motors and gyros predict an average of the last two measurements (to reduce the impact of noise): + "H Field P predictor:" + /* loopIteration, time: */ + PREDICT(INC) "," PREDICT(STRAIGHT_LINE) "," + /* PIDs: */ + PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," + PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," + PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," + /* rcCommand[0..2] */ + PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," PREDICT(PREVIOUS) "," + /* rcCommand[3] (Throttle): */ + PREDICT(PREVIOUS) "," + /* gyroData[0..2]: */ + PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," + /* accSmooth[0..2]: */ + PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," + /* Motor[0]: */ + PREDICT(AVERAGE_2) "," + /* Motor[1..7]: */ + PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," + PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," PREDICT(AVERAGE_2) "," + PREDICT(AVERAGE_2), + + /* RC fields are encoded together as a group, everything else is signed since they're diffs: */ + "H Field P encoding:" + /* loopIteration, time: */ + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + /* PIDs: */ + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + ENCODING(TAG2_3S32) "," ENCODING(TAG2_3S32) "," ENCODING(TAG2_3S32) "," + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + /* rcCommand[0..3] */ + ENCODING(TAG8_4S16) "," ENCODING(TAG8_4S16) "," ENCODING(TAG8_4S16) "," + ENCODING(TAG8_4S16) "," + /* gyroData[0..2]: */ + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + /* accSmooth[0..2]: */ + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + /* Motor[0]: */ + ENCODING(SIGNED_VB) "," + /* Motor[1..7]: */ + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + ENCODING(SIGNED_VB) +}; + +/** + * Additional fields to tack on to those above for tricopters (to record tail servo position) + */ +static const char * const blackboxAdditionalFieldsTricopter[] = { + //Field I name + "servo[5]", + + //Field I signed + "0", + + //Field I predictor + PREDICT(1500), + + //Field I encoding: + ENCODING(SIGNED_VB), + + //Field P predictor: + PREDICT(PREVIOUS), + + //Field P encoding: + ENCODING(SIGNED_VB) +}; + +static const char blackboxGpsHeader[] = + "H Field G name:" + "GPS_numSat,GPS_coord[0],GPS_coord[1],GPS_altitude,GPS_speed\n" + "H Field G signed:" + "0,1,1,0,0\n" + "H Field G predictor:" + PREDICT(0) "," PREDICT(HOME_COORD) "," PREDICT(HOME_COORD) "," PREDICT(0) "," PREDICT(0) "\n" + "H Field G encoding:" + ENCODING(UNSIGNED_VB) "," ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "," + ENCODING(UNSIGNED_VB) "," ENCODING(UNSIGNED_VB) "\n" + + "H Field H name:" + "GPS_home[0],GPS_home[1]\n" + "H Field H signed:" + "1,1\n" + "H Field H predictor:" + PREDICT(0) "," PREDICT(0) "\n" + "H Field H encoding:" + ENCODING(SIGNED_VB) "," ENCODING(SIGNED_VB) "\n"; + +typedef enum BlackboxState { + BLACKBOX_STATE_DISABLED = 0, + BLACKBOX_STATE_STOPPED, + BLACKBOX_STATE_SEND_HEADER, + BLACKBOX_STATE_SEND_FIELDINFO, + BLACKBOX_STATE_SEND_GPS_HEADERS, + BLACKBOX_STATE_SEND_SYSINFO, + 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 BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; +static uint32_t startTime; +static unsigned int headerXmitIndex; +static uint32_t blackboxIteration; + +static serialPort_t *blackboxPort; +static portMode_t previousPortMode; +static uint32_t previousBaudRate; + +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 int isTricopter() +{ + return masterConfig.mixerConfiguration == MULTITYPE_TRI; +} + +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); +} + +/** + * 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}; + enum { BITS_4 = 1}; + enum { BITS_6 = 2}; + enum { BITS_32 = 3}; + + enum { BYTES_1 = 0}; + enum { BYTES_2 = 1}; + enum { BYTES_3 = 2}; + enum { 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}; + enum { FIELD_4BIT = 1}; + enum { FIELD_8BIT = 2}; + enum { FIELD_16BIT = 3}; + + uint8_t selector; + int x; + + /* + * 4-bit fields can only be combined with their paired neighbor (there are two pairs), so choose a + * larger encoding if that's not possible. + */ + const uint8_t rcSelectorCleanup[16] = { + // Output selectors <- Input selectors + FIELD_ZERO << 2 | FIELD_ZERO, // zero, zero + FIELD_ZERO << 2 | FIELD_8BIT, // zero, 4-bit + FIELD_ZERO << 2 | FIELD_8BIT, // zero, 8-bit + FIELD_ZERO << 2 | FIELD_16BIT, // zero, 16-bit + FIELD_8BIT << 2 | FIELD_ZERO, // 4-bit, zero + FIELD_4BIT << 2 | FIELD_4BIT, // 4-bit, 4-bit + FIELD_8BIT << 2 | FIELD_8BIT, // 4-bit, 8-bit + FIELD_8BIT << 2 | FIELD_16BIT, // 4-bit, 16-bit + FIELD_8BIT << 2 | FIELD_ZERO, // 8-bit, zero + FIELD_8BIT << 2 | FIELD_8BIT, // 8-bit, 4-bit + FIELD_8BIT << 2 | FIELD_8BIT, // 8-bit, 8-bit + FIELD_8BIT << 2 | FIELD_16BIT, // 8-bit, 16-bit + FIELD_16BIT << 2 | FIELD_ZERO, // 16-bit, zero + FIELD_16BIT << 2 | FIELD_8BIT, // 16-bit, 4-bit + FIELD_16BIT << 2 | FIELD_8BIT, // 16-bit, 8-bit + FIELD_16BIT << 2 | FIELD_16BIT, // 16-bit, 16-bit + }; + + 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] <= 7 && values[x] >= -8) + selector |= FIELD_4BIT; + else if (values[x] <= 127 && values[x] >= -128) + selector |= FIELD_8BIT; + else + selector |= FIELD_16BIT; + } + + selector = rcSelectorCleanup[selector & 0x0F] | (rcSelectorCleanup[selector >> 4] << 4); + + blackboxWrite(selector); + + for (x = 0; x < 4; x++, selector >>= 2) { + switch (selector & 0x03) { + case FIELD_4BIT: + blackboxWrite((values[x] & 0x0F) | (values[x + 1] << 4)); + + //We write two selector fields: + x++; + selector >>= 2; + break; + case FIELD_8BIT: + blackboxWrite(values[x]); + break; + case FIELD_16BIT: + blackboxWrite(values[x]); + blackboxWrite(values[x] >> 8); + break; + } + } +} + +static void writeIntraframe(void) +{ + blackboxValues_t *blackboxCurrent = blackboxHistory[0]; + int x; + + blackboxWrite('I'); + + writeUnsignedVB(blackboxIteration); + writeUnsignedVB(blackboxCurrent->time); + + for (x = 0; x < 3; x++) + writeSignedVB(blackboxCurrent->axisP[x]); + + for (x = 0; x < 3; x++) + writeSignedVB(blackboxCurrent->axisI[x]); + + for (x = 0; x < 3; x++) + writeSignedVB(blackboxCurrent->axisD[x]); + + for (x = 0; x < 3; x++) + writeSignedVB(blackboxCurrent->rcCommand[x]); + + writeUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle] + + for (x = 0; x < 3; x++) + writeSignedVB(blackboxCurrent->gyroData[x]); + + for (x = 0; x < 3; 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 (isTricopter()) + 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[4]; + + 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 < 3; x++) + writeSignedVB(blackboxCurrent->axisP[x] - blackboxLast->axisP[x]); + + for (x = 0; x < 3; x++) + deltas[x] = blackboxCurrent->axisI[x] - blackboxLast->axisI[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); + + for (x = 0; x < 3; x++) + writeSignedVB(blackboxCurrent->axisD[x] - blackboxLast->axisD[x]); + + for (x = 0; x < 4; x++) + deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[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: + */ + writeTag8_4S16(deltas); + + //Since gyros, accs and motors are noisy, base the prediction on the average of the history: + for (x = 0; x < 3; x++) + writeSignedVB(blackboxHistory[0]->gyroData[x] - (blackboxHistory[1]->gyroData[x] + blackboxHistory[2]->gyroData[x]) / 2); + + for (x = 0; x < 3; 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 (isTricopter()) + 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) { + blackboxState = BLACKBOX_STATE_DISABLED; + return; + } + + startTime = millis(); + headerXmitIndex = 0; + blackboxIteration = 0; + blackboxState = BLACKBOX_STATE_SEND_HEADER; + + memset(&gpsHistory, 0, sizeof(gpsHistory)); + + //No need to clear the content of blackboxHistoryRing since our first frame will be an intra which overwrites it + + blackboxHistory[0] = &blackboxHistoryRing[0]; + blackboxHistory[1] = &blackboxHistoryRing[1]; + blackboxHistory[2] = &blackboxHistoryRing[2]; + } +} + +void finishBlackbox(void) +{ + if (blackboxState != BLACKBOX_STATE_DISABLED && blackboxState != BLACKBOX_STATE_STOPPED) { + blackboxState = 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 < 3; i++) + blackboxCurrent->axisP[i] = axisP[i]; + for (i = 0; i < 3; i++) + blackboxCurrent->axisI[i] = axisI[i]; + for (i = 0; i < 3; i++) + blackboxCurrent->axisD[i] = axisD[i]; + + for (i = 0; i < 4; i++) + blackboxCurrent->rcCommand[i] = rcCommand[i]; + + for (i = 0; i < 3; i++) + blackboxCurrent->gyroData[i] = gyroData[i]; + + for (i = 0; i < 3; i++) + blackboxCurrent->accSmooth[i] = accSmooth[i]; + + for (i = 0; i < motorCount; i++) + blackboxCurrent->motor[i] = motor[i]; + + //Tail servo for tricopters + blackboxCurrent->servo[5] = servo[5]; +} + +void handleBlackbox(void) +{ + int i; + const char *additionalHeader; + const int SERIAL_CHUNK_SIZE = 16; + static int charXmitIndex = 0; + int motorsToRemove, endIndex; + int blackboxIntercycleIndex, blackboxIntracycleIndex; + + union floatConvert_t { + float f; + uint32_t u; + } floatConvert; + + switch (blackboxState) { + case BLACKBOX_STATE_SEND_HEADER: + /* + * 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') { + blackboxState = BLACKBOX_STATE_SEND_FIELDINFO; + headerXmitIndex = 0; + charXmitIndex = 0; + } + } + break; + case BLACKBOX_STATE_SEND_FIELDINFO: + /* + * Send information about the fields we're recording to the log. + * + * Once again, we're chunking up the data so we don't exceed our datarate. This time we're trimming the + * excess field defs off the end of the header for motors we don't have. + */ + motorsToRemove = 8 - motorCount; + + if (headerXmitIndex < sizeof(blackboxHeaderFields) / sizeof(blackboxHeaderFields[0])){ + endIndex = strlen(blackboxHeaderFields[headerXmitIndex]) - (headerXmitIndex == 0 ? strlen(",motor[x]") : strlen(",x")) * motorsToRemove; + + for (i = charXmitIndex; i < charXmitIndex + SERIAL_CHUNK_SIZE && i < endIndex; i++) + blackboxWrite(blackboxHeaderFields[headerXmitIndex][i]); + + // Did we complete this line? + if (i == endIndex) { + if (isTricopter()) { + //Add fields to the end for the tail servo + blackboxWrite(','); + + for (additionalHeader = blackboxAdditionalFieldsTricopter[headerXmitIndex]; *additionalHeader; additionalHeader++) + blackboxWrite(*additionalHeader); + } + + blackboxWrite('\n'); + headerXmitIndex++; + charXmitIndex = 0; + } else { + charXmitIndex = i; + } + } else { + //Completed sending field information + + if (feature(FEATURE_GPS)) { + blackboxState = BLACKBOX_STATE_SEND_GPS_HEADERS; + } else { + blackboxState = BLACKBOX_STATE_SEND_SYSINFO; + } + headerXmitIndex = 0; + } + break; + case BLACKBOX_STATE_SEND_GPS_HEADERS: + for (i = 0; i < SERIAL_CHUNK_SIZE && blackboxGpsHeader[headerXmitIndex] != '\0'; i++, headerXmitIndex++) + blackboxWrite(blackboxGpsHeader[headerXmitIndex]); + + if (blackboxGpsHeader[headerXmitIndex] == '\0') { + blackboxState = BLACKBOX_STATE_SEND_SYSINFO; + headerXmitIndex = 0; + } + break; + case BLACKBOX_STATE_SEND_SYSINFO: + + switch (headerXmitIndex) { + 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 rcRate:%d\n", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcRate8); + break; + case 7: + blackboxPrintf("H minthrottle:%d\n", masterConfig.escAndServoConfig.minthrottle); + break; + case 8: + blackboxPrintf("H maxthrottle:%d\n", masterConfig.escAndServoConfig.maxthrottle); + break; + case 9: + floatConvert.f = gyro.scale; + blackboxPrintf("H gyro.scale:0x%x\n", floatConvert.u); + break; + case 10: + blackboxPrintf("H acc_1G:%u\n", acc_1G); + break; + case 11: + // One more pause for good luck + break; + default: + blackboxState = BLACKBOX_STATE_RUNNING; + } + + headerXmitIndex++; + break; + case BLACKBOX_STATE_RUNNING: + // Write a keyframe every 32 frames so we can resynchronise upon missing frames + blackboxIntercycleIndex = blackboxIteration % BLACKBOX_I_INTERVAL; + blackboxIntracycleIndex = blackboxIteration / BLACKBOX_I_INTERVAL; + + if (blackboxIntercycleIndex == 0) { + // Copy current system values into the blackbox + loadBlackboxState(); + writeIntraframe(); + } else { + if ((blackboxIntercycleIndex + 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] + || (blackboxIntercycleIndex == BLACKBOX_I_INTERVAL / 2 && blackboxIntracycleIndex % 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++; + break; + default: + break; + } +} + +static bool canUseBlackboxWithCurrentConfiguration(void) +{ + if (!feature(FEATURE_BLACKBOX)) + return false; + + return true; +} + +void initBlackbox(void) +{ + if (canUseBlackboxWithCurrentConfiguration()) + blackboxState = BLACKBOX_STATE_STOPPED; + else + blackboxState = BLACKBOX_STATE_DISABLED; +} diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index de5cc344d7..84804b3828 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -1,37 +1,37 @@ -/* - * 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 - -typedef struct blackboxValues_t { - uint32_t time; - - int32_t axisP[3], axisI[3], axisD[3]; - - int16_t rcCommand[4]; - int16_t gyroData[3]; - int16_t accSmooth[3]; - int16_t motor[MAX_SUPPORTED_MOTORS]; - int16_t servo[MAX_SUPPORTED_SERVOS]; -} 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 + +typedef struct blackboxValues_t { + uint32_t time; + + int32_t axisP[3], axisI[3], axisD[3]; + + int16_t rcCommand[4]; + int16_t gyroData[3]; + int16_t accSmooth[3]; + int16_t motor[MAX_SUPPORTED_MOTORS]; + int16_t servo[MAX_SUPPORTED_SERVOS]; +} blackboxValues_t; + +void initBlackbox(void); +void handleBlackbox(void); +void startBlackbox(void); +void finishBlackbox(void); diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index ca1812ca9c..faafe90ef2 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -339,9 +339,9 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat axisPID[axis] = PTerm + ITerm + DTerm; #ifdef BLACKBOX - axisP[axis] = PTerm; - axisI[axis] = ITerm; - axisD[axis] = DTerm; + axisP[axis] = PTerm; + axisI[axis] = ITerm; + axisD[axis] = DTerm; #endif } } diff --git a/src/main/mw.c b/src/main/mw.c index e6607cbccb..f6fbee39dc 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -307,7 +307,7 @@ void mwDisarm(void) #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { - finishBlackbox(); + finishBlackbox(); if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { mspAllocateSerialPorts(&masterConfig.serialConfig); } @@ -341,7 +341,7 @@ void mwArm(void) if (sharedBlackboxAndMspPort) { mspReleasePortIfAllocated(sharedBlackboxAndMspPort); } - startBlackbox(); + startBlackbox(); } #endif return; @@ -700,9 +700,9 @@ void loop(void) writeMotors(); #ifdef BLACKBOX - if (!cliMode && feature(FEATURE_BLACKBOX)) { - handleBlackbox(); - } + if (!cliMode && feature(FEATURE_BLACKBOX)) { + handleBlackbox(); + } #endif }