diff --git a/docs/Settings.md b/docs/Settings.md index 1f0edbd036..3730b0195a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3302,6 +3302,26 @@ P gain of altitude PID controller (Fixedwing) --- +### nav_fw_soaring_motor_stop + +Stops motor when Soaring mode enabled. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | | | + +--- + +### nav_fw_soaring_pitch_deadband + +Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within deadband allowing pitch to free float whilst soaring. + +| Default | Min | Max | +| --- | --- | --- | +| 5 | 0 | 15 | + +--- + ### nav_fw_yaw_deadband Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 08e695868e..a56b817151 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -93,6 +93,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXNAVCRUISE, "NAV CRUISE", 53 }, { BOXAUTOLEVEL, "AUTO LEVEL", 54 }, { BOXPLANWPMISSION, "WP PLANNER", 55 }, + { BOXSOARING, "SOARING", 56 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -218,14 +219,13 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXNAVRTH; activeBoxIds[activeBoxIdCount++] = BOXNAVWP; activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; + activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; activeBoxIds[activeBoxIdCount++] = BOXPLANWPMISSION; - if (feature(FEATURE_GPS)) { - activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; - if (STATE(AIRPLANE)) { - activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; - activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; - } + if (STATE(AIRPLANE)) { + activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; + activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; + activeBoxIds[activeBoxIdCount++] = BOXSOARING; } } @@ -388,6 +388,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) #endif CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)), BOXAUTOLEVEL); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)), BOXPLANWPMISSION); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSOARING)), BOXSOARING); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 9985f967ea..82da9d6df0 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -73,6 +73,7 @@ typedef enum { BOXNAVCRUISE = 44, BOXAUTOLEVEL = 45, BOXPLANWPMISSION = 46, + BOXSOARING = 47, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index e77e7c18b4..4e862e7896 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -46,7 +46,7 @@ typedef enum { ARMING_DISABLED_NO_PREARM = (1 << 28), ARMING_DISABLED_DSHOT_BEEPER = (1 << 29), - ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | + ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE | @@ -99,6 +99,7 @@ typedef enum { FLAPERON = (1 << 13), TURN_ASSISTANT = (1 << 14), TURTLE_MODE = (1 << 15), + SOARING_MODE = (1 << 16), } flightModeFlags_e; extern uint32_t flightModeFlags; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4130bfb447..8cc050ea46 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2520,6 +2520,17 @@ groups: default_value: "ALL_NAV" field: general.flags.nav_overrides_motor_stop table: nav_overrides_motor_stop + - name: nav_fw_soaring_motor_stop + description: "Stops motor when Soaring mode enabled." + default_value: OFF + field: general.flags.soaring_motor_stop + type: bool + - name: nav_fw_soaring_pitch_deadband + description: "Pitch Angle deadband when soaring mode enabled (deg). Angle mode inactive within deadband allowing pitch to free float whilst soaring." + default_value: 5 + field: fw.soaring_pitch_deadband + min: 0 + max: 15 - name: nav_rth_climb_first description: "If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor)" default_value: "ON" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index cee8231c5f..de3bca4ce1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -580,6 +580,17 @@ static float calcHorizonRateMagnitude(void) return horizonRateMagnitude; } +/* ANGLE freefloat deadband (degs).Angle error only starts to increase if atttiude outside deadband. */ +int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis) +{ + int16_t levelDatum = axis == FD_PITCH ? attitude.raw[axis] + DEGREES_TO_DECIDEGREES(fixedWingLevelTrim) : attitude.raw[axis]; + if (ABS(levelDatum) > deadband) { + return levelDatum > 0 ? deadband - levelDatum : -(levelDatum + deadband); + } else { + return 0; + } +} + static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT) { // This is ROLL/PITCH, run ANGLE/HORIZON controllers @@ -621,11 +632,20 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h actual = attitude.raw[axis]; } - const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - actual); + float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - actual); #else - const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); + float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); #endif + // Soaring mode deadband inactive if pitch/roll stick not centered to allow RC stick adjustment + if (FLIGHT_MODE(SOARING_MODE) && axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) { + angleErrorDeg = DECIDEGREES_TO_DEGREES((float)angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)); + if (!angleErrorDeg) { + pidState->errorGyroIf = 0.0f; + pidState->errorGyroIfLimit = 0.0f; + } + } + float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); // Apply simple LPF to angleRateTarget to make response less jerky @@ -766,6 +786,12 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); + if (FLIGHT_MODE(SOARING_MODE) && axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) { + if (!angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)) { + axisPID[FD_PITCH] = 0; // center pitch servo if pitch attitude within soaring mode deadband + } + } + #ifdef USE_AUTOTUNE_FIXED_WING if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) { autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -pidState->pidSumLimit, +pidState->pidSumLimit)); @@ -1283,6 +1309,7 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs) !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || areSticksDeflected() || (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) || + FLIGHT_MODE(SOARING_MODE) || navigationIsControllingAltitude() ) { flags |= PID_FREEZE_INTEGRATOR; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ddd779e152..0c21c4c3ea 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4270,6 +4270,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (FLIGHT_MODE(HEADFREE_MODE)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); } + if (FLIGHT_MODE(SOARING_MODE)) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_NAV_SOARING); + } #if defined(USE_NAV) if (posControl.flags.wpMissionPlannerActive) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 454178c06a..75b8aefeff 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -100,6 +100,7 @@ #define OSD_MSG_AUTOTUNE "(AUTOTUNE)" #define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO" #define OSD_MSG_HEADFREE "(HEADFREE)" +#define OSD_MSG_NAV_SOARING "(SOARING)" #define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" #if defined(USE_SAFE_HOME) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 10ae911270..f29f305c2f 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -118,6 +118,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT, .mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs .waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action + .soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled }, // General navigation parameters @@ -198,6 +199,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .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, + .soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled } ); @@ -3295,8 +3297,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) canActivateWaypoint = false; } - // LAUNCH mode has priority over any other NAV mode - if (STATE(FIXED_WING_LEGACY)) { + /* Airplane specific modes */ + if (STATE(AIRPLANE)) { + // LAUNCH mode has priority over any other NAV mode if (isNavLaunchEnabled()) { // FIXME: Only available for fixed wing aircrafts now if (canActivateLaunchMode) { canActivateLaunchMode = false; @@ -3317,6 +3320,16 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_IDLE; } } + + /* Soaring mode, disables altitude control in Position hold and Course hold modes. + * Pitch allowed to freefloat within defined Angle mode deadband */ + if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE))) { + if (!FLIGHT_MODE(SOARING_MODE)) { + ENABLE_FLIGHT_MODE(SOARING_MODE); + } + } else { + DISABLE_FLIGHT_MODE(SOARING_MODE); + } } // Failsafe_RTH (can override MANUAL) @@ -3905,7 +3918,7 @@ bool navigationInAutomaticThrottleMode(void) bool navigationIsControllingThrottle(void) { // Note that this makes a detour into mixer code to evaluate actual motor status - return navigationInAutomaticThrottleMode() && (getMotorStatus() != MOTOR_STOPPED_USER); + return navigationInAutomaticThrottleMode() && getMotorStatus() != MOTOR_STOPPED_USER && !FLIGHT_MODE(SOARING_MODE); } bool navigationIsControllingAltitude(void) { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 4dc78404e9..86ec5d1438 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -215,6 +215,7 @@ typedef struct navConfig_s { uint8_t rth_alt_control_override; // Override RTH Altitude and Climb First settings using Pitch and Roll stick uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor uint8_t safehome_usage_mode; // Controls when safehomes are used + uint8_t soaring_motor_stop; // stop motor when Soaring mode enabled uint8_t mission_planner_reset; // Allow WP Mission Planner reset using mode toggle (resets WPs to 0) uint8_t waypoint_mission_restart; // Waypoint mission restart action } flags; @@ -288,8 +289,9 @@ 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] uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled bool allow_manual_thr_increase; - bool useFwNavYawControl; - uint8_t yawControlDeadband; + bool useFwNavYawControl; + uint8_t yawControlDeadband; + uint8_t soaring_pitch_deadband; // soaring mode pitch angle deadband (deg) } fw; } navConfig_t; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index e711877c60..27c5a47925 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -663,8 +663,8 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, if (true) { #endif if (navStateFlags & NAV_CTL_ALT) { - if (getMotorStatus() == MOTOR_STOPPED_USER) { - // Motor has been stopped by user. Update target altitude and bypass navigation pitch/throttle control + if (getMotorStatus() == MOTOR_STOPPED_USER || FLIGHT_MODE(SOARING_MODE)) { + // Motor has been stopped by user or soaring mode enabled to override altitude control resetFixedWingAltitudeController(); setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); } else { @@ -689,6 +689,10 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) { applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs); } + + if (FLIGHT_MODE(SOARING_MODE) && navConfig()->general.flags.soaring_motor_stop) { + ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); + } } }