diff --git a/docs/Settings.md b/docs/Settings.md index f1d8c3ecc89..3fe84833ab7 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2662,6 +2662,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 COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps] + +| Default | Min | Max | +| --- | --- | --- | +| 20 | 0 | 120 | + +--- + ### nav_disarm_on_landing If set to ON, INAV disarms the FC after landing @@ -2752,16 +2762,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 0890b1d6056..59aba5f8fe9 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -231,6 +231,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); @@ -240,8 +242,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 127d10a8775..10a53df3e9b 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2584,6 +2584,12 @@ groups: default_value: ON field: general.flags.mission_planner_reset type: bool + - name: nav_cruise_yaw_rate + description: "Max YAW rate when NAV COURSE HOLD/CRUISE mode is enabled. Set to 0 to disable on fixed wing (Note: On multirotor setting to 0 will disable Course Hold/Cruise mode completely) [dps]" + default_value: 20 + field: general.cruise_yaw_rate + min: 0 + max: 120 - 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 @@ -2819,12 +2825,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 16d0ee2d9eb..b2fd0383a25 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -898,17 +898,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; } /* diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 8983fc5792d..f6c97e4733d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4905,6 +4905,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter // by OSD_FLYMODE. messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); } + if (STATE(MULTIROTOR) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + if (posControl.cruise.multicopterSpeed >= 50.0f) { + char buf[6]; + osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, false, false); + tfp_sprintf(messageBuf, "(SPD %s)", buf); + } else { + strcpy(messageBuf, "(HOLD)"); + } + messages[messageCount++] = messageBuf; + } if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) && !feature(FEATURE_FW_AUTOTRIM)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 17cfe3a5609..14da4282f8c 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -152,6 +152,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 @@ -207,7 +208,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, @@ -422,7 +422,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, @@ -481,7 +481,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_REQUIRE_THRTILT | 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, @@ -948,7 +948,7 @@ static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) return navFSM[state].stateFlags; } -static flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state) +flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state) { return navFSM[state].mapToFlightModes; } @@ -1055,7 +1055,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( { UNUSED(previousState); - if (!STATE(FIXED_WING_LEGACY)) { // Only on FW for now + if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control return NAV_FSM_EVENT_ERROR; } @@ -1067,7 +1067,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; @@ -1088,15 +1094,24 @@ 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; } - // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile - if (posControl.flags.isAdjustingHeading) { + const bool mcRollStickHeadingAdjustmentActive = STATE(MULTIROTOR) && ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband; + + // User demanding yaw -> yaw stick on FW, yaw or roll sticks on MR + // We record the desired course and change the desired target in the meanwhile + if (posControl.flags.isAdjustingHeading || mcRollStickHeadingAdjustmentActive) { + int16_t cruiseYawRate = DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate); + int16_t headingAdjustCommand = rcCommand[YAW]; + if (mcRollStickHeadingAdjustmentActive && ABS(rcCommand[ROLL]) > ABS(headingAdjustCommand)) { + headingAdjustCommand = -rcCommand[ROLL]; + } + 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)headingAdjustCommand, -500.0f, 500.0f, -cruiseYawRate, cruiseYawRate); 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)); @@ -1129,7 +1144,9 @@ 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 + if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control + return NAV_FSM_EVENT_ERROR; + } navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState); @@ -2448,13 +2465,13 @@ void checkSafeHomeState(bool shouldBeEnabled) safehomeNotApplicable = safehomeNotApplicable || (MULTI_FUNC_FLAG(MF_SUSPEND_SAFEHOMES) && !posControl.flags.forcedRTHActivated); #endif - 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 (posControl.safehomeState.distance == 0 || posControl.safehomeState.isApplied == shouldBeEnabled) { return; @@ -2485,8 +2502,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; @@ -3426,33 +3443,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) @@ -3471,29 +3489,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; - } - - if ((navStateFlags & NAV_RC_YAW) && (!FLIGHT_MODE(FAILSAFE_MODE))) { - posControl.flags.isAdjustingHeading = adjustHeadingFromRCInput(); - } - else { posControl.flags.isAdjustingHeading = false; + + return; } + + posControl.flags.isAdjustingAltitude = (navStateFlags & NAV_RC_ALT) && adjustAltitudeFromRCInput(); + posControl.flags.isAdjustingPosition = (navStateFlags & NAV_RC_POS) && adjustPositionFromRCInput(); + posControl.flags.isAdjustingHeading = (navStateFlags & NAV_RC_YAW) && adjustHeadingFromRCInput(); } /*----------------------------------------------------------- @@ -3850,17 +3860,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) @@ -4398,3 +4407,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 8458a3cd2c0..3a9223ff639 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -267,6 +267,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 91354d97aed..9bc0fa25dc2 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; @@ -445,7 +444,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 @@ -851,11 +850,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 7f0b4061ec7..93b6333af8a 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -218,7 +218,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; @@ -285,14 +285,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; } /*----------------------------------------------------------- @@ -402,8 +403,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 (navGetMappedFlightModes(posControl.navState) & NAV_COURSE_HOLD_MODE) { + if (rcPitchAdjustment) { + return adjustMulticopterCruiseSpeed(rcPitchAdjustment); + } + + return false; + } + // Process braking mode processMulticopterBrakingMode(rcPitchAdjustment || rcRollAdjustment); @@ -425,16 +462,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) @@ -463,15 +496,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: @@ -481,26 +527,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) @@ -660,49 +703,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) @@ -926,6 +973,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 872f417dddd..f5072c42195 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -327,6 +327,7 @@ typedef struct { int32_t course; int32_t previousCourse; timeMs_t lastCourseAdjustmentTime; + float multicopterSpeed; } navCruise_t; typedef struct { @@ -453,6 +454,7 @@ bool isFixedWingFlying(void); bool isMulticopterFlying(void); navigationFSMStateFlags_t navGetCurrentStateFlags(void); +flightModeFlags_e navGetMappedFlightModes(navigationFSMState_t state); void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFlags_t useMask, navigationHomeFlags_t homeFlags); void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask); @@ -462,7 +464,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt 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);