Merge pull request #227 from sherlockflight/master

Add blackbox flight data recorder feature

src/main/blackbox/blackbox.h

@@ -0,0 +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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "common/axis.h"
+#include <stdint.h>
+
+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);

src/main/blackbox/blackbox_fielddefs.h

@@ -0,0 +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 <http://www.gnu.org/licenses/>.
+ */
+
+#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;

src/main/config/config.c

@@ -445,6 +445,11 @@ static void resetConf(void)
     applyDefaultLedStripConfig(masterConfig.ledConfigs);
 #endif
 
+#ifdef BLACKBOX
+    masterConfig.blackbox_rate_num = 1;
+    masterConfig.blackbox_rate_denom = 1;
+#endif
+
     // alternative defaults AlienWii32 (activate via OPTIONS="ALIENWII32" during make for NAZE target)
 #ifdef ALIENWII32
     featureSet(FEATURE_RX_SERIAL);

src/main/config/config.h

@@ -40,7 +40,8 @@ typedef enum {
     FEATURE_RSSI_ADC = 1 << 15,
     FEATURE_LED_STRIP = 1 << 16,
     FEATURE_DISPLAY = 1 << 17,
-    FEATURE_ONESHOT125 = 1 << 18
+    FEATURE_ONESHOT125 = 1 << 18,
+    FEATURE_BLACKBOX = 1 << 19
 } features_e;
 
 bool feature(uint32_t mask);

src/main/config/config_master.h

@@ -85,6 +85,8 @@ typedef struct master_t {
     uint8_t current_profile_index;
     controlRateConfig_t controlRateProfiles[MAX_CONTROL_RATE_PROFILE_COUNT];
 
+    uint8_t blackbox_rate_num;
+    uint8_t blackbox_rate_denom;
 
     uint8_t magic_ef;                       // magic number, should be 0xEF
     uint8_t chk;                            // XOR checksum

src/main/flight/flight.c

@@ -45,6 +45,11 @@ extern uint16_t cycleTime;
 
 int16_t heading, magHold;
 int16_t axisPID[3];
+
+#ifdef BLACKBOX
+int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
+#endif
+
 uint8_t dynP8[3], dynI8[3], dynD8[3];
 
 static int32_t errorGyroI[3] = { 0, 0, 0 };
@@ -247,6 +252,12 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         delta1[axis] = delta;
         DTerm = (deltaSum * dynD8[axis]) / 32;
         axisPID[axis] = PTerm + ITerm - DTerm;
+
+#ifdef BLACKBOX
+        axisPID_P[axis] = PTerm;
+        axisPID_I[axis] = ITerm;
+        axisPID_D[axis] = -DTerm;
+#endif
     }
 }
 
@@ -330,6 +341,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
 
         // -----calculate total PID output
         axisPID[axis] = PTerm + ITerm + DTerm;
+
+#ifdef BLACKBOX
+        axisPID_P[axis] = PTerm;
+        axisPID_I[axis] = ITerm;
+        axisPID_D[axis] = DTerm;
+#endif
     }
 }
 

src/main/flight/flight.h

@@ -127,6 +127,8 @@ extern int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AX
 extern int32_t accSum[XYZ_AXIS_COUNT];
 extern int16_t axisPID[XYZ_AXIS_COUNT];
 
+extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
+
 extern int16_t heading, magHold;
 
 extern int32_t AltHold;

src/main/flight/mixer.c

@@ -47,7 +47,7 @@
 
 #define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4
 
-static uint8_t motorCount = 0;
+uint8_t motorCount = 0;
 int16_t motor[MAX_SUPPORTED_MOTORS];
 int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 int16_t servo[MAX_SUPPORTED_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };

src/main/io/serial.c

@@ -62,7 +62,10 @@ const serialPortFunctionScenario_e serialPortScenarios[SERIAL_PORT_SCENARIO_COUN
     SCENARIO_CLI_ONLY,
     SCENARIO_GPS_PASSTHROUGH_ONLY,
     SCENARIO_MSP_ONLY,
-    SCENARIO_SMARTPORT_TELEMETRY_ONLY
+    SCENARIO_SMARTPORT_TELEMETRY_ONLY,
+
+    SCENARIO_BLACKBOX_ONLY,
+    SCENARIO_MSP_CLI_BLACKBOX_GPS_PASTHROUGH
 };
 
 static serialConfig_t *serialConfig;
@@ -136,7 +139,8 @@ const functionConstraint_t functionConstraints[] = {
         { FUNCTION_MSP,                 9600,  115200, NO_AUTOBAUD, SPF_NONE },
         { FUNCTION_SERIAL_RX,           9600,  115200, NO_AUTOBAUD, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK },
         { FUNCTION_TELEMETRY,           9600,  19200,  NO_AUTOBAUD, SPF_NONE },
-        { FUNCTION_SMARTPORT_TELEMETRY, 57600, 57600,  NO_AUTOBAUD, SPF_SUPPORTS_BIDIR_MODE }
+        { FUNCTION_SMARTPORT_TELEMETRY, 57600, 57600,  NO_AUTOBAUD, SPF_SUPPORTS_BIDIR_MODE },
+        { FUNCTION_BLACKBOX,            115200,115200, NO_AUTOBAUD, SPF_NONE }
 };
 
 #define FUNCTION_CONSTRAINT_COUNT (sizeof(functionConstraints) / sizeof(functionConstraint_t))

src/main/io/serial.h

@@ -25,7 +25,8 @@ typedef enum {
     FUNCTION_SMARTPORT_TELEMETRY = (1 << 3),
     FUNCTION_SERIAL_RX           = (1 << 4),
     FUNCTION_GPS                 = (1 << 5),
-    FUNCTION_GPS_PASSTHROUGH     = (1 << 6)
+    FUNCTION_GPS_PASSTHROUGH     = (1 << 6),
+    FUNCTION_BLACKBOX            = (1 << 7)
 } serialPortFunction_e;
 
 typedef enum {
@@ -52,10 +53,12 @@ typedef enum {
     SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH   = FUNCTION_MSP | FUNCTION_CLI | FUNCTION_TELEMETRY | FUNCTION_SMARTPORT_TELEMETRY | FUNCTION_GPS_PASSTHROUGH,
     SCENARIO_SERIAL_RX_ONLY                     = FUNCTION_SERIAL_RX,
     SCENARIO_TELEMETRY_ONLY                     = FUNCTION_TELEMETRY,
-    SCENARIO_SMARTPORT_TELEMETRY_ONLY           = FUNCTION_SMARTPORT_TELEMETRY
+    SCENARIO_SMARTPORT_TELEMETRY_ONLY           = FUNCTION_SMARTPORT_TELEMETRY,
+    SCENARIO_BLACKBOX_ONLY                      = FUNCTION_BLACKBOX,
+    SCENARIO_MSP_CLI_BLACKBOX_GPS_PASTHROUGH    = FUNCTION_CLI | FUNCTION_MSP | FUNCTION_BLACKBOX | FUNCTION_GPS_PASSTHROUGH
 } serialPortFunctionScenario_e;
 
-#define SERIAL_PORT_SCENARIO_COUNT 10
+#define SERIAL_PORT_SCENARIO_COUNT 12
 #define SERIAL_PORT_SCENARIO_MAX (SERIAL_PORT_SCENARIO_COUNT - 1)
 extern const serialPortFunctionScenario_e serialPortScenarios[SERIAL_PORT_SCENARIO_COUNT];
 

src/main/io/serial_cli.c

@@ -135,7 +135,8 @@ static const char * const featureNames[] = {
     "RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP",
     "SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE",
     "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
-    "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", NULL
+    "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125",
+    "BLACKBOX", NULL
 };
 
 // sync this with sensors_e
@@ -404,6 +405,9 @@ const clivalue_t valueTable[] = {
     { "p_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDVEL], 0, 200 },
     { "i_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 },
     { "d_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 },
+
+    { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_num, 1, 32 },
+    { "blackbox_rate_denom",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_denom, 1, 32 },
 };
 
 #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))

src/main/main.c

@@ -66,6 +66,7 @@
 #include "sensors/acceleration.h"
 #include "sensors/gyro.h"
 #include "telemetry/telemetry.h"
+#include "blackbox/blackbox.h"
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
 #include "config/runtime_config.h"
@@ -344,6 +345,10 @@ void init(void)
         initTelemetry();
 #endif
 
+#ifdef BLACKBOX
+    initBlackbox();
+#endif
+
     previousTime = micros();
 
     if (masterConfig.mixerMode == MIXER_GIMBAL) {

src/main/mw.c

@@ -67,6 +67,7 @@
 #include "io/statusindicator.h"
 #include "rx/msp.h"
 #include "telemetry/telemetry.h"
+#include "blackbox/blackbox.h"
 
 #include "config/runtime_config.h"
 #include "config/config.h"
@@ -306,6 +307,15 @@ void mwDisarm(void)
             }
         }
 #endif
+
+#ifdef BLACKBOX
+        if (feature(FEATURE_BLACKBOX)) {
+            finishBlackbox();
+            if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) {
+                mspAllocateSerialPorts(&masterConfig.serialConfig);
+            }
+        }
+#endif
     }
 }
 
@@ -327,6 +337,16 @@ void mwArm(void)
                 }
             }
 #endif
+
+#ifdef BLACKBOX
+            if (feature(FEATURE_BLACKBOX)) {
+                serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
+                if (sharedBlackboxAndMspPort) {
+                    mspReleasePortIfAllocated(sharedBlackboxAndMspPort);
+                }
+                startBlackbox();
+            }
+#endif
             disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 
             return;
@@ -698,6 +718,12 @@ void loop(void)
         mixTable();
         writeServos();
         writeMotors();
+
+#ifdef BLACKBOX
+        if (!cliMode && feature(FEATURE_BLACKBOX)) {
+            handleBlackbox();
+        }
+#endif
     }
 
 #ifdef TELEMETRY

src/main/sensors/battery.c

@@ -31,6 +31,7 @@ uint16_t batteryWarningVoltage;
 uint16_t batteryCriticalVoltage;
 
 uint8_t vbat = 0;                   // battery voltage in 0.1V steps
+uint16_t vbatLatest = 0;            // most recent unsmoothed raw reading from vbat adc
 
 int32_t amperage = 0;               // amperage read by current sensor in centiampere (1/100th A)
 int32_t mAhDrawn = 0;               // milliampere hours drawn from the battery since start
@@ -54,7 +55,7 @@ void updateBatteryVoltage(void)
     uint16_t vbatSampleTotal = 0;
 
     // store the battery voltage with some other recent battery voltage readings
-    vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = adcGetChannel(ADC_BATTERY);
+    vbatSamples[(currentSampleIndex++) % BATTERY_SAMPLE_COUNT] = vbatLatest = adcGetChannel(ADC_BATTERY);
 
     // calculate vbat based on the average of recent readings
     for (index = 0; index < BATTERY_SAMPLE_COUNT; index++) {

src/main/sensors/battery.h

@@ -42,6 +42,7 @@ typedef enum {
 } batteryState_e;
 
 extern uint8_t vbat;
+extern uint16_t vbatLatest;
 extern uint8_t batteryCellCount;
 extern uint16_t batteryWarningVoltage;
 extern int32_t amperage;

src/main/target/NAZE/target.h

@@ -124,6 +124,7 @@
 #define LED_STRIP
 #define LED_STRIP_TIMER TIM3
 
+#define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX
 #define AUTOTUNE

src/main/target/NAZE32PRO/target.h

@@ -43,6 +43,7 @@
 #define SENSORS_SET (SENSOR_ACC)
 
 #define GPS
+#define BLACKBOX
 #define TELEMETRY
 #define SERIAL_RX
 #define AUTOTUNE