Blackbox code size savings by factoring common loops out into fuctions

src/main/blackbox/blackbox.c

@@ -479,25 +479,24 @@ static void writeIntraframe(void)
     blackboxWriteUnsignedVB(blackboxIteration);
     blackboxWriteUnsignedVB(blackboxCurrent->time);
 
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
+    blackboxWriteSignedVBArray(blackboxCurrent->axisPID_P, XYZ_AXIS_COUNT);
-        blackboxWriteSignedVB(blackboxCurrent->axisPID_P[x]);
+    blackboxWriteSignedVBArray(blackboxCurrent->axisPID_I, XYZ_AXIS_COUNT);
-    }
-
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
-        blackboxWriteSignedVB(blackboxCurrent->axisPID_I[x]);
-    }
 
+    // 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++) {
+    // Write roll, pitch and yaw first:
-        blackboxWriteSignedVB(blackboxCurrent->rcCommand[x]);
+    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++) {
+            blackboxWriteSigned16VBArray(blackboxCurrent->magADC, XYZ_AXIS_COUNT);
-                blackboxWriteSignedVB(blackboxCurrent->magADC[x]);
-            }
         }
 #endif
 
@@ -538,24 +535,20 @@ static void writeIntraframe(void)
         blackboxWriteUnsignedVB(blackboxCurrent->rssi);
     }
 
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
+    blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
-        blackboxWriteSignedVB(blackboxCurrent->gyroADC[x]);
+    blackboxWriteSigned16VBArray(blackboxCurrent->accSmooth, XYZ_AXIS_COUNT);
-    }
-
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
-        blackboxWriteSignedVB(blackboxCurrent->accSmooth[x]);
-    }
 
     //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++) {
+    arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT);
-        blackboxWriteSignedVB(blackboxCurrent->axisPID_P[x] - blackboxLast->axisPID_P[x]);
+    blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
-    }
-
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
-        deltas[x] = blackboxCurrent->axisPID_I[x] - blackboxLast->axisPID_I[x];
-    }
 
     /* 
      * The PID I field changes very slowly, most of the time +-2, so use an encoding
      * that can pack all three fields into one byte in that situation.
      */
+    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:
+    //Since gyros, accs and motors are noisy, base their predictions on the average of the history:
-    for (x = 0; x < XYZ_AXIS_COUNT; x++) {
+    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC),   XYZ_AXIS_COUNT);
-        blackboxWriteSignedVB(blackboxHistory[0]->gyroADC[x] - (blackboxHistory[1]->gyroADC[x] + blackboxHistory[2]->gyroADC[x]) / 2);
+    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accSmooth), XYZ_AXIS_COUNT);
-    }
+    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor),     motorCount);
-
-    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);
-    }
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) {
         blackboxWriteSignedVB(blackboxCurrent->servo[5] - blackboxLast->servo[5]);

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

@@ -95,3 +95,5 @@ float cos_approx(float x);
 #define sin_approx(x)   sinf(x)
 #define cos_approx(x)   cosf(x)
 #endif
+
+void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);