1
0
Fork 0
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:
Konstantin Sharlaimov 2017-02-01 00:12:30 +10:00 committed by GitHub
commit ae3b4fb8f8
6 changed files with 174 additions and 75 deletions

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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