mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 09:16:01 +03:00
Merge pull request #1196 from iNavFlight/failsafe-phase2-mods
Failsafe refactoring - phase 2
This commit is contained in:
commit
ae3b4fb8f8
6 changed files with 174 additions and 75 deletions
|
@ -320,6 +320,10 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
|
|||
"NOTCH"
|
||||
};
|
||||
|
||||
static const char * const lookupTableNoSignalThrottle[] = {
|
||||
"HOLD", "DROP"
|
||||
};
|
||||
|
||||
typedef struct lookupTableEntry_s {
|
||||
const char * const *values;
|
||||
const uint8_t valueCount;
|
||||
|
@ -375,6 +379,7 @@ typedef enum {
|
|||
TABLE_OSD,
|
||||
#endif
|
||||
TABLE_DEBUG,
|
||||
TABLE_RX_NOSIGNAL_THROTTLE,
|
||||
LOOKUP_TABLE_COUNT
|
||||
} lookupTableIndex_e;
|
||||
|
||||
|
@ -428,6 +433,7 @@ static const lookupTableEntry_t lookupTables[] = {
|
|||
{ lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
|
||||
#endif
|
||||
{ lookupTableDebug, sizeof(lookupTableDebug) / sizeof(char *) },
|
||||
{ lookupTableNoSignalThrottle, sizeof(lookupTableNoSignalThrottle) / sizeof(char *) },
|
||||
};
|
||||
|
||||
#define VALUE_TYPE_OFFSET 0
|
||||
|
@ -567,6 +573,7 @@ static const clivalue_t valueTable[] = {
|
|||
#endif
|
||||
{ "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) },
|
||||
{ "rx_max_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_max_usec) },
|
||||
{ "rx_nosignal_throttle", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RX_NOSIGNAL_THROTTLE }, PG_RX_CONFIG, offsetof(rxConfig_t, rxNoSignalThrottleBehavior) },
|
||||
|
||||
// PG_BLACKBOX_CONFIG
|
||||
#ifdef BLACKBOX
|
||||
|
@ -589,6 +596,7 @@ static const clivalue_t valueTable[] = {
|
|||
{ "failsafe_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_kill_switch) },
|
||||
{ "failsafe_throttle_low_delay",VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 300 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_throttle_low_delay) },
|
||||
{ "failsafe_procedure", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FAILSAFE_PROCEDURE }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_procedure) },
|
||||
{ "failsafe_stick_threshold", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_stick_motion_threshold) },
|
||||
{ "failsafe_fw_roll_angle", VAR_INT16 | MASTER_VALUE, .config.minmax = { -800, 800 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_fw_roll_angle) },
|
||||
{ "failsafe_fw_pitch_angle", VAR_INT16 | MASTER_VALUE, .config.minmax = { -800, 800 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_fw_pitch_angle) },
|
||||
{ "failsafe_fw_yaw_rate", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_FAILSAFE_CONFIG, offsetof(failsafeConfig_t, failsafe_fw_yaw_rate) },
|
||||
|
|
|
@ -173,28 +173,26 @@ static void updatePreArmingChecks(void)
|
|||
|
||||
void annexCode(void)
|
||||
{
|
||||
bool needRPY = true;
|
||||
bool needThrottle = true;
|
||||
int32_t throttleValue;
|
||||
|
||||
if (failsafeIsActive()) {
|
||||
const failsafeControlChannels_e failsafeControlInput = failsafeShouldApplyControlInput();
|
||||
|
||||
// In some cases our failsafe logic will still allow values from RX to be used
|
||||
if (failsafeControlInput != FAILSAFE_CONTROL_NONE) {
|
||||
failsafeApplyControlInput();
|
||||
|
||||
needRPY = !(failsafeControlInput & FAILSAFE_CONTROL_RPY);
|
||||
needThrottle = !(failsafeControlInput & FAILSAFE_CONTROL_THROTTLE);
|
||||
}
|
||||
// Failsafe will apply rcCommand for us
|
||||
failsafeApplyControlInput();
|
||||
}
|
||||
|
||||
if (needRPY) {
|
||||
else {
|
||||
// Compute ROLL PITCH and YAW command
|
||||
rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], currentControlRateProfile->rcExpo8, rcControlsConfig()->deadband);
|
||||
rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], currentControlRateProfile->rcExpo8, rcControlsConfig()->deadband);
|
||||
rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], currentControlRateProfile->rcYawExpo8, rcControlsConfig()->yaw_deadband);
|
||||
|
||||
//Compute THROTTLE command
|
||||
throttleValue = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
|
||||
throttleValue = (uint32_t)(throttleValue - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000]
|
||||
rcCommand[THROTTLE] = rcLookupThrottle(throttleValue);
|
||||
|
||||
// Signal updated rcCommand values to Failsafe system
|
||||
failsafeUpdateRcCommandValues();
|
||||
|
||||
if (FLIGHT_MODE(HEADFREE_MODE)) {
|
||||
const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
|
||||
const float cosDiff = cos_approx(radDiff);
|
||||
|
@ -205,13 +203,6 @@ void annexCode(void)
|
|||
}
|
||||
}
|
||||
|
||||
if (needThrottle) {
|
||||
//Compute THROTTLE command
|
||||
throttleValue = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
|
||||
throttleValue = (uint32_t)(throttleValue - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000]
|
||||
rcCommand[THROTTLE] = rcLookupThrottle(throttleValue);
|
||||
}
|
||||
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
LED0_ON;
|
||||
} else {
|
||||
|
@ -385,7 +376,7 @@ void processRx(timeUs_t currentTimeUs)
|
|||
|
||||
bool canUseHorizonMode = true;
|
||||
|
||||
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive() || naivationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
|
||||
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || naivationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
|
||||
// bumpless transfer to Level mode
|
||||
canUseHorizonMode = false;
|
||||
|
||||
|
|
|
@ -26,6 +26,7 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
#include "config/parameter_group.h"
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
||||
|
@ -39,6 +40,7 @@
|
|||
#include "fc/controlrate_profile.h"
|
||||
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
|
||||
#include "navigation/navigation.h"
|
||||
|
@ -73,8 +75,62 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
|
|||
.failsafe_fw_roll_angle = -200, // 20 deg left
|
||||
.failsafe_fw_pitch_angle = 100, // 10 deg dive (yes, positive means dive)
|
||||
.failsafe_fw_yaw_rate = -45, // 45 deg/s left yaw (4s for full turn)
|
||||
.failsafe_stick_motion_threshold = 0,
|
||||
);
|
||||
|
||||
typedef enum {
|
||||
FAILSAFE_CHANNEL_HOLD, // Hold last known good value
|
||||
FAILFAFE_CHANNEL_NEUTRAL, // RPY = zero, THR = zero
|
||||
FAILSAFE_CHANNEL_AUTO, // Defined by failsafe configured values
|
||||
} failsafeChannelBehavior_e;
|
||||
|
||||
typedef struct {
|
||||
bool forceAngleMode;
|
||||
failsafeChannelBehavior_e channelBehavior[4];
|
||||
} failsafeProcedureLogic_t;
|
||||
|
||||
static const failsafeProcedureLogic_t failsafeProcedureLogic[] = {
|
||||
[FAILSAFE_PROCEDURE_AUTO_LANDING] = {
|
||||
.forceAngleMode = true,
|
||||
.channelBehavior = {
|
||||
FAILSAFE_CHANNEL_AUTO, // ROLL
|
||||
FAILSAFE_CHANNEL_AUTO, // PITCH
|
||||
FAILSAFE_CHANNEL_AUTO, // YAW
|
||||
FAILSAFE_CHANNEL_AUTO // THROTTLE
|
||||
}
|
||||
},
|
||||
|
||||
[FAILSAFE_PROCEDURE_DROP_IT] = {
|
||||
.forceAngleMode = true,
|
||||
.channelBehavior = {
|
||||
FAILFAFE_CHANNEL_NEUTRAL, // ROLL
|
||||
FAILFAFE_CHANNEL_NEUTRAL, // PITCH
|
||||
FAILFAFE_CHANNEL_NEUTRAL, // YAW
|
||||
FAILFAFE_CHANNEL_NEUTRAL // THROTTLE
|
||||
}
|
||||
},
|
||||
|
||||
[FAILSAFE_PROCEDURE_RTH] = {
|
||||
.forceAngleMode = true,
|
||||
.channelBehavior = {
|
||||
FAILFAFE_CHANNEL_NEUTRAL, // ROLL
|
||||
FAILFAFE_CHANNEL_NEUTRAL, // PITCH
|
||||
FAILFAFE_CHANNEL_NEUTRAL, // YAW
|
||||
FAILSAFE_CHANNEL_HOLD // THROTTLE
|
||||
}
|
||||
},
|
||||
|
||||
[FAILSAFE_PROCEDURE_NONE] = {
|
||||
.forceAngleMode = false,
|
||||
.channelBehavior = {
|
||||
FAILSAFE_CHANNEL_HOLD, // ROLL
|
||||
FAILSAFE_CHANNEL_HOLD, // PITCH
|
||||
FAILSAFE_CHANNEL_HOLD, // YAW
|
||||
FAILSAFE_CHANNEL_HOLD // THROTTLE
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/*
|
||||
* Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
|
||||
*/
|
||||
|
@ -90,7 +146,6 @@ void failsafeReset(void)
|
|||
failsafeState.receivingRxDataPeriodPreset = 0;
|
||||
failsafeState.phase = FAILSAFE_IDLE;
|
||||
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
|
||||
failsafeState.shouldApplyControlInput = FAILSAFE_CONTROL_NONE;
|
||||
}
|
||||
|
||||
void failsafeInit(uint16_t deadband3d_throttle)
|
||||
|
@ -124,56 +179,82 @@ bool failsafeIsActive(void)
|
|||
return failsafeState.active;
|
||||
}
|
||||
|
||||
bool failsafeRequiresAngleMode(void)
|
||||
{
|
||||
return failsafeState.active && failsafeProcedureLogic[failsafeConfig()->failsafe_procedure].forceAngleMode;
|
||||
}
|
||||
|
||||
void failsafeStartMonitoring(void)
|
||||
{
|
||||
failsafeState.monitoring = true;
|
||||
}
|
||||
|
||||
failsafeControlChannels_e failsafeShouldApplyControlInput(void)
|
||||
{
|
||||
return failsafeState.shouldApplyControlInput;
|
||||
}
|
||||
|
||||
static bool failsafeShouldHaveCausedLandingByNow(void)
|
||||
{
|
||||
return (millis() > failsafeState.landingShouldBeFinishedAt);
|
||||
}
|
||||
|
||||
static void failsafeActivate(failsafePhase_e newPhase, failsafeControlChannels_e applyControlInput)
|
||||
static void failsafeActivate(failsafePhase_e newPhase)
|
||||
{
|
||||
failsafeState.active = true;
|
||||
failsafeState.phase = newPhase;
|
||||
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
|
||||
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
|
||||
failsafeState.shouldApplyControlInput = applyControlInput;
|
||||
|
||||
failsafeState.events++;
|
||||
}
|
||||
|
||||
void failsafeApplyControlInput(void)
|
||||
void failsafeUpdateRcCommandValues(void)
|
||||
{
|
||||
if (failsafeState.shouldApplyControlInput & FAILSAFE_CONTROL_RPY) {
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_RTH) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
rcCommand[i] = 0;
|
||||
}
|
||||
}
|
||||
else {
|
||||
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
|
||||
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
|
||||
rcCommand[YAW] = pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->rates[FD_YAW]);
|
||||
}
|
||||
}
|
||||
else {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
rcCommand[i] = 0;
|
||||
}
|
||||
if (!failsafeState.active) {
|
||||
for (int idx = 0; idx < 4; idx++) {
|
||||
failsafeState.lastGoodRcCommand[idx] = rcCommand[idx];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (failsafeState.shouldApplyControlInput & FAILSAFE_CONTROL_THROTTLE) {
|
||||
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
void failsafeApplyControlInput(void)
|
||||
{
|
||||
// Prepare FAILSAFE_CHANNEL_AUTO values for rcCommand
|
||||
int16_t autoRcCommand[4];
|
||||
if (STATE(FIXED_WING)) {
|
||||
autoRcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
|
||||
autoRcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
|
||||
autoRcCommand[YAW] = pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->rates[FD_YAW]);
|
||||
autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
}
|
||||
else {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
autoRcCommand[i] = 0;
|
||||
}
|
||||
autoRcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
}
|
||||
|
||||
// Apply channel values
|
||||
for (int idx = 0; idx < 4; idx++) {
|
||||
switch (failsafeProcedureLogic[failsafeConfig()->failsafe_procedure].channelBehavior[idx]) {
|
||||
case FAILSAFE_CHANNEL_HOLD:
|
||||
rcCommand[idx] = failsafeState.lastGoodRcCommand[idx];
|
||||
break;
|
||||
|
||||
case FAILFAFE_CHANNEL_NEUTRAL:
|
||||
switch (idx) {
|
||||
case ROLL:
|
||||
case PITCH:
|
||||
case YAW:
|
||||
rcCommand[idx] = 0;
|
||||
break;
|
||||
|
||||
case THROTTLE:
|
||||
rcCommand[idx] = feature(FEATURE_3D) ? rxConfig()->midrc : motorConfig()->minthrottle;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case FAILSAFE_CHANNEL_AUTO:
|
||||
rcCommand[idx] = autoRcCommand[idx];
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -209,15 +290,32 @@ void failsafeOnValidDataFailed(void)
|
|||
}
|
||||
}
|
||||
|
||||
static bool failsafeCheckStickMotion(void)
|
||||
{
|
||||
if (failsafeConfig()->failsafe_stick_motion_threshold > 0) {
|
||||
uint32_t totalRcDelta = 0;
|
||||
|
||||
totalRcDelta += ABS(rcData[ROLL] - rxConfig()->midrc);
|
||||
totalRcDelta += ABS(rcData[PITCH] - rxConfig()->midrc);
|
||||
totalRcDelta += ABS(rcData[YAW] - rxConfig()->midrc);
|
||||
|
||||
return totalRcDelta >= failsafeConfig()->failsafe_stick_motion_threshold;
|
||||
}
|
||||
else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
void failsafeUpdateState(void)
|
||||
{
|
||||
if (!failsafeIsMonitoring()) {
|
||||
return;
|
||||
}
|
||||
|
||||
bool receivingRxData = failsafeIsReceivingRxData();
|
||||
bool armed = ARMING_FLAG(ARMED);
|
||||
bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
|
||||
const bool receivingRxData = failsafeIsReceivingRxData();
|
||||
const bool armed = ARMING_FLAG(ARMED);
|
||||
const bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
|
||||
const bool sticksAreMoving = failsafeCheckStickMotion();
|
||||
beeperMode_e beeperMode = BEEPER_SILENCE;
|
||||
|
||||
// Beep RX lost only if we are not seeing data and we have been armed earlier
|
||||
|
@ -240,14 +338,14 @@ void failsafeUpdateState(void)
|
|||
// Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
|
||||
if (failsafeSwitchIsOn && failsafeConfig()->failsafe_kill_switch) {
|
||||
// KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
|
||||
failsafeActivate(FAILSAFE_LANDED, FAILSAFE_CONTROL_ALL); // skip auto-landing procedure
|
||||
failsafeActivate(FAILSAFE_LANDED); // skip auto-landing procedure
|
||||
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData
|
||||
reprocessState = true;
|
||||
} else if (!receivingRxData) {
|
||||
if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
|
||||
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
|
||||
// Don't disarm at all if `failsafe_throttle_low_delay` is set to zero
|
||||
failsafeActivate(FAILSAFE_LANDED, FAILSAFE_CONTROL_ALL); // skip auto-landing procedure
|
||||
failsafeActivate(FAILSAFE_LANDED); // skip auto-landing procedure
|
||||
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
|
||||
} else {
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
|
||||
|
@ -273,26 +371,26 @@ void failsafeUpdateState(void)
|
|||
switch (failsafeConfig()->failsafe_procedure) {
|
||||
case FAILSAFE_PROCEDURE_AUTO_LANDING:
|
||||
// Stabilize, and set Throttle to specified level
|
||||
failsafeActivate(FAILSAFE_LANDING, FAILSAFE_CONTROL_ALL);
|
||||
failsafeActivate(FAILSAFE_LANDING);
|
||||
break;
|
||||
|
||||
case FAILSAFE_PROCEDURE_DROP_IT:
|
||||
// Drop the craft
|
||||
failsafeActivate(FAILSAFE_LANDED, FAILSAFE_CONTROL_ALL); // skip auto-landing procedure
|
||||
failsafeActivate(FAILSAFE_LANDED); // skip auto-landing procedure
|
||||
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
|
||||
break;
|
||||
|
||||
#if defined(NAV)
|
||||
case FAILSAFE_PROCEDURE_RTH:
|
||||
// Proceed to handling & monitoring RTH navigation
|
||||
failsafeActivate(FAILSAFE_RETURN_TO_HOME, FAILSAFE_CONTROL_RPY);
|
||||
failsafeActivate(FAILSAFE_RETURN_TO_HOME);
|
||||
activateForcedRTH();
|
||||
break;
|
||||
#endif
|
||||
case FAILSAFE_PROCEDURE_NONE:
|
||||
default:
|
||||
// Do nothing procedure
|
||||
failsafeActivate(FAILSAFE_RX_LOSS_IDLE, FAILSAFE_CONTROL_NONE);
|
||||
failsafeActivate(FAILSAFE_RX_LOSS_IDLE);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -301,7 +399,7 @@ void failsafeUpdateState(void)
|
|||
|
||||
/* A very simple do-nothing failsafe procedure. The only thing it will do is monitor the receiver state and switch out of FAILSAFE condition */
|
||||
case FAILSAFE_RX_LOSS_IDLE:
|
||||
if (receivingRxData) {
|
||||
if (receivingRxData && sticksAreMoving) {
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
|
||||
reprocessState = true;
|
||||
}
|
||||
|
@ -309,7 +407,7 @@ void failsafeUpdateState(void)
|
|||
|
||||
#if defined(NAV)
|
||||
case FAILSAFE_RETURN_TO_HOME:
|
||||
if (receivingRxData) {
|
||||
if (receivingRxData && sticksAreMoving) {
|
||||
abortForcedRTH();
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
|
||||
reprocessState = true;
|
||||
|
@ -340,7 +438,7 @@ void failsafeUpdateState(void)
|
|||
#endif
|
||||
|
||||
case FAILSAFE_LANDING:
|
||||
if (receivingRxData) {
|
||||
if (receivingRxData && sticksAreMoving) {
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
|
||||
reprocessState = true;
|
||||
}
|
||||
|
|
|
@ -41,6 +41,8 @@ typedef struct failsafeConfig_s {
|
|||
int16_t failsafe_fw_roll_angle; // Settings to be applies during "LAND" procedure on a fixed-wing
|
||||
int16_t failsafe_fw_pitch_angle;
|
||||
int16_t failsafe_fw_yaw_rate;
|
||||
|
||||
uint16_t failsafe_stick_motion_threshold;
|
||||
} failsafeConfig_t;
|
||||
|
||||
PG_DECLARE(failsafeConfig_t, failsafeConfig);
|
||||
|
@ -76,13 +78,6 @@ typedef enum {
|
|||
RTH_HAS_LANDED // RTH is active and has landed.
|
||||
} rthState_e;
|
||||
|
||||
typedef enum {
|
||||
FAILSAFE_CONTROL_NONE = 0,
|
||||
FAILSAFE_CONTROL_RPY = 1 << 0,
|
||||
FAILSAFE_CONTROL_THROTTLE = 1 << 1,
|
||||
FAILSAFE_CONTROL_ALL = (FAILSAFE_CONTROL_RPY | FAILSAFE_CONTROL_THROTTLE)
|
||||
} failsafeControlChannels_e;
|
||||
|
||||
typedef struct failsafeState_s {
|
||||
int16_t events;
|
||||
bool monitoring;
|
||||
|
@ -97,7 +92,7 @@ typedef struct failsafeState_s {
|
|||
timeMs_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData
|
||||
failsafePhase_e phase;
|
||||
failsafeRxLinkState_e rxLinkState;
|
||||
failsafeControlChannels_e shouldApplyControlInput;
|
||||
int16_t lastGoodRcCommand[4];
|
||||
} failsafeState_t;
|
||||
|
||||
void failsafeInit(uint16_t deadband3d_throttle);
|
||||
|
@ -114,7 +109,8 @@ void failsafeOnRxSuspend(uint32_t suspendPeriod);
|
|||
void failsafeOnRxResume(void);
|
||||
bool failsafeMayRequireNavigationMode(void);
|
||||
void failsafeApplyControlInput(void);
|
||||
failsafeControlChannels_e failsafeShouldApplyControlInput(void);
|
||||
bool failsafeRequiresAngleMode(void);
|
||||
void failsafeUpdateRcCommandValues(void);
|
||||
|
||||
void failsafeOnValidDataReceived(void);
|
||||
void failsafeOnValidDataFailed(void);
|
||||
|
|
|
@ -120,7 +120,8 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
|
|||
.rssi_channel = 0,
|
||||
.rssi_scale = RSSI_SCALE_DEFAULT,
|
||||
.rssi_ppm_invert = 0,
|
||||
.rcSmoothing = 1
|
||||
.rcSmoothing = 1,
|
||||
.rxNoSignalThrottleBehavior = RX_NOSIGNAL_THROTTLE_HOLD,
|
||||
);
|
||||
|
||||
void resetAllRxChannelRangeConfigurations(void)
|
||||
|
@ -417,11 +418,12 @@ static uint16_t getRxNosignalValue(uint8_t channel)
|
|||
return rxConfig()->midrc;
|
||||
|
||||
case THROTTLE:
|
||||
if (feature(FEATURE_3D))
|
||||
return rxConfig()->midrc;
|
||||
else
|
||||
//return rxConfig()->rx_min_usec;
|
||||
if (rxConfig()->rxNoSignalThrottleBehavior == RX_NOSIGNAL_THROTTLE_DROP) {
|
||||
return feature(FEATURE_3D) ? rxConfig()->midrc : rxConfig()->rx_min_usec;
|
||||
}
|
||||
else {
|
||||
return rcData[channel];
|
||||
}
|
||||
|
||||
default:
|
||||
return rcData[channel];
|
||||
|
|
|
@ -91,6 +91,9 @@ typedef struct rxChannelRangeConfig_s {
|
|||
} rxChannelRangeConfig_t;
|
||||
PG_DECLARE_ARRAY(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs);
|
||||
|
||||
#define RX_NOSIGNAL_THROTTLE_HOLD 0
|
||||
#define RX_NOSIGNAL_THROTTLE_DROP 1
|
||||
|
||||
typedef struct rxConfig_s {
|
||||
uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order
|
||||
uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first.
|
||||
|
@ -109,6 +112,7 @@ typedef struct rxConfig_s {
|
|||
uint16_t rx_min_usec;
|
||||
uint16_t rx_max_usec;
|
||||
uint8_t rcSmoothing; // Enable/Disable RC filtering
|
||||
uint8_t rxNoSignalThrottleBehavior;
|
||||
} rxConfig_t;
|
||||
|
||||
PG_DECLARE(rxConfig_t, rxConfig);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue