Heading controller

src/main/navigation/navigation_fixedwing.c

@@ -289,6 +289,34 @@ bool adjustFixedWingPositionFromRCInput(void)
     return (rcRollAdjustment);
 }
 
+float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing) {
+    static float limit = 0.0f;
+
+    if (limit == 0.0f) {
+        limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
+    }
+
+    const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
+
+    const float yawAdjustment = navPidApply2(
+        &posControl.pids.fw_heading, 
+        0, 
+        applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100), 
+        US2S(deltaMicros),
+        -limit,
+        limit,
+        yawPidFlags
+        ) / 100.0f;
+
+    DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
+    DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
+    DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
+    DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
+    DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
+
+    return yawAdjustment;
+}
+
 static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta_t deltaMicros)
 {
     static timeUs_t previousTimeMonitoringUpdate;
@@ -349,32 +377,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
      * It is working in relative mode and we aim to keep error at zero
      */
     if (STATE(FW_HEADING_USE_YAW)) {
-
+        posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
-        static float limit = 0.0f;
-
-        if (limit == 0.0f) {
-            limit = pidProfile()->navFwPosHdgPidsumLimit * 100.0f;
-        }
-
-        const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
-
-        float yawAdjustment = navPidApply2(
-            &posControl.pids.fw_heading, 
-            0, 
-            applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100), 
-            US2S(deltaMicros),
-            -limit,
-            limit,
-            yawPidFlags
-            ) / 100.0f;
-
-        DEBUG_SET(DEBUG_NAV_YAW, 0, posControl.pids.fw_heading.proportional);
-        DEBUG_SET(DEBUG_NAV_YAW, 1, posControl.pids.fw_heading.integral);
-        DEBUG_SET(DEBUG_NAV_YAW, 2, posControl.pids.fw_heading.derivative);
-        DEBUG_SET(DEBUG_NAV_YAW, 3, navHeadingError);
-        DEBUG_SET(DEBUG_NAV_YAW, 4, posControl.pids.fw_heading.output_constrained);
-
-        posControl.rcAdjustment[YAW] = yawAdjustment;
     } else {
         posControl.rcAdjustment[YAW] = 0;
     }

src/main/navigation/navigation_private.h

@@ -445,7 +445,7 @@ bool adjustFixedWingHeadingFromRCInput(void);
 bool adjustFixedWingPositionFromRCInput(void);
 
 void applyFixedWingPositionController(timeUs_t currentTimeUs);
-
+float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing);
 void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
 
 void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);

src/main/navigation/navigation_rover_boat.c

@@ -28,6 +28,7 @@ FILE_COMPILE_FOR_SIZE
 
 #ifdef USE_NAV
 
+#include "build/debug.h"
 #include "common/utils.h"
 #include "fc/rc_controls.h"
 #include "flight/mixer.h"
@@ -36,6 +37,34 @@ FILE_COMPILE_FOR_SIZE
 #include "navigation/navigation_private.h"
 
 static bool isYawAdjustmentValid = false;
+static int32_t navHeadingError;
+
+static void update2DPositionHeadingController(timeUs_t currentTimeUs, timeDelta_t deltaMicros)
+{
+    static timeUs_t previousTimeMonitoringUpdate;
+    static int32_t previousHeadingError;
+    static bool errorIsDecreasing;
+
+    int32_t targetBearing = calculateBearingToDestination(&posControl.desiredState.pos);
+
+    /*
+     * Calculate NAV heading error
+     * Units are centidegrees
+     */
+    navHeadingError = wrap_18000(targetBearing - posControl.actualState.yaw);
+
+    // Slow error monitoring (2Hz rate)
+    if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
+        // Check if error is decreasing over time
+        errorIsDecreasing = (ABS(previousHeadingError) > ABS(navHeadingError));
+
+        // Save values for next iteration
+        previousHeadingError = navHeadingError;
+        previousTimeMonitoringUpdate = currentTimeUs;
+    }
+
+    posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
+}
 
 void applyRoverBoatPositionController(timeUs_t currentTimeUs)
 {
@@ -61,17 +90,8 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs)
             previousTimePositionUpdate = currentTimeUs;
 
             if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
-                // Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ)
+                update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate);
-                // Account for pilot's roll input (move position target left/right at max of max_manual_speed)
+            } else {
-                // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
-                // FIXME: verify the above
-                // calculateVirtualPositionTarget_FW(HZ2S(MIN_POSITION_UPDATE_RATE_HZ) * 2);
-
-                // updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate);
-
-                //FIXME build a simple 2D position controller 
-            }
-            else {
                 resetFixedWingPositionController();
             }
 
@@ -98,7 +118,6 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
             rcCommand[YAW] = posControl.rcAdjustment[YAW];
         }
 
-        // const uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
         rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
     }
 }
@@ -111,7 +130,7 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
         rcCommand[YAW] = 0;
         rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
     } else if (navStateFlags & NAV_CTL_POS) {
-        applyFixedWingPositionController(currentTimeUs);
+        applyRoverBoatPositionController(currentTimeUs);
         applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
     }
 }