diff --git a/make/source.mk b/make/source.mk index f781f6f473..565c806c2b 100644 --- a/make/source.mk +++ b/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 \ diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 5c155676b6..266115ff7c 100755 --- a/src/main/navigation/navigation.c +++ b/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 { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index fe2e34ba18..9b01c2c679 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/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; } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 2a17449318..41a5b4e95a 100755 --- a/src/main/navigation/navigation_private.h +++ b/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 diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c new file mode 100644 index 0000000000..ea61afaa91 --- /dev/null +++ b/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 \ No newline at end of file