Merge pull request #1157 from cleanflight/blackbox-pause

Add pause/resume switch support to Blackbox

docs/Blackbox.md

@@ -230,6 +230,14 @@ After downloading the log, be sure to erase the chip to make it ready for reuse
 If you try to start recording a new flight when the dataflash is already full, Blackbox logging will be disabled and
 nothing will be recorded.
 
+### Usage - Logging switch
+If you're recording to an onboard flash chip, you probably want to disable Blackbox recording when not required in order
+to save storage space. To do this, you can add a Blackbox flight mode to one of your AUX channels on the Configurator's 
+modes tab. Once you've added a mode, Blackbox will only log flight data when the mode is active.
+
+A log header will always be recorded at arming time, even if logging is paused. You can freely pause and resume logging 
+while in flight.
+
 ## Converting logs to CSV or PNG
 After your flights, you'll have a series of flight log files with a .TXT extension. You'll need to decode these with
 the `blackbox_decode` tool to create CSV (comma-separated values) files for analysis, or render them into a series of PNG

docs/Modes.md

@@ -27,6 +27,7 @@ auxillary receiver channels and other events such as failsafe detection.
 | 20      | 19     | TELEMETRY  | Enable telemetry via switch                                          |
 | 21      | 20     | AUTOTUNE   | Autotune Pitch/Roll PIDs                                             |
 | 22      | 21     | SONAR      | Altitude hold mode (sonar sensor only)                               |
+| 26      | 25     | BLACKBOX   | Enable BlackBox logging                                              |
 
 ## Mode details
 

src/main/blackbox/blackbox.c

@@ -256,6 +256,7 @@ typedef enum BlackboxState {
     BLACKBOX_STATE_SEND_GPS_G_HEADER,
     BLACKBOX_STATE_SEND_SLOW_HEADER,
     BLACKBOX_STATE_SEND_SYSINFO,
+    BLACKBOX_STATE_PAUSED,
     BLACKBOX_STATE_RUNNING,
     BLACKBOX_STATE_SHUTTING_DOWN
 } BlackboxState;
@@ -349,6 +350,8 @@ static blackboxMainState_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 blackboxMainState_t* blackboxHistory[3];
 
+static bool blackboxModeActivationConditionPresent = false;
+
 static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
 {
     switch (condition) {
@@ -454,9 +457,6 @@ static void blackboxSetState(BlackboxState newState)
             xmitState.headerIndex = 0;
         break;
         case BLACKBOX_STATE_RUNNING:
-            blackboxIteration = 0;
-            blackboxPFrameIndex = 0;
-            blackboxIFrameIndex = 0;
             blackboxSlowFrameIterationTimer = SLOW_FRAME_INTERVAL; //Force a slow frame to be written on the first iteration
         break;
         case BLACKBOX_STATE_SHUTTING_DOWN:
@@ -479,25 +479,24 @@ static void writeIntraframe(void)
     blackboxWriteUnsignedVB(blackboxIteration);
     blackboxWriteUnsignedVB(blackboxCurrent->time);
 
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
-        blackboxWriteSignedVB(blackboxCurrent->axisPID_P[x]);
-    }
-
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
-        blackboxWriteSignedVB(blackboxCurrent->axisPID_I[x]);
-    }
+    blackboxWriteSignedVBArray(blackboxCurrent->axisPID_P, XYZ_AXIS_COUNT);
+    blackboxWriteSignedVBArray(blackboxCurrent->axisPID_I, XYZ_AXIS_COUNT);
 
+    // Don't bother writing the current D term if the corresponding PID setting is zero
     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
         if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0 + x)) {
             blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]);
         }
     }
 
-    for (x = 0; x < 3; x++) {
-        blackboxWriteSignedVB(blackboxCurrent->rcCommand[x]);
-    }
+    // Write roll, pitch and yaw first:
+    blackboxWriteSigned16VBArray(blackboxCurrent->rcCommand, 3);
 
-    blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[3] - masterConfig.escAndServoConfig.minthrottle); //Throttle lies in range [minthrottle..maxthrottle]
+    /*
+     * Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
+     * Throttle lies in range [minthrottle..maxthrottle]:
+     */
+    blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - masterConfig.escAndServoConfig.minthrottle);
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
         /*
@@ -516,9 +515,7 @@ static void writeIntraframe(void)
 
 #ifdef MAG
         if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_MAG)) {
-            for (x = 0; x < XYZ_AXIS_COUNT; x++) {
-                blackboxWriteSignedVB(blackboxCurrent->magADC[x]);
-            }
+            blackboxWriteSigned16VBArray(blackboxCurrent->magADC, XYZ_AXIS_COUNT);
         }
 #endif
 
@@ -538,24 +535,20 @@ static void writeIntraframe(void)
         blackboxWriteUnsignedVB(blackboxCurrent->rssi);
     }
 
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
-        blackboxWriteSignedVB(blackboxCurrent->gyroADC[x]);
-    }
-
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
-        blackboxWriteSignedVB(blackboxCurrent->accSmooth[x]);
-    }
+    blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
+    blackboxWriteSigned16VBArray(blackboxCurrent->accSmooth, XYZ_AXIS_COUNT);
 
     //Motors can be below minthrottle when disarmed, but that doesn't happen much
     blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle);
 
-    //Motors tend to be similar to each other
+    //Motors tend to be similar to each other so use the first motor's value as a predictor of the others
     for (x = 1; x < motorCount; x++) {
         blackboxWriteSignedVB(blackboxCurrent->motor[x] - blackboxCurrent->motor[0]);
     }
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) {
-        blackboxWriteSignedVB(blackboxHistory[0]->servo[5] - 1500);
+        //Assume the tail spends most of its time around the center
+        blackboxWriteSignedVB(blackboxCurrent->servo[5] - 1500);
     }
 
     //Rotate our history buffers:
@@ -568,6 +561,20 @@ static void writeIntraframe(void)
     blackboxHistory[0] = ((blackboxHistory[0] - blackboxHistoryRing + 1) % 3) + blackboxHistoryRing;
 }
 
+static void blackboxWriteMainStateArrayUsingAveragePredictor(int arrOffsetInHistory, int count)
+{
+    int16_t *curr  = (int16_t*) ((char*) (blackboxHistory[0]) + arrOffsetInHistory);
+    int16_t *prev1 = (int16_t*) ((char*) (blackboxHistory[1]) + arrOffsetInHistory);
+    int16_t *prev2 = (int16_t*) ((char*) (blackboxHistory[2]) + arrOffsetInHistory);
+
+    for (int i = 0; i < count; i++) {
+        // Predictor is the average of the previous two history states
+        int32_t predictor = (prev1[i] + prev2[i]) / 2;
+
+        blackboxWriteSignedVB(curr[i] - predictor);
+    }
+}
+
 static void writeInterframe(void)
 {
     int x;
@@ -586,18 +593,14 @@ static void writeInterframe(void)
      */
     blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time));
 
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
-        blackboxWriteSignedVB(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];
-    }
+    arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
+    blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
 
     /* 
      * 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.
      */
+    arraySubInt32(deltas, blackboxCurrent->axisPID_I, blackboxLast->axisPID_I, XYZ_AXIS_COUNT);
     blackboxWriteTag2_3S32(deltas);
     
     /*
@@ -657,18 +660,10 @@ static void writeInterframe(void)
 
     blackboxWriteTag8_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++) {
-        blackboxWriteSignedVB(blackboxHistory[0]->gyroADC[x] - (blackboxHistory[1]->gyroADC[x] + blackboxHistory[2]->gyroADC[x]) / 2);
-    }
-
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
-        blackboxWriteSignedVB(blackboxHistory[0]->accSmooth[x] - (blackboxHistory[1]->accSmooth[x] + blackboxHistory[2]->accSmooth[x]) / 2);
-    }
-
-    for (x = 0; x < motorCount; x++) {
-        blackboxWriteSignedVB(blackboxHistory[0]->motor[x] - (blackboxHistory[1]->motor[x] + blackboxHistory[2]->motor[x]) / 2);
-    }
+    //Since gyros, accs and motors are noisy, base their predictions on the average of the history:
+    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC),   XYZ_AXIS_COUNT);
+    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accSmooth), XYZ_AXIS_COUNT);
+    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor),     motorCount);
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) {
         blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]);
@@ -795,6 +790,12 @@ void startBlackbox(void)
          * cache those now.
          */
         blackboxBuildConditionCache();
+        
+        blackboxModeActivationConditionPresent = isModeActivationConditionPresent(currentProfile->modeActivationConditions, BOXBLACKBOX);
+
+        blackboxIteration = 0;
+        blackboxPFrameIndex = 0;
+        blackboxIFrameIndex = 0;
 
         /*
          * Record the beeper's current idea of the last arming beep time, so that we can detect it changing when
@@ -811,7 +812,7 @@ void startBlackbox(void)
  */
 void finishBlackbox(void)
 {
-    if (blackboxState == BLACKBOX_STATE_RUNNING) {
+    if (blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_PAUSED) {
         blackboxLogEvent(FLIGHT_LOG_EVENT_LOG_END, NULL);
 
         blackboxSetState(BLACKBOX_STATE_SHUTTING_DOWN);
@@ -1099,7 +1100,8 @@ static bool blackboxWriteSysinfo()
  */
 void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
 {
-    if (blackboxState != BLACKBOX_STATE_RUNNING) {
+    // Only allow events to be logged after headers have been written
+    if (!(blackboxState == BLACKBOX_STATE_RUNNING || blackboxState == BLACKBOX_STATE_PAUSED)) {
         return;
     }
 
@@ -1141,6 +1143,10 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
                 blackboxWriteSignedVB(data->inflightAdjustment.newValue);
             }
         break;
+        case FLIGHT_LOG_EVENT_LOGGING_RESUME:
+            blackboxWriteUnsignedVB(data->loggingResume.logIteration);
+            blackboxWriteUnsignedVB(data->loggingResume.currentTime);
+        break;
         case FLIGHT_LOG_EVENT_LOG_END:
             blackboxPrint("End of log");
             blackboxWrite(0);
@@ -1175,11 +1181,28 @@ static bool blackboxShouldLogPFrame(uint32_t pFrameIndex)
     return (pFrameIndex + masterConfig.blackbox_rate_num - 1) % masterConfig.blackbox_rate_denom < masterConfig.blackbox_rate_num;
 }
 
+static bool blackboxShouldLogIFrame() {
+    return blackboxPFrameIndex == 0;
+}
+
+// Called once every FC loop in order to keep track of how many FC loop iterations have passed
+static void blackboxAdvanceIterationTimers()
+{
+    blackboxSlowFrameIterationTimer++;
+    blackboxIteration++;
+    blackboxPFrameIndex++;
+
+    if (blackboxPFrameIndex == BLACKBOX_I_INTERVAL) {
+        blackboxPFrameIndex = 0;
+        blackboxIFrameIndex++;
+    }
+}
+
 // Called once every FC loop in order to log the current state
 static void blackboxLogIteration()
 {
     // Write a keyframe every BLACKBOX_I_INTERVAL frames so we can resynchronise upon missing frames
-    if (blackboxPFrameIndex == 0) {
+    if (blackboxShouldLogIFrame()) {
         /*
          * Don't log a slow frame if the slow data didn't change ("I" frames are already large enough without adding
          * an additional item to write at the same time)
@@ -1226,15 +1249,6 @@ static void blackboxLogIteration()
 
     //Flush every iteration so that our runtime variance is minimized
     blackboxDeviceFlush();
-
-    blackboxSlowFrameIterationTimer++;
-    blackboxIteration++;
-    blackboxPFrameIndex++;
-
-    if (blackboxPFrameIndex == BLACKBOX_I_INTERVAL) {
-        blackboxPFrameIndex = 0;
-        blackboxIFrameIndex++;
-    }
 }
 
 /**
@@ -1305,10 +1319,33 @@ void handleBlackbox(void)
                 blackboxSetState(BLACKBOX_STATE_RUNNING);
             }
         break;
+        case BLACKBOX_STATE_PAUSED:
+            // Only allow resume to occur during an I-frame iteration, so that we have an "I" base to work from
+            if (IS_RC_MODE_ACTIVE(BOXBLACKBOX) && blackboxShouldLogIFrame()) {
+                // Write a log entry so the decoder is aware that our large time/iteration skip is intended
+                flightLogEvent_loggingResume_t resume;
+
+                resume.logIteration = blackboxIteration;
+                resume.currentTime = currentTime;
+
+                blackboxLogEvent(FLIGHT_LOG_EVENT_LOGGING_RESUME, (flightLogEventData_t *) &resume);
+                blackboxSetState(BLACKBOX_STATE_RUNNING);
+                
+                blackboxLogIteration();
+            }
+
+            // Keep the logging timers ticking so our log iteration continues to advance
+            blackboxAdvanceIterationTimers();
+        break;
         case BLACKBOX_STATE_RUNNING:
             // On entry to this state, blackboxIteration, blackboxPFrameIndex and blackboxIFrameIndex are reset to 0
+            if (blackboxModeActivationConditionPresent && !IS_RC_MODE_ACTIVE(BOXBLACKBOX)) {
+                blackboxSetState(BLACKBOX_STATE_PAUSED);
+            } else {
+                blackboxLogIteration();
+            }
 
-            blackboxLogIteration();
+            blackboxAdvanceIterationTimers();
         break;
         case BLACKBOX_STATE_SHUTTING_DOWN:
             //On entry of this state, startTime is set and a flush is performed

src/main/blackbox/blackbox_fielddefs.h

@@ -107,6 +107,7 @@ typedef enum FlightLogEvent {
     FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11,
     FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12,
     FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT = 13,
+    FLIGHT_LOG_EVENT_LOGGING_RESUME = 14,
     FLIGHT_LOG_EVENT_LOG_END = 255
 } FlightLogEvent;
 
@@ -147,6 +148,11 @@ typedef struct flightLogEvent_inflightAdjustment_t {
     float newFloatValue;
 } flightLogEvent_inflightAdjustment_t;
 
+typedef struct flightLogEvent_loggingResume_t {
+    uint32_t logIteration;
+    uint32_t currentTime;
+} flightLogEvent_loggingResume_t;
+
 typedef union flightLogEventData_t
 {
     flightLogEvent_syncBeep_t syncBeep;
@@ -154,6 +160,7 @@ typedef union flightLogEventData_t
     flightLogEvent_autotuneCycleResult_t autotuneCycleResult;
     flightLogEvent_autotuneTargets_t autotuneTargets;
     flightLogEvent_inflightAdjustment_t inflightAdjustment;
+    flightLogEvent_loggingResume_t loggingResume;
 } flightLogEventData_t;
 
 typedef struct flightLogEvent_t

src/main/blackbox/blackbox_io.c

@@ -154,6 +154,20 @@ void blackboxWriteSignedVB(int32_t value)
     blackboxWriteUnsignedVB(zigzagEncode(value));
 }
 
+void blackboxWriteSignedVBArray(int32_t *array, int count)
+{
+    for (int i = 0; i < count; i++) {
+        blackboxWriteSignedVB(array[i]);
+    }
+}
+
+void blackboxWriteSigned16VBArray(int16_t *array, int count)
+{
+    for (int i = 0; i < count; i++) {
+        blackboxWriteSignedVB(array[i]);
+    }
+}
+
 void blackboxWriteS16(int16_t value)
 {
     blackboxWrite(value & 0xFF);

src/main/blackbox/blackbox_io.h

@@ -41,6 +41,8 @@ int blackboxPrint(const char *s);
 
 void blackboxWriteUnsignedVB(uint32_t value);
 void blackboxWriteSignedVB(int32_t value);
+void blackboxWriteSignedVBArray(int32_t *array, int count);
+void blackboxWriteSigned16VBArray(int16_t *array, int count);
 void blackboxWriteS16(int16_t value);
 void blackboxWriteTag2_3S32(int32_t *values);
 void blackboxWriteTag8_4S16(int32_t *values);

src/main/common/maths.c

@@ -237,3 +237,10 @@ int32_t quickMedianFilter9(int32_t * v)
     QMF_SORT(p[4], p[2]);
     return p[4];
 }
+
+void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count)
+{
+    for (int i = 0; i < count; i++) {
+        dest[i] = array1[i] - array2[i];
+    }
+}

src/main/common/maths.h

@@ -94,4 +94,6 @@ float cos_approx(float x);
 #else
 #define sin_approx(x)   sinf(x)
 #define cos_approx(x)   cosf(x)
-#endif
+#endif
+
+void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);

src/main/io/rc_controls.c

@@ -310,6 +310,21 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
 
 }
 
+bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
+{
+    uint8_t index;
+
+    for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
+        modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
+        
+        if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) {
+            return true;
+        }
+    }
+    
+    return false;
+}
+
 bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range) {
     if (!IS_RANGE_USABLE(range)) {
         return false;
@@ -732,20 +747,10 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
 
 void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse)
 {
-    uint8_t index;
-
     escAndServoConfig = escAndServoConfigToUse;
     pidProfile = pidProfileToUse;
 
-    isUsingSticksToArm = true;
-
-    for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
-        modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
-        if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
-            isUsingSticksToArm = false;
-            break;
-        }
-    }
+    isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM);
 }
 
 void resetAdjustmentStates(void)

src/main/io/rc_controls.h

@@ -46,6 +46,7 @@ typedef enum {
     BOXSERVO1,
     BOXSERVO2,
     BOXSERVO3,
+    BOXBLACKBOX,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
@@ -239,3 +240,4 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rx
 bool isUsingSticksForArming(void);
 
 int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
+bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId);

src/main/io/serial_msp.c

@@ -345,7 +345,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXSERVO1, "SERVO1;", 23 },
     { BOXSERVO2, "SERVO2;", 24 },
     { BOXSERVO3, "SERVO3;", 25 },
-    
+    { BOXBLACKBOX, "BLACKBOX;", 26 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -692,6 +692,12 @@ void mspInit(serialConfig_t *serialConfig)
     }
 #endif
 
+#ifdef BLACKBOX
+    if (feature(FEATURE_BLACKBOX)){
+        activeBoxIds[activeBoxIdCount++] = BOXBLACKBOX;
+    }
+#endif
+
     memset(mspPorts, 0x00, sizeof(mspPorts));
     mspAllocateSerialPorts(serialConfig);
 }
@@ -812,7 +818,8 @@ static bool processOutCommand(uint8_t cmdMSP)
             IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
             IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) << BOXAUTOTUNE |
             IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
-            IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM;
+            IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
+            IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX;
         for (i = 0; i < activeBoxIdCount; i++) {
             int flag = (tmp & (1 << activeBoxIds[i]));
             if (flag)