1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 08:45:31 +03:00

Initial build

This commit is contained in:
breadoven 2021-07-11 21:49:54 +01:00
parent e5145e4cc2
commit 64a3ef6833
11 changed files with 100 additions and 14 deletions

View file

@ -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 ### nav_fw_yaw_deadband
Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course

View file

@ -92,6 +92,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXTURTLE, "TURTLE", 52 }, { BOXTURTLE, "TURTLE", 52 },
{ BOXNAVCRUISE, "NAV CRUISE", 53 }, { BOXNAVCRUISE, "NAV CRUISE", 53 },
{ BOXAUTOLEVEL, "AUTO LEVEL", 54 }, { BOXAUTOLEVEL, "AUTO LEVEL", 54 },
{ BOXSOARING, "SOARING", 55 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF } { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
}; };
@ -210,6 +211,7 @@ void initActiveBoxIds(void)
} }
if (STATE(AIRPLANE)) { if (STATE(AIRPLANE)) {
activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN; activeBoxIds[activeBoxIdCount++] = BOXLOITERDIRCHN;
activeBoxIds[activeBoxIdCount++] = BOXSOARING;
} }
} }
@ -247,7 +249,7 @@ void initActiveBoxIds(void)
if (!feature(FEATURE_FW_AUTOTRIM)) { if (!feature(FEATURE_FW_AUTOTRIM)) {
activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM; activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
} }
#if defined(USE_AUTOTUNE_FIXED_WING) #if defined(USE_AUTOTUNE_FIXED_WING)
activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE; activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
#endif #endif
@ -385,6 +387,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE);
#endif #endif
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)), BOXAUTOLEVEL); 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)); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
for (uint32_t i = 0; i < activeBoxIdCount; i++) { for (uint32_t i = 0; i < activeBoxIdCount; i++) {

View file

@ -72,6 +72,7 @@ typedef enum {
BOXTURTLE = 43, BOXTURTLE = 43,
BOXNAVCRUISE = 44, BOXNAVCRUISE = 44,
BOXAUTOLEVEL = 45, BOXAUTOLEVEL = 45,
BOXSOARING = 46,
CHECKBOX_ITEM_COUNT CHECKBOX_ITEM_COUNT
} boxId_e; } boxId_e;

View file

@ -46,7 +46,7 @@ typedef enum {
ARMING_DISABLED_NO_PREARM = (1 << 28), ARMING_DISABLED_NO_PREARM = (1 << 28),
ARMING_DISABLED_DSHOT_BEEPER = (1 << 29), 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_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED | ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED |
ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE |
@ -99,6 +99,7 @@ typedef enum {
FLAPERON = (1 << 13), FLAPERON = (1 << 13),
TURN_ASSISTANT = (1 << 14), TURN_ASSISTANT = (1 << 14),
TURTLE_MODE = (1 << 15), TURTLE_MODE = (1 << 15),
SOARING_MODE = (1 << 16),
} flightModeFlags_e; } flightModeFlags_e;
extern uint32_t flightModeFlags; extern uint32_t flightModeFlags;

View file

@ -2499,6 +2499,17 @@ groups:
default_value: "ALL_NAV" default_value: "ALL_NAV"
field: general.flags.nav_overrides_motor_stop field: general.flags.nav_overrides_motor_stop
table: 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 - 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)" 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" default_value: "ON"

View file

@ -593,6 +593,17 @@ static float calcHorizonRateMagnitude(void)
return horizonRateMagnitude; 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) static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT)
{ {
// This is ROLL/PITCH, run ANGLE/HORIZON controllers // 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]; actual = attitude.raw[axis];
} }
const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - actual); float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - actual);
#else #else
const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
#endif #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); 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 // 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); 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 #ifdef USE_AUTOTUNE_FIXED_WING
if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) { if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -pidState->pidSumLimit, +pidState->pidSumLimit)); 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) || !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) ||
areSticksDeflected() || areSticksDeflected() ||
(!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) || (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) ||
FLIGHT_MODE(SOARING_MODE) ||
navigationIsControllingAltitude() navigationIsControllingAltitude()
) { ) {
flags |= PID_FREEZE_INTEGRATOR; flags |= PID_FREEZE_INTEGRATOR;

View file

@ -3984,6 +3984,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
if (FLIGHT_MODE(HEADFREE_MODE)) { if (FLIGHT_MODE(HEADFREE_MODE)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); 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 // Pick one of the available messages. Each message lasts
// a second. // a second.

View file

@ -99,6 +99,7 @@
#define OSD_MSG_AUTOTUNE "(AUTOTUNE)" #define OSD_MSG_AUTOTUNE "(AUTOTUNE)"
#define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO" #define OSD_MSG_AUTOTUNE_ACRO "SWITCH TO ACRO"
#define OSD_MSG_HEADFREE "(HEADFREE)" #define OSD_MSG_HEADFREE "(HEADFREE)"
#define OSD_MSG_NAV_SOARING "(SOARING)"
#define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" #define OSD_MSG_UNABLE_ARM "UNABLE TO ARM"
#if defined(USE_SAFE_HOME) #if defined(USE_SAFE_HOME)

View file

@ -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 .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, .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT,
.safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_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 // 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, .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT, .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_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; canActivateWaypoint = false;
} }
// LAUNCH mode has priority over any other NAV mode /* Airplane specific modes */
if (STATE(FIXED_WING_LEGACY)) { if (STATE(AIRPLANE)) {
// LAUNCH mode has priority over any other NAV mode
if (isNavLaunchEnabled()) { // FIXME: Only available for fixed wing aircrafts now if (isNavLaunchEnabled()) { // FIXME: Only available for fixed wing aircrafts now
if (canActivateLaunchMode) { if (canActivateLaunchMode) {
canActivateLaunchMode = false; canActivateLaunchMode = false;
@ -3203,6 +3206,16 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
return NAV_FSM_EVENT_SWITCH_TO_IDLE; 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) // Failsafe_RTH (can override MANUAL)
@ -3672,7 +3685,7 @@ bool navigationInAutomaticThrottleMode(void)
bool navigationIsControllingThrottle(void) bool navigationIsControllingThrottle(void)
{ {
// Note that this makes a detour into mixer code to evaluate actual motor status // 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) { bool navigationIsControllingAltitude(void) {

View file

@ -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 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 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 safehome_usage_mode; // Controls when safehomes are used
uint8_t soaring_motor_stop; // stop motor when Soaring mode enabled
} flags; } flags;
uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) 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 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 uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
bool allow_manual_thr_increase; bool allow_manual_thr_increase;
bool useFwNavYawControl; bool useFwNavYawControl;
uint8_t yawControlDeadband; uint8_t yawControlDeadband;
uint8_t soaring_pitch_deadband; // soaring mode pitch angle deadband (deg)
} fw; } fw;
} navConfig_t; } navConfig_t;

View file

@ -243,18 +243,18 @@ static int8_t loiterDirection(void) {
if (pidProfile()->loiter_direction == NAV_LOITER_YAW) { if (pidProfile()->loiter_direction == NAV_LOITER_YAW) {
if (rcCommand[YAW] < -250) { if (rcCommand[YAW] < -250) {
loiterDirYaw = 1; //RIGHT //yaw is contrariwise loiterDirYaw = 1; //RIGHT //yaw is contrariwise
} }
if (rcCommand[YAW] > 250) { if (rcCommand[YAW] > 250) {
loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c
} }
dir = loiterDirYaw; dir = loiterDirYaw;
} }
if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) { if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) {
dir *= -1; dir *= -1;
} }
@ -651,8 +651,8 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
if (true) { if (true) {
#endif #endif
if (navStateFlags & NAV_CTL_ALT) { if (navStateFlags & NAV_CTL_ALT) {
if (getMotorStatus() == MOTOR_STOPPED_USER) { if (getMotorStatus() == MOTOR_STOPPED_USER || FLIGHT_MODE(SOARING_MODE)) {
// Motor has been stopped by user. Update target altitude and bypass navigation pitch/throttle control // Motor has been stopped by user or soaring mode enabled to override altitude control
resetFixedWingAltitudeController(); resetFixedWingAltitudeController();
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z);
} else { } else {
@ -677,6 +677,10 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) { if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs); applyFixedWingPitchRollThrottleController(navStateFlags, currentTimeUs);
} }
if (FLIGHT_MODE(SOARING_MODE) && navConfig()->general.flags.soaring_motor_stop) {
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
}
} }
} }