Add BlackBox recording for G-Tune

cleanflight · Mar 2, 2015 · c6baeed · c6baeed
1 parent 5b7523c
commit c6baeed
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 3 deletions.
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
@@ -1009,6 +1009,11 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data)
             blackboxWriteS16(data->autotuneTargets.firstPeakAngle);
             blackboxWriteS16(data->autotuneTargets.secondPeakAngle);
         break;
+        case FLIGHT_LOG_EVENT_GTUNE_RESULT:
+            blackboxWrite(data->gtuneCycleResult.gtuneAxis);
+            blackboxWrite(data->gtuneCycleResult.gtuneGyroAVG);
+            blackboxWrite(data->gtuneCycleResult.gtuneNewP);
+        break;
         case FLIGHT_LOG_EVENT_LOG_END:
             blackboxPrint("End of log");
             blackboxWrite(0);

diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h
@@ -102,6 +102,7 @@ typedef enum FlightLogEvent {
     FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10,
     FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11,
     FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12,
+    FLIGHT_LOG_EVENT_GTUNE_RESULT = 20,
     FLIGHT_LOG_EVENT_LOG_END = 255
 } FlightLogEvent;
 
@@ -134,12 +135,19 @@ typedef struct flightLogEvent_autotuneTargets_t {
     uint16_t firstPeakAngle, secondPeakAngle;
 } flightLogEvent_autotuneTargets_t;
 
+typedef struct flightLogEvent_gtuneCycleResult_t {
+    uint8_t gtuneAxis;
+    uint32_t gtuneGyroAVG;
+    uint8_t gtuneNewP;
+} flightLogEvent_gtuneCycleResult_t;
+
 typedef union flightLogEventData_t
 {
     flightLogEvent_syncBeep_t syncBeep;
     flightLogEvent_autotuneCycleStart_t autotuneCycleStart;
     flightLogEvent_autotuneCycleResult_t autotuneCycleResult;
     flightLogEvent_autotuneTargets_t autotuneTargets;
+    flightLogEvent_gtuneCycleResult_t gtuneCycleResult;
 } flightLogEventData_t;
 
 typedef struct flightLogEvent_t

diff --git a/src/main/flight/gtune.c b/src/main/flight/gtune.c
@@ -151,9 +151,22 @@ void calculate_Gtune(uint8_t axis)
                 } else {
                     if (ABS(diff_G) > threshP && axis != FD_YAW) result_P64[axis] -= 32; // Don't use antiwobble for YAW
                 }
-                result_P64[axis] = constrain(result_P64[axis], (int16_t)pidProfile->gtune_lolimP[axis] << 6, (int16_t)pidProfile->gtune_hilimP[axis] << 6);
-                if (floatPID) pidProfile->P_f[axis] = (float)((result_P64[axis] >> 6) / 10); // new P value for float PID
-                else pidProfile->P8[axis] = result_P64[axis] >> 6;              // new P value
+                int16_t newP = constrain((result_P64[axis] >> 6), (int16_t)pidProfile->gtune_lolimP[axis], (int16_t)pidProfile->gtune_hilimP[axis]);
+
+#ifdef BLACKBOX
+                if (feature(FEATURE_BLACKBOX)) {
+                	flightLogEvent_gtuneCycleResult_t eventData;
+
+                    eventData.gtuneAxis = axis;
+                    eventData.gtuneGyroAVG = AvgGyro[axis];
+                    eventData.gtuneNewP = newP;
+
+                    blackboxLogEvent(FLIGHT_LOG_EVENT_GTUNE_RESULT, (flightLogEventData_t*)&eventData);
+                }
+#endif
+
+                if (floatPID) pidProfile->P_f[axis] = (float)(newP / 10);     // new P value for float PID
+                else pidProfile->P8[axis] = newP;                             // new P value
             }
             OldError[axis] = error;
         }