Merge pull request #5592 from iNavFlight/dzikuvx-simple-2d-navigation

Simplified 2D navigation routines

make/source.mk

@@ -201,6 +201,7 @@ COMMON_SRC = \
             navigation/navigation_pos_estimator.c \
             navigation/navigation_pos_estimator_agl.c \
             navigation/navigation_pos_estimator_flow.c \
+            navigation/navigation_rover_boat.c \
             sensors/barometer.c \
             sensors/pitotmeter.c \
             sensors/rangefinder.c \

src/main/navigation/navigation.c

@@ -2982,7 +2982,9 @@ void applyWaypointNavigationAndAltitudeHold(void)
 
     /* Process controllers */
     navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
-    if (STATE(FIXED_WING_LEGACY)) {
+    if (STATE(ROVER) || STATE(BOAT)) {
+        applyRoverBoatNavigationController(navStateFlags, currentTimeUs);
+    } else if (STATE(FIXED_WING_LEGACY)) {
         applyFixedWingNavigationController(navStateFlags, currentTimeUs);
     }
     else {

src/main/navigation/navigation_fixedwing.c

@@ -203,6 +203,9 @@ bool adjustFixedWingHeadingFromRCInput(void)
 static fpVector3_t virtualDesiredPosition;
 static pt1Filter_t fwPosControllerCorrectionFilterState;
 
+/*
+ * TODO Currently this function resets both FixedWing and Rover & Boat position controller 
+ */
 void resetFixedWingPositionController(void)
 {
     virtualDesiredPosition.x = 0;
@@ -286,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;
@@ -346,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)) {
-
-        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;
+        posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
     } else {
         posControl.rcAdjustment[YAW] = 0;
     }

src/main/navigation/navigation_private.h

@@ -444,6 +444,8 @@ bool adjustFixedWingAltitudeFromRCInput(void);
 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);
@@ -456,4 +458,9 @@ bool isFixedWingLaunchFinishedOrAborted(void);
 void abortFixedWingLaunch(void);
 void applyFixedWingLaunchController(timeUs_t currentTimeUs);
 
+/*
+ * Rover specific functions
+ */
+void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
+
 #endif

src/main/navigation/navigation_rover_boat.c

@@ -0,0 +1,138 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "platform.h"
+
+FILE_COMPILE_FOR_SIZE
+
+#ifdef USE_NAV
+
+#include "build/debug.h"
+#include "common/utils.h"
+#include "fc/rc_controls.h"
+#include "flight/mixer.h"
+
+#include "navigation/navigation.h"
+#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)
+{
+    static timeUs_t previousTimePositionUpdate;         // Occurs @ GPS update rate
+    static timeUs_t previousTimeUpdate;                 // Occurs @ looptime rate
+
+    const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
+    previousTimeUpdate = currentTimeUs;
+
+    // If last position update was too long in the past - ignore it (likely restarting altitude controller)
+    if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+        previousTimeUpdate = currentTimeUs;
+        previousTimePositionUpdate = currentTimeUs;
+        resetFixedWingPositionController();
+        return;
+    }
+
+    // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
+    if ((posControl.flags.estPosStatus >= EST_USABLE)) {
+        // If we have new position - update velocity and acceleration controllers
+        if (posControl.flags.horizontalPositionDataNew) {
+            const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+            previousTimePositionUpdate = currentTimeUs;
+
+            if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+                update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate);
+            } else {
+                resetFixedWingPositionController();
+            }
+
+            // Indicate that information is no longer usable
+            posControl.flags.horizontalPositionDataConsumed = 1;
+        }
+
+        isYawAdjustmentValid = true;
+    }
+    else {
+        isYawAdjustmentValid = false;
+    }
+}
+
+void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
+{
+    UNUSED(currentTimeUs);
+    rcCommand[ROLL] = 0;
+    rcCommand[PITCH] = 0;
+
+    if (navStateFlags & NAV_CTL_POS) {
+
+        if (isYawAdjustmentValid) {
+            rcCommand[YAW] = posControl.rcAdjustment[YAW];
+        }
+
+        rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
+    }
+}
+
+void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
+{
+    if (navStateFlags & NAV_CTL_EMERG) {
+        rcCommand[ROLL] = 0;
+        rcCommand[PITCH] = 0;
+        rcCommand[YAW] = 0;
+        rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
+    } else if (navStateFlags & NAV_CTL_POS) {
+        applyRoverBoatPositionController(currentTimeUs);
+        applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
+    }
+}
+
+#endif