Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Initial build

Browse source
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
Download patch file Download diff file Expand all files Collapse all files

20
docs/Settings.md
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
Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course

3
src/main/fc/fc_msp_box.c
View file

@ -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;
}
}
@ -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++) {

1
src/main/fc/rc_modes.h
View file

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

1
src/main/fc/runtime_config.h
View file

@ -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;

11
src/main/fc/settings.yaml
View file

@ -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"

31
src/main/flight/pid.c
View file

@ -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;

3
src/main/io/osd.c
View file

@ -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.

1
src/main/io/osd.h
View file

@ -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)

17
src/main/navigation/navigation.c
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
.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;
}
/* Airplane specific modes */
if (STATE(AIRPLANE)) {
// LAUNCH mode has priority over any other NAV mode
if (STATE(FIXED_WING_LEGACY)) {
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) {

2
src/main/navigation/navigation.h
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 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)
@ -269,6 +270,7 @@ typedef struct navConfig_s {
bool allow_manual_thr_increase;
bool useFwNavYawControl;
uint8_t yawControlDeadband;
uint8_t soaring_pitch_deadband; // soaring mode pitch angle deadband (deg)
} fw;
} navConfig_t;

8
src/main/navigation/navigation_fixedwing.c
View file

@ -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);
}
}
}