From 64a3ef68333c3df876832613ea910e49c0940401 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 11 Jul 2021 21:49:54 +0100 Subject: [PATCH 1/3] Initial build --- docs/Settings.md | 20 ++++++++++++++ src/main/fc/fc_msp_box.c | 5 +++- src/main/fc/rc_modes.h | 1 + src/main/fc/runtime_config.h | 3 ++- src/main/fc/settings.yaml | 11 ++++++++ src/main/flight/pid.c | 31 ++++++++++++++++++++-- src/main/io/osd.c | 3 +++ src/main/io/osd.h | 1 + src/main/navigation/navigation.c | 19 ++++++++++--- src/main/navigation/navigation.h | 6 +++-- src/main/navigation/navigation_fixedwing.c | 14 ++++++---- 11 files changed, 100 insertions(+), 14 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index f61ca5ef513..d04d8c50935 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -3372,6 +3372,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 9b0a90e54b1..dc719dd45e0 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -92,6 +92,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXTURTLE, "TURTLE", 52 }, { BOXNAVCRUISE, "NAV CRUISE", 53 }, { BOXAUTOLEVEL, "AUTO LEVEL", 54 }, + { BOXSOARING, "SOARING", 55 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -210,6 +211,7 @@ void initActiveBoxIds(void) } if (STATE(AIRPLANE)) { activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; + activeBoxIds[activeBoxIdCount++] = BOXSOARING; } } @@ -247,7 +249,7 @@ void initActiveBoxIds(void) if (!feature(FEATURE_FW_AUTOTRIM)) { activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM; } - + #if defined(USE_AUTOTUNE_FIXED_WING) activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE; #endif @@ -385,6 +387,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE); #endif CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)), BOXAUTOLEVEL); + 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 9c47866cab8..933b27a6bdf 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -72,6 +72,7 @@ typedef enum { BOXTURTLE = 43, BOXNAVCRUISE = 44, BOXAUTOLEVEL = 45, + BOXSOARING = 46, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index e77e7c18b4e..4e862e78964 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 1d821e4467a..cfcf3133b65 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2499,6 +2499,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 ac4b36cf451..904955317ce 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -593,6 +593,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 @@ -634,11 +645,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 @@ -776,6 +796,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)); @@ -1298,6 +1324,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 2bfc41c94e1..f16cc36051c 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3984,6 +3984,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); + } } // Pick one of the available messages. Each message lasts // a second. diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 8863d9a7320..5ae67392265 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -99,6 +99,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 d5694cfaf41..6aac7ed2105 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -114,6 +114,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT, // Override RTH Altitude and Climb First using Pitch and Roll stick .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT, .safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT, + .soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled }, // General navigation parameters @@ -191,6 +192,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 } ); @@ -3181,8 +3183,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; @@ -3203,6 +3206,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) @@ -3672,7 +3685,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 3ca1b1a4e72..0bbc87752b3 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -199,6 +199,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 } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) @@ -267,8 +268,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 484529944a0..0121cbe21ff 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -243,18 +243,18 @@ static int8_t loiterDirection(void) { if (pidProfile()->loiter_direction == NAV_LOITER_YAW) { - if (rcCommand[YAW] < -250) { + if (rcCommand[YAW] < -250) { loiterDirYaw = 1; //RIGHT //yaw is contrariwise } - if (rcCommand[YAW] > 250) { + if (rcCommand[YAW] > 250) { loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c } dir = loiterDirYaw; } - if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) { + if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) { dir *= -1; } @@ -651,8 +651,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 { @@ -677,6 +677,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); + } } } From 1d64e0a34f25d938e656063eedece69cae4596f3 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 12 Jul 2021 09:44:04 +0100 Subject: [PATCH 2/3] Update fc_msp_box.c --- src/main/fc/fc_msp_box.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index dc719dd45e0..4e5cd8c2887 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -211,7 +211,6 @@ void initActiveBoxIds(void) } if (STATE(AIRPLANE)) { activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; - activeBoxIds[activeBoxIdCount++] = BOXSOARING; } } @@ -225,6 +224,7 @@ void initActiveBoxIds(void) if (STATE(AIRPLANE)) { activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; + activeBoxIds[activeBoxIdCount++] = BOXSOARING; } } } From 6f6371b3c0e923707248c281ef69fe2e04b082ec Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 12 Jul 2021 10:18:14 +0100 Subject: [PATCH 3/3] Update fc_msp_box.c --- src/main/fc/fc_msp_box.c | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 4e5cd8c2887..18112a8ba74 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -218,14 +218,12 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXNAVRTH; activeBoxIds[activeBoxIdCount++] = BOXNAVWP; activeBoxIds[activeBoxIdCount++] = BOXHOMERESET; + activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; - if (feature(FEATURE_GPS)) { - activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; - if (STATE(AIRPLANE)) { - activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; - activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; - activeBoxIds[activeBoxIdCount++] = BOXSOARING; - } + if (STATE(AIRPLANE)) { + activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; + activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; + activeBoxIds[activeBoxIdCount++] = BOXSOARING; } }