From 505b58f9f0ff22dba641c0d356270d50ae43132e Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 3 Aug 2023 21:33:53 +0100 Subject: [PATCH 1/3] initial build --- docs/Settings.md | 20 +- src/main/fc/fc_msp_box.c | 4 +- src/main/fc/settings.yaml | 12 +- src/main/flight/pid.c | 15 +- src/main/navigation/navigation.c | 147 ++++++++------- src/main/navigation/navigation.h | 2 +- src/main/navigation/navigation_fixedwing.c | 8 +- src/main/navigation/navigation_multicopter.c | 181 ++++++++++++------- src/main/navigation/navigation_private.h | 3 +- 9 files changed, 216 insertions(+), 176 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index dda075cee64..69578ded008 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 fbd3bf07cb4..fa595676805 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 a0904af6f32..d3388be8fb4 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 20333071b0b..0161ac25d88 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 3ebcf53730c..34ac8e34200 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; - } - - 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(); } /*----------------------------------------------------------- @@ -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 cf4289e14a1..5c7dca762ba 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 b82b4c58013..0b4d8934d0a 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 864cde8b334..ad8b0a59bcd 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 0598467d28a..62459ca0c44 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); From 4f396a39ea66efde217d254441c2cd9fc3f8f96b Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 8 Aug 2023 22:03:34 +0100 Subject: [PATCH 2/3] Add roll control and OSD message --- docs/Settings.md | 4 +- src/main/fc/settings.yaml | 4 +- src/main/io/osd.c | 84 ++++++++++++++++++-------------- src/main/navigation/navigation.c | 23 +++++++-- 4 files changed, 71 insertions(+), 44 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 69578ded008..d4c888aaf31 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2654,11 +2654,11 @@ 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] +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 | 60 | +| 20 | 0 | 120 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d3388be8fb4..3e901022c9f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2572,11 +2572,11 @@ groups: 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]" + 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: 60 + 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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 63312af46c2..b05c1d5a50e 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -542,7 +542,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) buff[0] = ' '; } -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values +#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values if (isBfCompatibleVideoSystem(osdConfig())) { totalDigits++; digits++; @@ -636,8 +636,8 @@ static inline void osdFormatFlyTime(char *buff, textAttributes_t *attr) } } -/** - * Trim whitespace from string. +/** + * Trim whitespace from string. * Used in Stats screen on lines with multiple values. */ char *osdFormatTrimWhiteSpace(char *buff) @@ -648,7 +648,7 @@ char *osdFormatTrimWhiteSpace(char *buff) while(isspace((unsigned char)*buff)) buff++; // All spaces? - if(*buff == 0) + if(*buff == 0) return buff; // Trim trailing spaces @@ -1094,7 +1094,7 @@ void osdCrosshairPosition(uint8_t *x, uint8_t *y) * Check if this OSD layout is using scaled or unscaled throttle. * If both are used, it will default to scaled. */ -bool osdUsingScaledThrottle(void) +bool osdUsingScaledThrottle(void) { bool usingScaledThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_SCALED_THROTTLE_POS]); bool usingRCThrottle = OSD_VISIBLE(osdLayoutsConfig()->item_pos[currentLayout][OSD_THROTTLE_POS]); @@ -2979,7 +2979,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); } if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = buff[3] = '-'; + buff[0] = buff[1] = buff[2] = buff[3] = '-'; buff[digits] = SYM_MAH_MI_0; // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode buff[digits + 1] = SYM_MAH_MI_1; buff[digits + 2] = '\0'; @@ -2993,7 +2993,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%s%c", buff, SYM_AH_NM); } if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = buff[3] = '-'; + buff[0] = buff[1] = buff[2] = buff[3] = '-'; buff[digits] = SYM_MAH_NM_0; buff[digits + 1] = SYM_MAH_NM_1; buff[digits + 2] = '\0'; @@ -3009,7 +3009,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); } if (!efficiencyValid) { - buff[0] = buff[1] = buff[2] = buff[3] = '-'; + buff[0] = buff[1] = buff[2] = buff[3] = '-'; buff[digits] = SYM_MAH_KM_0; buff[digits + 1] = SYM_MAH_KM_1; buff[digits + 2] = '\0'; @@ -4094,7 +4094,7 @@ static void osdUpdateStats(void) static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) { const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"}; - uint8_t top = 1; // Start one line down leaving space at the top of the screen. + uint8_t top = 1; // Start one line down leaving space at the top of the screen. size_t multiValueLengthOffset = 0; const uint8_t statNameX = osdDisplayIsHD() ? 11 : 1; @@ -4116,14 +4116,14 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) } if (isSinglePageStatsCompatible || page == 0) { - if (feature(FEATURE_GPS)) { + if (feature(FEATURE_GPS)) { if (isSinglePageStatsCompatible) { displayWrite(osdDisplayPort, statNameX, top, "MAX/AVG SPEED :"); osdFormatVelocityStr(buff, stats.max_3D_speed, true, false); osdLeftAlignString(buff); strcat(osdFormatTrimWhiteSpace(buff),"/"); multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); osdGenerateAverageVelocityStr(buff); osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); @@ -4160,7 +4160,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) osdLeftAlignString(buff); strcat(osdFormatTrimWhiteSpace(buff), "%/"); multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); itoa(stats.min_rssi_dbm, buff, 10); tfp_sprintf(buff, "%s%c", buff, SYM_DBM); osdLeftAlignString(buff); @@ -4175,7 +4175,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) itoa(stats.min_rssi_dbm, buff, 10); tfp_sprintf(buff, "%s%c", buff, SYM_DBM); displayWrite(osdDisplayPort, statValuesX, top++, buff); - } + } displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); itoa(stats.min_lq, buff, 10); @@ -4201,7 +4201,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); } - + if (isSinglePageStatsCompatible || page == 1) { if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); @@ -4323,20 +4323,20 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) } } - const float max_gforce = accGetMeasuredMaxG(); + const float max_gforce = accGetMeasuredMaxG(); displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); displayWrite(osdDisplayPort, statValuesX, top++, buff); - const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); + const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); const float acc_extremes_min = acc_extremes[Z].min; const float acc_extremes_max = acc_extremes[Z].max; displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4); osdLeftAlignString(buff); - strcat(osdFormatTrimWhiteSpace(buff),"/"); - multiValueLengthOffset = strlen(buff); - displayWrite(osdDisplayPort, statValuesX, top, buff); + strcat(osdFormatTrimWhiteSpace(buff),"/"); + multiValueLengthOffset = strlen(buff); + displayWrite(osdDisplayPort, statValuesX, top, buff); osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3); osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); @@ -4546,41 +4546,41 @@ static void osdRefresh(timeUs_t currentTimeUs) statsCurrentPage = 0; statsAutoPagingEnabled = osdConfig()->stats_page_auto_swap_time > 0 ? true : false; osdShowStats(statsSinglePageCompatible, statsCurrentPage); - osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); + osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); } armState = ARMING_FLAG(ARMED); } // This block is entered when we're showing the "Splash", "Armed" or "Stats" screens - if (resumeRefreshAt) { - + if (resumeRefreshAt) { + // Handle events only when the "Stats" screen is being displayed. if (statsDisplayed) { // Manual paging stick commands are only applicable to multi-page stats. // ****************************** - // For single-page stats, this effectively disables the ability to cancel the + // For single-page stats, this effectively disables the ability to cancel the // automatic paging/updates with the stick commands. So unless stats_page_auto_swap_time - // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then - // "Saved Settings" should display if it is active within the refresh interval. + // is set to 0 or greater than 4 (saved settings display interval is 5 seconds), then + // "Saved Settings" should display if it is active within the refresh interval. // ****************************** // With multi-page stats, "Saved Settings" could also be missed if the user - // has canceled automatic paging using the stick commands, because that is only - // updated when osdShowStats() is called. So, in that case, they would only see - // the "Saved Settings" message if they happen to manually change pages using the - // stick commands within the interval the message is displayed. + // has canceled automatic paging using the stick commands, because that is only + // updated when osdShowStats() is called. So, in that case, they would only see + // the "Saved Settings" message if they happen to manually change pages using the + // stick commands within the interval the message is displayed. bool manualPageUpRequested = false; - bool manualPageDownRequested = false; + bool manualPageDownRequested = false; if (!statsSinglePageCompatible) { // These methods ensure the paging stick commands are held for a brief period - // Otherwise it can result in a race condition where the stats are - // updated too quickly and can result in partial blanks, etc. - if (osdIsPageUpStickCommandHeld()) { + // Otherwise it can result in a race condition where the stats are + // updated too quickly and can result in partial blanks, etc. + if (osdIsPageUpStickCommandHeld()) { manualPageUpRequested = true; statsAutoPagingEnabled = false; } else if (osdIsPageDownStickCommandHeld()) { - manualPageDownRequested = true; + manualPageDownRequested = true; statsAutoPagingEnabled = false; } } @@ -4603,7 +4603,7 @@ static void osdRefresh(timeUs_t currentTimeUs) // Process manual page change events for multi-page stats. if (manualPageUpRequested) { osdShowStats(statsSinglePageCompatible, 1); - statsCurrentPage = 1; + statsCurrentPage = 1; } else if (manualPageDownRequested) { osdShowStats(statsSinglePageCompatible, 0); statsCurrentPage = 0; @@ -4612,7 +4612,7 @@ static void osdRefresh(timeUs_t currentTimeUs) } // Handle events when either "Splash", "Armed" or "Stats" screens are displayed. - if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { + if ((currentTimeUs > resumeRefreshAt) || OSD_RESUME_UPDATES_STICK_COMMAND) { // Time elapsed or canceled by stick commands. // Exit to normal OSD operation. displayClearScreen(osdDisplayPort); @@ -4622,7 +4622,7 @@ static void osdRefresh(timeUs_t currentTimeUs) // Continue "Splash", "Armed" or "Stats" screens. displayHeartbeat(osdDisplayPort); } - + return; } @@ -4865,6 +4865,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 34ac8e34200..a8060a0b2e8 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1061,6 +1061,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE( { UNUSED(previousState); + if (STATE(MULTIROTOR) && !navConfig()->general.cruise_yaw_rate) { // course hold not possible on MR without yaw control + 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()) { @@ -1100,11 +1104,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS 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) { + int16_t cruiseYawRate = DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate); + 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 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()->general.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->general.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)); @@ -1137,6 +1150,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(n static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState) { + 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); return navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(previousState); From 972527b726b3e9fb0c786175dbe06033771a8f15 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 14 Aug 2023 22:27:44 +0100 Subject: [PATCH 3/3] fixes --- src/main/navigation/navigation.c | 6 +++--- src/main/navigation/navigation_multicopter.c | 2 +- src/main/navigation/navigation_private.h | 1 + 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a8060a0b2e8..1f6cc8f0c37 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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_REQUIRE_MAGHOLD | 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, @@ -954,7 +954,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; } @@ -1104,12 +1104,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ; } - int16_t cruiseYawRate = DEGREES_TO_CENTIDEGREES(navConfig()->general.cruise_yaw_rate); 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]; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index ad8b0a59bcd..6c0f2fa8f60 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -437,7 +437,7 @@ static void setMulticopterStopPosition(void) bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment) { - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { + if (navGetMappedFlightModes(posControl.navState) & NAV_COURSE_HOLD_MODE) { if (rcPitchAdjustment) { return adjustMulticopterCruiseSpeed(rcPitchAdjustment); } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 62459ca0c44..bc19a5e2979 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -443,6 +443,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);