mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 16:55:29 +03:00
Merge pull request #1873 from iNavFlight/de_failsafe_nav_fix
Fix failsafe & navigation clash
This commit is contained in:
commit
b9fe92e8a9
3 changed files with 16 additions and 0 deletions
|
@ -88,12 +88,14 @@ typedef enum {
|
||||||
} failsafeChannelBehavior_e;
|
} failsafeChannelBehavior_e;
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
|
bool bypassNavigation;
|
||||||
bool forceAngleMode;
|
bool forceAngleMode;
|
||||||
failsafeChannelBehavior_e channelBehavior[4];
|
failsafeChannelBehavior_e channelBehavior[4];
|
||||||
} failsafeProcedureLogic_t;
|
} failsafeProcedureLogic_t;
|
||||||
|
|
||||||
static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
|
static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
|
||||||
[FAILSAFE_PROCEDURE_AUTO_LANDING] = {
|
[FAILSAFE_PROCEDURE_AUTO_LANDING] = {
|
||||||
|
.bypassNavigation = true,
|
||||||
.forceAngleMode = true,
|
.forceAngleMode = true,
|
||||||
.channelBehavior = {
|
.channelBehavior = {
|
||||||
FAILSAFE_CHANNEL_AUTO, // ROLL
|
FAILSAFE_CHANNEL_AUTO, // ROLL
|
||||||
|
@ -104,6 +106,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
|
||||||
},
|
},
|
||||||
|
|
||||||
[FAILSAFE_PROCEDURE_DROP_IT] = {
|
[FAILSAFE_PROCEDURE_DROP_IT] = {
|
||||||
|
.bypassNavigation = true,
|
||||||
.forceAngleMode = true,
|
.forceAngleMode = true,
|
||||||
.channelBehavior = {
|
.channelBehavior = {
|
||||||
FAILSAFE_CHANNEL_NEUTRAL, // ROLL
|
FAILSAFE_CHANNEL_NEUTRAL, // ROLL
|
||||||
|
@ -114,6 +117,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
|
||||||
},
|
},
|
||||||
|
|
||||||
[FAILSAFE_PROCEDURE_RTH] = {
|
[FAILSAFE_PROCEDURE_RTH] = {
|
||||||
|
.bypassNavigation = false,
|
||||||
.forceAngleMode = true,
|
.forceAngleMode = true,
|
||||||
.channelBehavior = {
|
.channelBehavior = {
|
||||||
FAILSAFE_CHANNEL_NEUTRAL, // ROLL
|
FAILSAFE_CHANNEL_NEUTRAL, // ROLL
|
||||||
|
@ -124,6 +128,7 @@ static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
|
||||||
},
|
},
|
||||||
|
|
||||||
[FAILSAFE_PROCEDURE_NONE] = {
|
[FAILSAFE_PROCEDURE_NONE] = {
|
||||||
|
.bypassNavigation = false,
|
||||||
.forceAngleMode = false,
|
.forceAngleMode = false,
|
||||||
.channelBehavior = {
|
.channelBehavior = {
|
||||||
FAILSAFE_CHANNEL_HOLD, // ROLL
|
FAILSAFE_CHANNEL_HOLD, // ROLL
|
||||||
|
@ -164,6 +169,11 @@ void failsafeInit(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef NAV
|
#ifdef NAV
|
||||||
|
bool failsafeBypassNavigation(void)
|
||||||
|
{
|
||||||
|
return failsafeState.active && failsafeState.controlling && failsafeProcedureLogic[failsafeConfig()->failsafe_procedure].bypassNavigation;
|
||||||
|
}
|
||||||
|
|
||||||
bool failsafeMayRequireNavigationMode(void)
|
bool failsafeMayRequireNavigationMode(void)
|
||||||
{
|
{
|
||||||
return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH;
|
return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH;
|
||||||
|
|
|
@ -164,6 +164,7 @@ void failsafeApplyControlInput(void);
|
||||||
bool failsafeRequiresAngleMode(void);
|
bool failsafeRequiresAngleMode(void);
|
||||||
bool failsafeRequiresMotorStop(void);
|
bool failsafeRequiresMotorStop(void);
|
||||||
bool failsafeShouldApplyControlInput(void);
|
bool failsafeShouldApplyControlInput(void);
|
||||||
|
bool failsafeBypassNavigation(void);
|
||||||
void failsafeUpdateRcCommandValues(void);
|
void failsafeUpdateRcCommandValues(void);
|
||||||
|
|
||||||
void failsafeOnValidDataReceived(void);
|
void failsafeOnValidDataReceived(void);
|
||||||
|
|
|
@ -2326,6 +2326,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
||||||
|
|
||||||
//We can switch modes only when ARMED
|
//We can switch modes only when ARMED
|
||||||
if (ARMING_FLAG(ARMED)) {
|
if (ARMING_FLAG(ARMED)) {
|
||||||
|
// Ask failsafe system if we can use navigation system
|
||||||
|
if (failsafeBypassNavigation()) {
|
||||||
|
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||||
|
}
|
||||||
|
|
||||||
// Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
|
// Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
|
||||||
bool canActivateAltHold = canActivateAltHoldMode();
|
bool canActivateAltHold = canActivateAltHoldMode();
|
||||||
bool canActivatePosHold = canActivatePosHoldMode();
|
bool canActivatePosHold = canActivatePosHoldMode();
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue