Blackbox: make logging of autotune events more detailed

src/main/blackbox/blackbox.c

@@ -354,6 +354,12 @@ static void writeSignedVB(int32_t value)
     writeUnsignedVB((uint32_t)((value << 1) ^ (value >> 31)));
 }
 
+static void writeS16(int16_t value)
+{
+    blackboxWrite(value & 0xFF);
+    blackboxWrite((value >> 8) & 0xFF);
+}
+
 /**
  * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits
  */
@@ -1270,17 +1276,24 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
         break;
         case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START:
             blackboxWrite(data->autotuneCycleStart.phase);
-            blackboxWrite(data->autotuneCycleStart.cycle);
+            blackboxWrite(data->autotuneCycleStart.cycle | (data->autotuneCycleStart.rising ? 0x80 : 0));
             blackboxWrite(data->autotuneCycleStart.p);
             blackboxWrite(data->autotuneCycleStart.i);
             blackboxWrite(data->autotuneCycleStart.d);
         break;
         case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT:
-            blackboxWrite(data->autotuneCycleResult.overshot);
+            blackboxWrite(data->autotuneCycleResult.flags);
             blackboxWrite(data->autotuneCycleStart.p);
             blackboxWrite(data->autotuneCycleStart.i);
             blackboxWrite(data->autotuneCycleStart.d);
         break;
+        case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS:
+            writeS16(data->autotuneTargets.currentAngle);
+            blackboxWrite((uint8_t) data->autotuneTargets.targetAngle);
+            blackboxWrite((uint8_t) data->autotuneTargets.targetAngleAtPeak);
+            writeS16(data->autotuneTargets.firstPeakAngle);
+            writeS16(data->autotuneTargets.secondPeakAngle);
+        break;
         case FLIGHT_LOG_EVENT_LOG_END:
             blackboxPrint("End of log");
             blackboxWrite(0);

src/main/blackbox/blackbox_fielddefs.h

@@ -101,6 +101,7 @@ typedef enum FlightLogEvent {
     FLIGHT_LOG_EVENT_SYNC_BEEP = 0,
     FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10,
     FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11,
+    FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12,
     FLIGHT_LOG_EVENT_LOG_END = 255
 } FlightLogEvent;
 
@@ -114,21 +115,31 @@ typedef struct flightLogEvent_autotuneCycleStart_t {
     uint8_t p;
     uint8_t i;
     uint8_t d;
+    uint8_t rising;
 } flightLogEvent_autotuneCycleStart_t;
 
+#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT 1
+#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT 2
+
 typedef struct flightLogEvent_autotuneCycleResult_t {
-    uint8_t overshot;
+    uint8_t flags;
     uint8_t p;
     uint8_t i;
     uint8_t d;
 } flightLogEvent_autotuneCycleResult_t;
 
+typedef struct flightLogEvent_autotuneTargets_t {
+    uint16_t currentAngle;
+    int8_t targetAngle, targetAngleAtPeak;
+    uint16_t firstPeakAngle, secondPeakAngle;
+} flightLogEvent_autotuneTargets_t;
+
 typedef union flightLogEventData_t
 {
     flightLogEvent_syncBeep_t syncBeep;
     flightLogEvent_autotuneCycleStart_t autotuneCycleStart;
     flightLogEvent_autotuneCycleResult_t autotuneCycleResult;
-
+    flightLogEvent_autotuneTargets_t autotuneTargets;
 } flightLogEventData_t;
 
 typedef struct flightLogEvent_t

src/main/flight/autotune.c

@@ -161,11 +161,32 @@ static void autotuneLogCycleStart()
         eventData.p = pid.p * MULTIWII_P_MULTIPLIER;
         eventData.i = pid.i * MULTIWII_I_MULTIPLIER;
         eventData.d = pid.d;
+        eventData.rising = rising ? 1 : 0;
 
         blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START, (flightLogEventData_t*)&eventData);
     }
 }
 
+static void autotuneLogAngleTargets(float currentAngle)
+{
+    if (feature(FEATURE_BLACKBOX)) {
+        flightLogEvent_autotuneTargets_t eventData;
+
+        // targetAngle is always just -AUTOTUNE_TARGET_ANGLE or +AUTOTUNE_TARGET_ANGLE so no need for float precision:
+        eventData.targetAngle = (int) targetAngle;
+        // and targetAngleAtPeak is set to targetAngle so it has the same small precision requirement:
+        eventData.targetAngleAtPeak = (int) targetAngleAtPeak;
+
+        // currentAngle is integer decidegrees divided by 10, so just reverse that process to get an integer again:
+        eventData.currentAngle = round(currentAngle * 10);
+        // the peak angles are only ever set to currentAngle, so they get the same treatment:
+        eventData.firstPeakAngle = round(firstPeakAngle * 10);
+        eventData.secondPeakAngle = round(secondPeakAngle * 10);
+
+        blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS, (flightLogEventData_t*)&eventData);
+    }
+}
+
 #endif
 
 static void startNewCycle(void)
@@ -199,6 +220,7 @@ static void updateTargetAngle(void)
 float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle)
 {
     float currentAngle;
+    bool overshot;
 
     if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) {
         return errorAngle;
@@ -229,6 +251,10 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin
     debug[2] = DEGREES_TO_DECIDEGREES(targetAngle);
 #endif
 
+#ifdef BLACKBOX
+    autotuneLogAngleTargets(currentAngle);
+#endif
+
     if (secondPeakAngle == 0) {
         // The peak will be when our angular velocity is negative.  To be sure we are in the right place,
         // we also check to make sure our angle position is greater than zero.
@@ -244,20 +270,22 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin
             switch (cycle) {
                 case CYCLE_TUNE_I:
                     // when checking the I value, we would like to overshoot the target position by half of the max oscillation.
-                    if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) {
+                    overshot = currentAngle - targetAngle >= AUTOTUNE_MAX_OSCILLATION_ANGLE / 2;
-                        pid.i *= AUTOTUNE_INCREASE_MULTIPLIER;
+                    
-                    } else {
+                    if (overshot) {
                         pid.i *= AUTOTUNE_DECREASE_MULTIPLIER;
                         if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) {
                             pid.i = AUTOTUNE_MINIMUM_I_VALUE;
                         }
+                    } else {
+                        pid.i *= AUTOTUNE_INCREASE_MULTIPLIER;
                     }
 
 #ifdef BLACKBOX
                     if (feature(FEATURE_BLACKBOX)) {
                         flightLogEvent_autotuneCycleResult_t eventData;
 
-                        eventData.overshot = currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 ? 0 : 1;
+                        eventData.flags = overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT: 0;
                         eventData.p = pidProfile->P8[pidIndex];
                         eventData.i = pidProfile->I8[pidIndex];
                         eventData.d = pidProfile->D8[pidIndex];
@@ -300,7 +328,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin
             // analyze the data
             // Our goal is to have zero overshoot and to have AUTOTUNE_MAX_OSCILLATION_ANGLE amplitude
 
-            bool overshot = firstPeakAngle > targetAngleAtPeak;
+            overshot = firstPeakAngle > targetAngleAtPeak;
             if (overshot) {
 #ifdef DEBUG_AUTOTUNE
                 debug[0] = 1;
@@ -338,7 +366,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin
             if (feature(FEATURE_BLACKBOX)) {
                 flightLogEvent_autotuneCycleResult_t eventData;
 
-                eventData.overshot = overshot;
+                eventData.flags = (overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT : 0) | (timedOut ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT : 0);
                 eventData.p = pidProfile->P8[pidIndex];
                 eventData.i = pidProfile->I8[pidIndex];
                 eventData.d = pidProfile->D8[pidIndex];