diff --git a/docs/Settings.md b/docs/Settings.md index dda075cee6..69578ded00 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2652,6 +2652,16 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci --- +### nav_cruise_yaw_rate + +Max YAW rate when NAV CRUISE mode is enabled (set to 0 to disable) [dps] + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 0 | 60 | + +--- + ### nav_disarm_on_landing If set to ON, INAV disarms the FC after landing @@ -2742,16 +2752,6 @@ Cruise throttle in GPS assisted modes, this includes RTH. Should be set high eno --- -### nav_fw_cruise_yaw_rate - -Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps] - -| Default | Min | Max | -| --- | --- | --- | -| 20 | 0 | 60 | - ---- - ### nav_fw_dive_angle Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index fbd3bf07cb..fa59567680 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -227,6 +227,8 @@ void initActiveBoxIds(void) if (!STATE(ALTITUDE_CONTROL) || (STATE(ALTITUDE_CONTROL) && navReadyAltControl)) { ADD_ACTIVE_BOX(BOXNAVRTH); ADD_ACTIVE_BOX(BOXNAVWP); + ADD_ACTIVE_BOX(BOXNAVCRUISE); + ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD); ADD_ACTIVE_BOX(BOXHOMERESET); ADD_ACTIVE_BOX(BOXGCSNAV); ADD_ACTIVE_BOX(BOXPLANWPMISSION); @@ -236,8 +238,6 @@ void initActiveBoxIds(void) } if (STATE(AIRPLANE)) { - ADD_ACTIVE_BOX(BOXNAVCRUISE); - ADD_ACTIVE_BOX(BOXNAVCOURSEHOLD); ADD_ACTIVE_BOX(BOXSOARING); } } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a0904af6f3..d3388be8fb 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2571,6 +2571,12 @@ groups: default_value: ON field: general.flags.mission_planner_reset type: bool + - name: nav_cruise_yaw_rate + description: "Max YAW rate when NAV CRUISE mode is enabled (set to 0 to disable) [dps]" + default_value: 20 + field: general.cruise_yaw_rate + min: 0 + max: 60 - name: nav_mc_bank_angle description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude" default_value: 30 @@ -2806,12 +2812,6 @@ groups: field: fw.launch_abort_deadband min: 2 max: 250 - - name: nav_fw_cruise_yaw_rate - description: "Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]" - default_value: 20 - field: fw.cruise_yaw_rate - min: 0 - max: 60 - name: nav_fw_allow_manual_thr_increase description: "Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing" default_value: OFF diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 20333071b0..0161ac25d8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -900,17 +900,14 @@ float pidHeadingHold(float dT) { float headingHoldRate; + /* Convert absolute error into relative to current heading */ int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget; - /* - * Convert absolute error into relative to current heading - */ - if (error <= -180) { - error += 360; - } - - if (error >= +180) { + /* Convert absolute error into relative to current heading */ + if (error > 180) { error -= 360; + } else if (error < -180) { + error += 360; } /* @@ -1124,7 +1121,7 @@ void FAST_CODE pidController(float dT) pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON levelingEnabled = true; - } + } } if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 3ebcf53730..34ac8e3420 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -159,6 +159,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT, // Changes sensitivity of landing detection .auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled .rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT, + .cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps }, // MC-specific @@ -213,7 +214,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF .launch_abort_deadband = SETTING_NAV_FW_LAUNCH_ABORT_DEADBAND_DEFAULT, // 100 us - .cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT, .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT, .yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT, @@ -428,7 +428,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, + .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW, .mapToFlightModes = NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, @@ -487,7 +487,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, + .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, @@ -1061,10 +1061,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( { UNUSED(previousState); - if (!STATE(FIXED_WING_LEGACY)) { // Only on FW for now - return NAV_FSM_EVENT_ERROR; - } - DEBUG_SET(DEBUG_CRUISE, 0, 1); // Switch to IDLE if we do not have an healty position. Try the next iteration. if (checkForPositionSensorTimeout()) { @@ -1073,7 +1069,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( resetPositionController(); - posControl.cruise.course = posControl.actualState.cog; // Store the course to follow + if (STATE(AIRPLANE)) { + posControl.cruise.course = posControl.actualState.cog; // Store the course to follow + } else { // Multicopter + posControl.cruise.course = posControl.actualState.yaw; + posControl.cruise.multicopterSpeed = constrainf(posControl.actualState.velXY, 10.0f, navConfig()->general.max_manual_speed); + posControl.desiredState.pos = posControl.actualState.abs.pos; + } posControl.cruise.previousCourse = posControl.cruise.course; posControl.cruise.lastCourseAdjustmentTime = 0; @@ -1094,7 +1096,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS DEBUG_SET(DEBUG_CRUISE, 0, 2); DEBUG_SET(DEBUG_CRUISE, 2, 0); - if (posControl.flags.isAdjustingPosition) { + if (STATE(AIRPLANE) && posControl.flags.isAdjustingPosition) { // inhibit for MR, pitch/roll only adjusts speed using pitch return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ; } @@ -1102,7 +1104,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS if (posControl.flags.isAdjustingHeading) { timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastCourseAdjustmentTime; if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference. - float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate)); + float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate)); float centidegsPerIteration = rateTarget * MS2S(timeDifference); posControl.cruise.course = wrap_36000(posControl.cruise.course - centidegsPerIteration); DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.course)); @@ -1135,8 +1137,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState) { - if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now - navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState); return navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(previousState); @@ -2469,27 +2469,27 @@ void checkSafeHomeState(bool shouldBeEnabled) posControl.flags.rthTrackbackActive || (!safehome_applied && posControl.homeDistance < navConfig()->general.min_rth_distance); - if (safehomeNotApplicable) { - shouldBeEnabled = false; - } else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) { - // if safehomes are only used with failsafe and we're trying to enable safehome - // then enable the safehome only with failsafe - shouldBeEnabled = posControl.flags.forcedRTHActivated; - } + if (safehomeNotApplicable) { + shouldBeEnabled = false; + } else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) { + // if safehomes are only used with failsafe and we're trying to enable safehome + // then enable the safehome only with failsafe + shouldBeEnabled = posControl.flags.forcedRTHActivated; + } // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything - if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) { - return; - } + if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) { + return; + } if (shouldBeEnabled) { - // set home to safehome + // set home to safehome setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - safehome_applied = true; - } else { - // set home to original arming point + safehome_applied = true; + } else { + // set home to original arming point setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - safehome_applied = false; - } - // if we've changed the home position, update the distance and direction + safehome_applied = false; + } + // if we've changed the home position, update the distance and direction updateHomePosition(); } @@ -2506,8 +2506,8 @@ bool findNearestSafeHome(void) gpsLocation_t shLLH; shLLH.alt = 0; for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) { - if (!safeHomeConfig(i)->enabled) - continue; + if (!safeHomeConfig(i)->enabled) + continue; shLLH.lat = safeHomeConfig(i)->lat; shLLH.lon = safeHomeConfig(i)->lon; @@ -2523,7 +2523,7 @@ bool findNearestSafeHome(void) } } if (safehome_index >= 0) { - safehome_distance = nearest_safehome_distance; + safehome_distance = nearest_safehome_distance; } else { safehome_distance = 0; } @@ -3441,33 +3441,34 @@ bool isNavHoldPositionActive(void) navigationIsExecutingAnEmergencyLanding(); } -float getActiveWaypointSpeed(void) +float getActiveSpeed(void) { - if (posControl.flags.isAdjustingPosition) { - // In manual control mode use different cap for speed + /* Currently only applicable for multicopter */ + + // Speed limit for modes where speed manually controlled + if (posControl.flags.isAdjustingPosition || FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { return navConfig()->general.max_manual_speed; } - else { - uint16_t waypointSpeed = navConfig()->general.auto_speed; - if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) { - if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) { - float wpSpecificSpeed = 0.0f; - if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME) - wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time - else - wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case + uint16_t waypointSpeed = navConfig()->general.auto_speed; - if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) { - waypointSpeed = wpSpecificSpeed; - } else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) { - waypointSpeed = navConfig()->general.max_auto_speed; - } + if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) { + if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) { + float wpSpecificSpeed = 0.0f; + if(posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME) + wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p2; // P1 is hold time + else + wpSpecificSpeed = posControl.waypointList[posControl.activeWaypointIndex].p1; // default case + + if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) { + waypointSpeed = wpSpecificSpeed; + } else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) { + waypointSpeed = navConfig()->general.max_auto_speed; } } - - return waypointSpeed; } + + return waypointSpeed; } bool isWaypointNavTrackingActive(void) @@ -3486,29 +3487,21 @@ static void processNavigationRCAdjustments(void) { /* Process pilot's RC input. Disable all pilot's input when in FAILSAFE_MODE */ navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState); - if ((navStateFlags & NAV_RC_ALT) && (!FLIGHT_MODE(FAILSAFE_MODE))) { - posControl.flags.isAdjustingAltitude = adjustAltitudeFromRCInput(); - } - else { - posControl.flags.isAdjustingAltitude = false; - } - if (navStateFlags & NAV_RC_POS) { - posControl.flags.isAdjustingPosition = adjustPositionFromRCInput() && !FLIGHT_MODE(FAILSAFE_MODE); - if (STATE(MULTIROTOR) && FLIGHT_MODE(FAILSAFE_MODE)) { + if (FLIGHT_MODE(FAILSAFE_MODE)) { + if (STATE(MULTIROTOR) && navStateFlags & NAV_RC_POS) { resetMulticopterBrakingMode(); } - } - else { + posControl.flags.isAdjustingAltitude = false; posControl.flags.isAdjustingPosition = false; + posControl.flags.isAdjustingHeading = false; + + return; } - if ((navStateFlags & NAV_RC_YAW) && (!FLIGHT_MODE(FAILSAFE_MODE))) { - posControl.flags.isAdjustingHeading = adjustHeadingFromRCInput(); - } - else { - posControl.flags.isAdjustingHeading = false; - } + posControl.flags.isAdjustingAltitude = (navStateFlags & NAV_RC_ALT) && adjustAltitudeFromRCInput(); + posControl.flags.isAdjustingPosition = (navStateFlags & NAV_RC_POS) && adjustPositionFromRCInput(); + posControl.flags.isAdjustingHeading = (navStateFlags & NAV_RC_YAW) && adjustHeadingFromRCInput(); } /*----------------------------------------------------------- @@ -3864,17 +3857,16 @@ int8_t navigationGetHeadingControlState(void) } // For multirotors it depends on navigation system mode + // Course hold requires Auto Control to update heading hold target whilst RC adjustment active if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) { - if (posControl.flags.isAdjustingHeading) { + if (posControl.flags.isAdjustingHeading && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { return NAV_HEADING_CONTROL_MANUAL; } - else { - return NAV_HEADING_CONTROL_AUTO; - } - } - else { - return NAV_HEADING_CONTROL_NONE; + + return NAV_HEADING_CONTROL_AUTO; } + + return NAV_HEADING_CONTROL_NONE; } bool navigationTerrainFollowingEnabled(void) @@ -4411,3 +4403,8 @@ bool isAdjustingHeading(void) { int32_t getCruiseHeadingAdjustment(void) { return wrap_18000(posControl.cruise.course - posControl.cruise.previousCourse); } + +int32_t navigationGetHeadingError(void) +{ + return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog); +} diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index cf4289e14a..5c7dca762b 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -268,6 +268,7 @@ typedef struct navConfig_s { uint8_t land_detect_sensitivity; // Sensitivity of landing detector uint16_t auto_disarm_delay; // safety time delay for landing detector uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately) + uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled } general; struct { @@ -315,7 +316,6 @@ typedef struct navConfig_s { uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] bool launch_manual_throttle; // Allows launch with manual throttle control uint8_t launch_abort_deadband; // roll/pitch stick movement deadband for launch abort - uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled bool allow_manual_thr_increase; bool useFwNavYawControl; uint8_t yawControlDeadband; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index b82b4c5801..0b4d8934d0 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -71,7 +71,6 @@ static bool isRollAdjustmentValid = false; static bool isYawAdjustmentValid = false; static float throttleSpeedAdjustment = 0; static bool isAutoThrottleManuallyIncreased = false; -static int32_t navHeadingError; static float navCrossTrackError; static int8_t loiterDirYaw = 1; bool needToCalculateCircularLoiter; @@ -440,7 +439,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta * Calculate NAV heading error * Units are centidegrees */ - navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog); + int32_t navHeadingError = wrap_18000(virtualTargetBearing - posControl.actualState.cog); // Forced turn direction // If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg @@ -845,11 +844,6 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, } } -int32_t navigationGetHeadingError(void) -{ - return navHeadingError; -} - float navigationGetCrossTrackError(void) { return navCrossTrackError; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 864cde8b33..ad8b0a59bc 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -222,7 +222,7 @@ void resetMulticopterAltitudeController(void) pt1FilterReset(&posControl.pids.vel[Z].dterm_filter_state, 0.0f); if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { - const float maxSpeed = getActiveWaypointSpeed(); + const float maxSpeed = getActiveSpeed(); nav_speed_up = maxSpeed; nav_accel_z = maxSpeed; nav_speed_down = navConfig()->general.max_auto_climb_rate; @@ -289,14 +289,15 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) bool adjustMulticopterHeadingFromRCInput(void) { if (ABS(rcCommand[YAW]) > rcControlsConfig()->pos_hold_deadband) { - // Can only allow pilot to set the new heading if doing PH, during RTH copter will target itself to home - posControl.desiredState.yaw = posControl.actualState.yaw; + // Heading during Cruise Hold mode set by Nav function so no adjustment required here + if (!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + posControl.desiredState.yaw = posControl.actualState.yaw; + } return true; } - else { - return false; - } + + return false; } /*----------------------------------------------------------- @@ -406,8 +407,44 @@ void resetMulticopterPositionController(void) } } +static bool adjustMulticopterCruiseSpeed(int16_t rcPitchAdjustment) +{ + static timeMs_t lastUpdateTimeMs; + const timeMs_t currentTimeMs = millis(); + const timeMs_t updateDeltaTimeMs = currentTimeMs - lastUpdateTimeMs; + lastUpdateTimeMs = currentTimeMs; + + const float rcVelX = rcPitchAdjustment * navConfig()->general.max_manual_speed / (float)(500 - rcControlsConfig()->pos_hold_deadband); + + if (rcVelX > posControl.cruise.multicopterSpeed) { + posControl.cruise.multicopterSpeed = rcVelX; + } else if (rcVelX < 0 && updateDeltaTimeMs < 100) { + posControl.cruise.multicopterSpeed += MS2S(updateDeltaTimeMs) * rcVelX / 2.0f; + } else { + return false; + } + posControl.cruise.multicopterSpeed = constrainf(posControl.cruise.multicopterSpeed, 10.0f, navConfig()->general.max_manual_speed); + + return true; +} + +static void setMulticopterStopPosition(void) +{ + fpVector3_t stopPosition; + calculateMulticopterInitialHoldPosition(&stopPosition); + setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY); +} + bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment) { + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + if (rcPitchAdjustment) { + return adjustMulticopterCruiseSpeed(rcPitchAdjustment); + } + + return false; + } + // Process braking mode processMulticopterBrakingMode(rcPitchAdjustment || rcRollAdjustment); @@ -429,16 +466,12 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR return true; } - else { + else if (posControl.flags.isAdjustingPosition) { // Adjusting finished - reset desired position to stay exactly where pilot released the stick - if (posControl.flags.isAdjustingPosition) { - fpVector3_t stopPosition; - calculateMulticopterInitialHoldPosition(&stopPosition); - setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY); - } - - return false; + setMulticopterStopPosition(); } + + return false; } static float getVelocityHeadingAttenuationFactor(void) @@ -467,15 +500,28 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax) static void updatePositionVelocityController_MC(const float maxSpeed) { + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + // Position held at cruise speeds below 0.5 m/s, otherwise desired neu velocities set directly from cruise speed + if (posControl.cruise.multicopterSpeed >= 50) { + // Rotate multicopter x velocity from body frame to earth frame + posControl.desiredState.vel.x = posControl.cruise.multicopterSpeed * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.cruise.course)); + posControl.desiredState.vel.y = posControl.cruise.multicopterSpeed * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.cruise.course)); + + return; + } else if (posControl.flags.isAdjustingPosition) { + setMulticopterStopPosition(); + } + } + const float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x; const float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y; // Calculate target velocity - float newVelX = posErrorX * posControl.pids.pos[X].param.kP; - float newVelY = posErrorY * posControl.pids.pos[Y].param.kP; + float neuVelX = posErrorX * posControl.pids.pos[X].param.kP; + float neuVelY = posErrorY * posControl.pids.pos[Y].param.kP; // Scale velocity to respect max_speed - float newVelTotal = calc_length_pythagorean_2D(newVelX, newVelY); + float neuVelTotal = calc_length_pythagorean_2D(neuVelX, neuVelY); /* * We override computed speed with max speed in following cases: @@ -485,26 +531,23 @@ static void updatePositionVelocityController_MC(const float maxSpeed) if ( ((navGetCurrentStateFlags() & NAV_AUTO_WP || posControl.flags.rthTrackbackActive) && !isNavHoldPositionActive() && - newVelTotal < maxSpeed && + neuVelTotal < maxSpeed && !navConfig()->mc.slowDownForTurning - ) || newVelTotal > maxSpeed + ) || neuVelTotal > maxSpeed ) { - newVelX = maxSpeed * (newVelX / newVelTotal); - newVelY = maxSpeed * (newVelY / newVelTotal); - newVelTotal = maxSpeed; + neuVelX = maxSpeed * (neuVelX / neuVelTotal); + neuVelY = maxSpeed * (neuVelY / neuVelTotal); + neuVelTotal = maxSpeed; } - posControl.pids.pos[X].output_constrained = newVelX; - posControl.pids.pos[Y].output_constrained = newVelY; + posControl.pids.pos[X].output_constrained = neuVelX; + posControl.pids.pos[Y].output_constrained = neuVelY; // Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode) const float velHeadFactor = getVelocityHeadingAttenuationFactor(); - const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed); - posControl.desiredState.vel.x = newVelX * velHeadFactor * velExpoFactor; - posControl.desiredState.vel.y = newVelY * velHeadFactor * velExpoFactor; - - navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767); - navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767); + const float velExpoFactor = getVelocityExpoAttenuationFactor(neuVelTotal, maxSpeed); + posControl.desiredState.vel.x = neuVelX * velHeadFactor * velExpoFactor; + posControl.desiredState.vel.y = neuVelY * velHeadFactor * velExpoFactor; } static float computeNormalizedVelocity(const float value, const float maxValue) @@ -664,49 +707,53 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA static void applyMulticopterPositionController(timeUs_t currentTimeUs) { - static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate - bool bypassPositionController; - - // We should passthrough rcCommand is adjusting position in GPS_ATTI mode - bypassPositionController = (navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI) && posControl.flags.isAdjustingPosition; - // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode // and pilots input would be passed thru to PID controller - if ((posControl.flags.estPosStatus >= EST_USABLE)) { - // If we have new position - update velocity and acceleration controllers - if (posControl.flags.horizontalPositionDataNew) { - const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; - previousTimePositionUpdate = currentTimeUs; - - if (!bypassPositionController) { - // Update position controller - if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { - // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s - const float maxSpeed = getActiveWaypointSpeed(); - updatePositionVelocityController_MC(maxSpeed); - updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed); - } - else { - // Position update has not occurred in time (first start or glitch), reset altitude controller - resetMulticopterPositionController(); - } - } - - // Indicate that information is no longer usable - posControl.flags.horizontalPositionDataConsumed = true; - } - } - else { + if (posControl.flags.estPosStatus < EST_USABLE) { /* No position data, disable automatic adjustment, rcCommand passthrough */ posControl.rcAdjustment[PITCH] = 0; posControl.rcAdjustment[ROLL] = 0; - bypassPositionController = true; + + return; } - if (!bypassPositionController) { - rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]); - rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); + // Passthrough rcCommand if adjusting position in GPS_ATTI mode except when Course Hold active + bool bypassPositionController = !FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && + navConfig()->general.flags.user_control_mode == NAV_GPS_ATTI && + posControl.flags.isAdjustingPosition; + + if (posControl.flags.horizontalPositionDataNew) { + // Indicate that information is no longer usable + posControl.flags.horizontalPositionDataConsumed = true; + + static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + previousTimePositionUpdate = currentTimeUs; + + if (bypassPositionController) { + return; + } + + // If we have new position data - update velocity and acceleration controllers + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { + // Get max speed for current NAV mode + float maxSpeed = getActiveSpeed(); + updatePositionVelocityController_MC(maxSpeed); + updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed); + + navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767); + navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767); + } + else { + // Position update has not occurred in time (first start or glitch), reset position controller + resetMulticopterPositionController(); + } + } else if (bypassPositionController) { + return; } + + rcCommand[PITCH] = pidAngleToRcCommand(posControl.rcAdjustment[PITCH], pidProfile()->max_angle_inclination[FD_PITCH]); + rcCommand[ROLL] = pidAngleToRcCommand(posControl.rcAdjustment[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); } bool isMulticopterFlying(void) @@ -933,6 +980,10 @@ void resetMulticopterHeadingController(void) static void applyMulticopterHeadingController(void) { + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { // heading set by Nav during Course Hold so disable yaw stick input + rcCommand[YAW] = 0; + } + updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw)); } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 0598467d28..62459ca0c4 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -325,6 +325,7 @@ typedef struct { int32_t course; int32_t previousCourse; timeMs_t lastCourseAdjustmentTime; + float multicopterSpeed; } navCruise_t; typedef struct { @@ -451,7 +452,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti bool isNavHoldPositionActive(void); bool isLastMissionWaypoint(void); -float getActiveWaypointSpeed(void); +float getActiveSpeed(void); bool isWaypointNavTrackingActive(void); void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);