mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 08:45:31 +03:00
Initial build
This commit is contained in:
parent
e5145e4cc2
commit
64a3ef6833
11 changed files with 100 additions and 14 deletions
|
@ -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
|
||||||
|
|
|
@ -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++) {
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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"
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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.
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue