refactoring

martinbudden · Nov 1, 2016 · f45521c · f45521c
1 parent 7879fac
commit f45521c
Show file tree

Hide file tree

Showing 6 changed files with 17 additions and 13 deletions.
diff --git a/src/main/fc/msp_fc.c b/src/main/fc/msp_fc.c
@@ -1094,7 +1094,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_HEAD:
-        updateMagHoldHeading(sbufReadU16(src), false);
+        updateMagHoldHeading(sbufReadU16(src));
         break;
 
     case MSP_SET_RAW_RC:

diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c
@@ -230,7 +230,7 @@ void mwArm(void)
             ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 
-            updateMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw), true);
+            resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 
 #ifdef BLACKBOX
             if (feature(FEATURE_BLACKBOX)) {
@@ -401,7 +401,7 @@ void processRx(void)
     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
             if (!FLIGHT_MODE(MAG_MODE)) {
-                updateMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw), true);
+                resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
                 ENABLE_FLIGHT_MODE(MAG_MODE);
             }
         } else {

diff --git a/src/main/flight/navigation_rewrite_fixedwing.c b/src/main/flight/navigation_rewrite_fixedwing.c
@@ -286,7 +286,7 @@ static void updatePositionHeadingController_FW(uint32_t deltaMicros)
 
     // Update magHold heading lock in case pilot is using MAG mode (prevent MAGHOLD to fight navigation)
     posControl.desiredState.yaw = wrap_36000(posControl.actualState.yaw + headingError);
-    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw), false);
+    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
 
     // Add pitch compensation
     //posControl.rcAdjustment[PITCH] = -CENTIDEGREES_TO_DECIDEGREES(ABS(rollAdjustment)) * 0.50f;
@@ -480,7 +480,7 @@ void calculateFixedWingInitialHoldPosition(t_fp_vector * pos)
 
 void resetFixedWingHeadingController(void)
 {
-    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw), false);
+    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
 }
 
 void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, uint32_t currentTime)

diff --git a/src/main/flight/navigation_rewrite_multicopter.c b/src/main/flight/navigation_rewrite_multicopter.c
@@ -615,12 +615,12 @@ void calculateMulticopterInitialHoldPosition(t_fp_vector * pos)
 
 void resetMulticopterHeadingController(void)
 {
-    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw), false);
+    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
 }
 
 static void applyMulticopterHeadingController(void)
 {
-    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw), false);
+    updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
 }
 
 void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, uint32_t currentTime)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -350,12 +350,15 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
 #endif
 }
 
-void updateMagHoldHeading(int16_t heading, uint8_t reset)
+void updateMagHoldHeading(int16_t heading)
 {
     magHoldTargetHeading = heading;
-    if (reset) {
-        pt1FilterReset(&magHoldRateFilter, (float) heading);
-    }
+}
+
+void resetMagHoldHeading(int16_t heading)
+{
+    updateMagHoldHeading(heading);
+    pt1FilterReset(&magHoldRateFilter, (float) heading);
 }
 
 int16_t getMagHoldHeading() {
@@ -476,7 +479,7 @@ void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *co
     uint8_t magHoldState = getMagHoldState();
 
     if (magHoldState == MAG_HOLD_UPDATE_HEADING) {
-        updateMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw), false);
+        updateMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
     }
 
     for (int axis = 0; axis < 3; axis++) {

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