Simplified 2D navigation routenes

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_private.h

@@ -444,6 +444,8 @@ bool adjustFixedWingAltitudeFromRCInput(void);
 bool adjustFixedWingHeadingFromRCInput(void);
 bool adjustFixedWingPositionFromRCInput(void);
 
+void applyFixedWingPositionController(timeUs_t currentTimeUs);
+
 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,68 @@
+/*
+ * 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 "common/utils.h"
+#include "fc/rc_controls.h"
+#include "flight/mixer.h"
+
+#include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+
+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];
+        // }
+
+        // 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);
+    }
+}
+
+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) {
+        applyFixedWingPositionController(currentTimeUs);
+        applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
+    }
+}
+
+#endif