Merge pull request cleanflight#726 from iNavFlight/reset-maghold-on-arm

MagHold controller reset on arm and MAGHOLD enable
martinbudden · Nov 1, 2016 · 14dc698 · 14dc698
2 parents 6d02467 + 1b8c11d
commit 14dc698
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 4 deletions.
diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c
@@ -230,6 +230,8 @@ void mwArm(void)
             ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 
+            resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
+
 #ifdef BLACKBOX
             if (feature(FEATURE_BLACKBOX)) {
                 serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
@@ -399,8 +401,8 @@ void processRx(void)
     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
             if (!FLIGHT_MODE(MAG_MODE)) {
+                resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
                 ENABLE_FLIGHT_MODE(MAG_MODE);
-                updateMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
             }
         } else {
             DISABLE_FLIGHT_MODE(MAG_MODE);
@@ -656,4 +658,3 @@ void taskUpdateRxMain(void)
     updatePIDCoefficients(&currentProfile->pidProfile, currentControlRateProfile, &masterConfig.rxConfig);
     isRXDataNew = true;
 }
-
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -88,6 +88,7 @@ extern bool motorLimitReached;
 extern float dT;
 
 int16_t magHoldTargetHeading;
+static pt1Filter_t magHoldRateFilter;
 
 // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
 static float tpaFactor;
@@ -354,6 +355,12 @@ void updateMagHoldHeading(int16_t heading)
     magHoldTargetHeading = heading;
 }
 
+void resetMagHoldHeading(int16_t heading)
+{
+    updateMagHoldHeading(heading);
+    pt1FilterReset(&magHoldRateFilter, 0.0f);
+}
+
 int16_t getMagHoldHeading() {
     return magHoldTargetHeading;
 }
@@ -394,8 +401,6 @@ uint8_t getMagHoldState()
  */
 float pidMagHold(const pidProfile_t *pidProfile)
 {
-
-    static pt1Filter_t magHoldRateFilter;
     float magHoldRate;
 
     int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHoldTargetHeading;

diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
@@ -95,4 +95,5 @@ enum {
 };
 
 void updateMagHoldHeading(int16_t heading);
+void resetMagHoldHeading(int16_t heading);
 int16_t getMagHoldHeading();