Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 11:59:58 +03:00
Code Issues Wiki Activity

Improve GPS Rescue Pitch smoothing and disarming (#12343)

Browse source
This commit is contained in:
ctzsnooze 2023-03-08 16:10:25 +11:00 • committed by GitHub
parent f0e7b21264
commit e957f0dfa6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
16 changed files with 255 additions and 193 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/blackbox/blackbox.c
View file

@ -1532,12 +1532,14 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->initialAltitudeM)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_SPEED, "%d", gpsRescueConfig()->rescueGroundspeed)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_ANGLE_MAX, "%d", gpsRescueConfig()->angle)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d", gpsRescueConfig()->pitchCutoffHz)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCENT_DIST, "%d", gpsRescueConfig()->descentDistanceM)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DESCEND_RATE, "%d", gpsRescueConfig()->descendRate)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_LANDING_ALT, "%d", gpsRescueConfig()->targetLandingAltitudeM)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, "%d", gpsRescueConfig()->disarmThreshold)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, "%d", gpsRescueConfig()->throttleMin)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, "%d", gpsRescueConfig()->throttleMax)

4
src/main/cli/settings.c
View file

@ -1017,12 +1017,14 @@ const clivalue_t valueTable[] = {
{ PARAM_NAME_GPS_RESCUE_RETURN_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 2, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) },
{ PARAM_NAME_GPS_RESCUE_RETURN_SPEED, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) },
{ PARAM_NAME_GPS_RESCUE_PITCH_ANGLE_MAX, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) },
{ PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 80 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, maxRescueAngle) },
{ PARAM_NAME_GPS_RESCUE_ROLL_MIX, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rollMix) },
{ PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 255 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, pitchCutoffHz) },
{ PARAM_NAME_GPS_RESCUE_DESCENT_DIST, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) },
{ PARAM_NAME_GPS_RESCUE_DESCEND_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 25, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descendRate) },
{ PARAM_NAME_GPS_RESCUE_LANDING_ALT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) },
{ PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 250 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, disarmThreshold) },
{ PARAM_NAME_GPS_RESCUE_THROTTLE_MIN, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) },
{ PARAM_NAME_GPS_RESCUE_THROTTLE_MAX, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) },

10
src/main/cms/cms_menu_gps_rescue.c
View file

@ -63,6 +63,7 @@ static uint8_t gpsRescueConfig_throttleP, gpsRescueConfig_throttleI, gpsRescueCo
static uint8_t gpsRescueConfig_yawP;
static uint8_t gpsRescueConfig_velP, gpsRescueConfig_velI, gpsRescueConfig_velD;
static uint8_t gpsRescueConfig_pitchCutoffHz;
static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
{
@ -78,6 +79,7 @@ static const void *cms_menuGpsRescuePidOnEnter(displayPort_t *pDisp)
gpsRescueConfig_velI = gpsRescueConfig()->velI;
gpsRescueConfig_velD = gpsRescueConfig()->velD;
gpsRescueConfig_pitchCutoffHz = gpsRescueConfig()->pitchCutoffHz;
return NULL;
}
@ -96,6 +98,8 @@ static const void *cms_menuGpsRescuePidOnExit(displayPort_t *pDisp, const OSD_En
gpsRescueConfigMutable()->velI = gpsRescueConfig_velI;
gpsRescueConfigMutable()->velD = gpsRescueConfig_velD;
gpsRescueConfigMutable()->pitchCutoffHz = gpsRescueConfig_pitchCutoffHz;
return NULL;
}
@ -113,6 +117,8 @@ const OSD_Entry cms_menuGpsRescuePidEntries[] =
{ "VELOCITY I", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velI, 0, 255, 1 } },
{ "VELOCITY D", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_velD, 0, 255, 1 } },
{ "SMOOTHING", OME_UINT8 | REBOOT_REQUIRED, NULL, &(OSD_UINT8_t){ &gpsRescueConfig_pitchCutoffHz, 10, 255, 1 } },
{"BACK", OME_Back, NULL, NULL},
{NULL, OME_END, NULL, NULL}
};
@ -139,7 +145,7 @@ static const void *cmsx_menuGpsRescueOnEnter(displayPort_t *pDisp)
gpsRescueConfig_initialAltitudeM = gpsRescueConfig()->initialAltitudeM;
gpsRescueConfig_rescueGroundspeed = gpsRescueConfig()->rescueGroundspeed;
gpsRescueConfig_angle = gpsRescueConfig()->angle;
gpsRescueConfig_angle = gpsRescueConfig()->maxRescueAngle;
gpsRescueConfig_descentDistanceM = gpsRescueConfig()->descentDistanceM;
gpsRescueConfig_descendRate = gpsRescueConfig()->descendRate;
@ -167,7 +173,7 @@ static const void *cmsx_menuGpsRescueOnExit(displayPort_t *pDisp, const OSD_Entr
gpsRescueConfigMutable()->initialAltitudeM = gpsRescueConfig_initialAltitudeM;
gpsRescueConfigMutable()->rescueGroundspeed = gpsRescueConfig_rescueGroundspeed;
gpsRescueConfigMutable()->angle = gpsRescueConfig_angle;
gpsRescueConfigMutable()->maxRescueAngle = gpsRescueConfig_angle;
gpsRescueConfigMutable()->descentDistanceM = gpsRescueConfig_descentDistanceM;
gpsRescueConfigMutable()->descendRate = gpsRescueConfig_descendRate;

4
src/main/fc/parameter_names.h
View file

@ -143,12 +143,14 @@
#define PARAM_NAME_GPS_RESCUE_RETURN_ALT "gps_rescue_return_alt"
#define PARAM_NAME_GPS_RESCUE_RETURN_SPEED "gps_rescue_ground_speed"
#define PARAM_NAME_GPS_RESCUE_PITCH_ANGLE_MAX "gps_rescue_pitch_angle_max"
#define PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE "gps_rescue_max_angle"
#define PARAM_NAME_GPS_RESCUE_ROLL_MIX "gps_rescue_roll_mix"
#define PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF "gps_rescue_pitch_cutoff"
#define PARAM_NAME_GPS_RESCUE_DESCENT_DIST "gps_rescue_descent_dist"
#define PARAM_NAME_GPS_RESCUE_DESCEND_RATE "gps_rescue_descend_rate"
#define PARAM_NAME_GPS_RESCUE_LANDING_ALT "gps_rescue_landing_alt"
#define PARAM_NAME_GPS_RESCUE_DISARM_THRESHOLD "gps_rescue_disarm_threshold"
#define PARAM_NAME_GPS_RESCUE_THROTTLE_MIN "gps_rescue_throttle_min"
#define PARAM_NAME_GPS_RESCUE_THROTTLE_MAX "gps_rescue_throttle_max"

10
src/main/fc/rc_controls.c
View file

@ -177,10 +177,16 @@ void processRcStickPositions(void)
resetTryingToArm();
// Disarming via ARM BOX
resetArmingDisabled();
const bool switchFailsafe = (failsafeIsActive() && (IS_RC_MODE_ACTIVE(BOXFAILSAFE) || IS_RC_MODE_ACTIVE(BOXGPSRESCUE)));
if (ARMING_FLAG(ARMED) && (failsafeIsReceivingRxData() || switchFailsafe)) {
const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
if (ARMING_FLAG(ARMED) && (failsafeIsReceivingRxData() || boxFailsafeSwitchIsOn)) {
// in a true signal loss situation, allow disarm only once we regain validated RxData (failsafeIsReceivingRxData = true),
// to avoid potentially false disarm signals soon after link recover
// Note that BOXFAILSAFE will also drive failsafeIsReceivingRxData false (immediately at start or end)
// That's why we explicitly allow disarm here BOXFAILSAFE switch is active
// Note that BOXGPSRESCUE mode does not trigger failsafe - we can always disarm in that mode
rcDisarmTicks++;
if (rcDisarmTicks > 3) {
// require three duplicate disarm values in a row before we disarm
disarm(DISARM_REASON_SWITCH);
}
}

47
src/main/flight/failsafe.c
View file

@ -104,7 +104,7 @@ void failsafeReset(void)
failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod;
failsafeState.phase = FAILSAFE_IDLE;
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
failsafeState.failsafeSwitchWasOn = false;
failsafeState.boxFailsafeSwitchWasOn = false;
}
void failsafeInit(void)
@ -125,7 +125,7 @@ bool failsafeIsMonitoring(void)
return failsafeState.monitoring;
}
bool failsafeIsActive(void) // real or switch-induced stage 2 failsafe
bool failsafeIsActive(void) // real or BOXFAILSAFE induced stage 2 failsafe is currently active
{
return failsafeState.active;
}
@ -143,9 +143,9 @@ static bool failsafeShouldHaveCausedLandingByNow(void)
bool failsafeIsReceivingRxData(void)
{
return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
// False with failsafe switch or when no valid packets for 100ms or any flight channel invalid for 300ms,
// stays false until after recovery period expires
// Link down is the trigger for the various failsafe stage 2 outcomes.
// False with BOXFAILSAFE switch or when no valid packets for 100ms or any flight channel invalid for 300ms,
// becomes true immediately BOXFAILSAFE switch reverts, or after recovery period expires when valid packets are received
// rxLinkState RXLINK_DOWN (not up) is the trigger for the various failsafe stage 2 outcomes.
}
void failsafeOnRxSuspend(uint32_t usSuspendPeriod)
@ -160,7 +160,10 @@ void failsafeOnRxResume(void)
}
void failsafeOnValidDataReceived(void)
// runs when packets are received for more than the signal validation period (100ms)
// enters stage 2
// runs, after prior a signal loss, immediately when packets are received or the BOXFAILSAFE switch is reverted
// rxLinkState will go RXLINK_UP immediately if BOXFAILSAFE goes back ON since receivingRxDataPeriodPreset is set to zero in that case
// otherwise RXLINK_UP is delayed for the recovery period (failsafe_recovery_delay, default 1s, 0-20, min 0.2s)
{
unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
// clear RXLOSS in OSD immediately we get a good packet, and un-set its arming block
@ -188,14 +191,16 @@ void failsafeOnValidDataReceived(void)
}
void failsafeOnValidDataFailed(void)
// runs when packets are lost for more than the signal validation period (100ms)
// run from rc.c when packets are lost for more than the signal validation period (100ms), or immediately BOXFAILSAFE switch is active
// after the stage 1 delay has expired, sets the rxLinkState to RXLINK_DOWN, ie not up, causing failsafeIsReceivingRxData to become false
// if failsafe is configured to go direct to stage 2, this is emulated immediately in failsafeUpdateState()
{
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
// set RXLOSS in OSD and block arming after 100ms of signal loss (is restored in rx.c immediately signal returns)
failsafeState.validRxDataFailedAt = millis();
if ((cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod)) {
// sets rxLinkState = DOWN to initiate stage 2 failsafe, if no validated signal for the stage 1 period
// sets rxLinkState = DOWN to initiate stage 2 failsafe
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
// show RXLOSS and block arming
}
@ -225,19 +230,18 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
}
bool receivingRxData = failsafeIsReceivingRxData();
// returns state of FAILSAFE_RXLINK_UP
// FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived, after the various Stage 1 and recovery delays
// failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour
// returns state of FAILSAFE_RXLINK_UP, which
// goes false after the stage 1 delay, whether from signal loss or BOXFAILSAFE switch activation
// goes true immediately BOXFAILSAFE switch is reverted, or after recovery delay once signal recovers
// essentially means 'should be in failsafe stage 2'
DEBUG_SET(DEBUG_FAILSAFE, 2, receivingRxData); // from Rx alone, not considering switch
bool armed = ARMING_FLAG(ARMED);
bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
beeperMode_e beeperMode = BEEPER_SILENCE;
if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) {
// Aux switch set to failsafe stage2 emulates immediate loss of signal without waiting
failsafeOnValidDataFailed();
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE) && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) {
// Force immediate stage 2 responses if mode is failsafe stage2 to emulate immediate loss of signal without waiting
receivingRxData = false;
}
@ -253,13 +257,14 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
switch (failsafeState.phase) {
case FAILSAFE_IDLE:
failsafeState.failsafeSwitchWasOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
failsafeState.boxFailsafeSwitchWasOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
// store and use the switch state as it was at the start of the failsafe
if (armed) {
// Track throttle command below minimum time
if (calculateThrottleStatus() != THROTTLE_LOW) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
if (failsafeState.failsafeSwitchWasOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
if (failsafeState.boxFailsafeSwitchWasOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
// Failsafe switch is configured as KILL switch and is switched ON
failsafeState.active = true;
failsafeState.events++;
@ -291,7 +296,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
}
} else {
// When NOT armed, enable failsafe mode to show warnings in OSD
if (failsafeState.failsafeSwitchWasOn) {
if (failsafeState.boxFailsafeSwitchWasOn) {
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
} else {
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
@ -327,7 +332,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
break;
#endif
}
if (failsafeState.failsafeSwitchWasOn) {
if (failsafeState.boxFailsafeSwitchWasOn) {
failsafeState.receivingRxDataPeriodPreset = 0;
// recover immediately if failsafe was triggered by a switch
} else {
@ -359,7 +364,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
#ifdef USE_GPS_RESCUE
case FAILSAFE_GPS_RESCUE:
if (receivingRxData) {
if (areSticksActive(failsafeConfig()->failsafe_stick_threshold) || failsafeState.failsafeSwitchWasOn) {
if (areSticksActive(failsafeConfig()->failsafe_stick_threshold) || failsafeState.boxFailsafeSwitchWasOn) {
// exits the rescue immediately if failsafe was initiated by switch, otherwise
// requires stick input to exit the rescue after a true Rx loss failsafe
// NB this test requires stick inputs to be received during GPS Rescue see PR #7936 for rationale
@ -418,7 +423,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
break;
}
DEBUG_SET(DEBUG_FAILSAFE, 0, failsafeState.failsafeSwitchWasOn);
DEBUG_SET(DEBUG_FAILSAFE, 0, failsafeState.boxFailsafeSwitchWasOn);
DEBUG_SET(DEBUG_FAILSAFE, 3, failsafeState.phase);
} while (reprocessState);

2
src/main/flight/failsafe.h
View file

@ -90,7 +90,7 @@ typedef struct failsafeState_s {
uint32_t receivingRxDataPeriodPreset; // preset for the required period of valid rxData
failsafePhase_e phase;
failsafeRxLinkState_e rxLinkState;
bool failsafeSwitchWasOn;
bool boxFailsafeSwitchWasOn;
} failsafeState_t;
void failsafeInit(void);

231
src/main/flight/gps_rescue.c
View file

@ -89,6 +89,11 @@ typedef struct {
float descentRateModifier;
float yawAttenuator;
float disarmThreshold;
float velocityITermAccumulator;
float velocityPidCutoff;
float velocityPidCutoffModifier;
float proximityToLandingArea;
float velocityItermRelax;
} rescueIntent_s;
typedef struct {
@ -102,6 +107,7 @@ typedef struct {
float errorAngle;
float gpsDataIntervalSeconds;
float altitudeDataIntervalSeconds;
float gpsRescueTaskIntervalSeconds;
float velocityToHomeCmS;
float alitutudeStepCm;
float maxPitchStep;
@ -118,10 +124,7 @@ typedef struct {
#define GPS_RESCUE_MAX_YAW_RATE 180 // deg/sec max yaw rate
#define GPS_RESCUE_MIN_DESCENT_DIST_M 5 // minimum descent distance
#define GPS_RESCUE_MAX_ITERM_VELOCITY 1000 // max iterm value for velocity
#define GPS_RESCUE_MAX_ITERM_THROTTLE 200 // max iterm value for throttle
#define GPS_RESCUE_MAX_PITCH_RATE 3000 // max change in pitch per second in degrees * 100
#define GPS_RESCUE_DISARM_THRESHOLD 2.0f // disarm threshold in G's
#define GPS_RESCUE_MAX_THROTTLE_ITERM 200 // max iterm value for throttle in degrees * 100
static float rescueThrottle;
static float rescueYaw;
@ -129,27 +132,29 @@ float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
bool magForceDisable = false;
static bool newGPSData = false;
static pt2Filter_t throttleDLpf;
static pt2Filter_t velocityDLpf;
static pt3Filter_t pitchLpf;
static pt1Filter_t velocityDLpf;
static pt3Filter_t velocityUpsampleLpf;
rescueState_s rescueState;
void gpsRescueInit(void)
{
const float sampleTimeS = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ);
float cutoffHz, gain;
rescueState.sensor.gpsRescueTaskIntervalSeconds = HZ_TO_INTERVAL(TASK_GPS_RESCUE_RATE_HZ);
float cutoffHz, gain;
cutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
gain = pt2FilterGain(cutoffHz, sampleTimeS);
gain = pt2FilterGain(cutoffHz, rescueState.sensor.gpsRescueTaskIntervalSeconds);
pt2FilterInit(&throttleDLpf, gain);
cutoffHz = 0.8f;
gain = pt2FilterGain(cutoffHz, 1.0f);
pt2FilterInit(&velocityDLpf, gain);
cutoffHz = gpsRescueConfig()->pitchCutoffHz / 100.0f;
rescueState.intent.velocityPidCutoff = cutoffHz;
rescueState.intent.velocityPidCutoffModifier = 1.0f;
gain = pt1FilterGain(cutoffHz, 1.0f);
pt1FilterInit(&velocityDLpf, gain);
cutoffHz = 4.0f;
gain = pt3FilterGain(cutoffHz, sampleTimeS);
pt3FilterInit(&pitchLpf, gain);
cutoffHz *= 4.0f;
gain = pt3FilterGain(cutoffHz, rescueState.sensor.gpsRescueTaskIntervalSeconds);
pt3FilterInit(&velocityUpsampleLpf, gain);
}
/*
@ -210,7 +215,6 @@ static void rescueAttainPosition(void)
// runs at 100hz, but only updates RPYT settings when new GPS Data arrives and when not in idle phase.
static float previousVelocityError = 0.0f;
static float velocityI = 0.0f;
static float previousPitchAdjustment = 0.0f;
static float throttleI = 0.0f;
static float previousAltitudeError = 0.0f;
static int16_t throttleAdjustment = 0;
@ -226,11 +230,10 @@ static void rescueAttainPosition(void)
// Initialize internal variables each time GPS Rescue is started
previousVelocityError = 0.0f;
velocityI = 0.0f;
previousPitchAdjustment = 0.0f;
throttleI = 0.0f;
previousAltitudeError = 0.0f;
throttleAdjustment = 0;
rescueState.intent.disarmThreshold = GPS_RESCUE_DISARM_THRESHOLD;
rescueState.intent.disarmThreshold = gpsRescueConfig()->disarmThreshold / 10.0f;
return;
case RESCUE_DO_NOTHING:
// 20s of slow descent for switch induced sanity failures to allow time to recover
@ -255,7 +258,7 @@ static void rescueAttainPosition(void)
// I component
throttleI += 0.1f * gpsRescueConfig()->throttleI * altitudeError * rescueState.sensor.altitudeDataIntervalSeconds;
throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE, 1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE);
throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_THROTTLE_ITERM, 1.0f * GPS_RESCUE_MAX_THROTTLE_ITERM);
// up to 20% increase in throttle from I alone
// D component is error based, so includes positive boost when climbing and negative boost on descent
@ -266,8 +269,6 @@ static void rescueAttainPosition(void)
float throttleD = pt2FilterApply(&throttleDLpf, verticalSpeed);
rescueState.intent.disarmThreshold = GPS_RESCUE_DISARM_THRESHOLD - throttleD / 15.0f; // make disarm more likely if throttle D is high
throttleD = gpsRescueConfig()->throttleD * throttleD;
// acceleration component not currently implemented - was needed previously due to GPS lag, maybe not needed now.
@ -294,12 +295,13 @@ static void rescueAttainPosition(void)
// if the course over ground, due to wind or pre-exiting movement, is different from the attitude of the quad, the GPS correction will be less accurate
// the craft should not return much less than 5m/s during the rescue or the GPS corrections may be inaccurate.
// the faster the return speed, the more accurate the IMU will be, but the consequences of IMU error at the start are greater
// A compass (magnetometer) is vital for accurate GPS rescue at slow speeds, but must be calibrated and validated.
// A compass (magnetometer) is vital for accurate GPS rescue at slow speeds, but must be calibrated and validated
// WARNING: Some GPS units give false Home values! Always check the arrow points to home on leaving home.
rescueYaw = rescueState.sensor.errorAngle * gpsRescueConfig()->yawP * rescueState.intent.yawAttenuator * 0.1f;
rescueYaw = constrainf(rescueYaw, -GPS_RESCUE_MAX_YAW_RATE, GPS_RESCUE_MAX_YAW_RATE);
// rescueYaw is the yaw rate in deg/s to correct the heading error
// *** mix in some roll. very important for heading tracking, since a yaw rate means the quad has drifted sideways
const float rollMixAttenuator = constrainf(1.0f - fabsf(rescueYaw) * 0.01f, 0.0f, 1.0f);
// less roll at higher yaw rates, no roll at 100 deg/s of yaw
const float rollAdjustment = -rescueYaw * gpsRescueConfig()->rollMix * rollMixAttenuator;
@ -307,12 +309,12 @@ static void rescueAttainPosition(void)
// when gpsRescueConfig()->rollMix is zero, there is no roll adjustment
// rollAdjustment is degrees * 100
// note that the roll element has the opposite sign to the yaw element *before* GET_DIRECTION
const float rollLimit = 100.0f * rescueState.intent.rollAngleLimitDeg;
gpsRescueAngle[AI_ROLL] = constrainf(rollAdjustment, -rollLimit, rollLimit);
// gpsRescueAngle is added to the normal roll Angle Mode corrections in pid.c
rescueYaw *= GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
// rescueYaw is the yaw rate in deg/s to correct the heading error
/**
Pitch / velocity controller
@ -322,51 +324,53 @@ static void rescueAttainPosition(void)
const float sampleIntervalNormaliseFactor = rescueState.sensor.gpsDataIntervalSeconds * 10.0f;
const float velocityError = (rescueState.intent.targetVelocityCmS - rescueState.sensor.velocityToHomeCmS);
const float velocityError = rescueState.intent.targetVelocityCmS - rescueState.sensor.velocityToHomeCmS;
// velocityError is in cm per second, positive means too slow.
// NB positive pitch setpoint means nose down.
// target velocity can be very negative leading to large error before the start, with overshoot
// P component
const float velocityP = velocityError * gpsRescueConfig()->velP;
// I component
velocityI += 0.01f * gpsRescueConfig()->velI * velocityError * sampleIntervalNormaliseFactor;
// increase amount added when GPS sample rate is slower
velocityI = constrainf(velocityI, -1.0f * GPS_RESCUE_MAX_ITERM_VELOCITY, 1.0f * GPS_RESCUE_MAX_ITERM_VELOCITY);
// I component alone cannot exceed a pitch angle of 10%
velocityI += 0.01f * gpsRescueConfig()->velI * velocityError * sampleIntervalNormaliseFactor * rescueState.intent.velocityItermRelax;
// velocityItermRelax is a time-based factor, 0->1 with time constant of 1s from when we start to fly home
// avoids excess iTerm during the initial acceleration phase.
velocityI *= rescueState.intent.proximityToLandingArea;
// reduce iTerm sharply when velocity decreases in landing phase, to minimise overshoot during deceleration
const float pitchAngleLimit = rescueState.intent.pitchAngleLimitDeg * 100.0f;
const float velocityPILimit = 0.5f * pitchAngleLimit;
velocityI = constrainf(velocityI, -velocityPILimit, velocityPILimit);
// I component alone cannot exceed half the max pitch angle
// D component
float velocityD = ((velocityError - previousVelocityError) / sampleIntervalNormaliseFactor);
previousVelocityError = velocityError;
const float gain = pt2FilterGain(0.8f, HZ_TO_INTERVAL(gpsGetSampleRateHz()));
pt2FilterUpdateCutoff(&velocityDLpf, gain);
velocityD = pt2FilterApply(&velocityDLpf, velocityD);
velocityD *= gpsRescueConfig()->velD;
const float velocityIAttenuator = rescueState.intent.targetVelocityCmS / gpsRescueConfig()->rescueGroundspeed;
// reduces iTerm as target velocity decreases, to minimise overshoot during deceleration to landing phase
// smooth the D steps
const float cutoffHz = rescueState.intent.velocityPidCutoff * rescueState.intent.velocityPidCutoffModifier;
// note that this cutoff is increased up to 2x as we get closer to landing point in descend()
const float gain = pt1FilterGain(cutoffHz, rescueState.sensor.gpsDataIntervalSeconds);
pt1FilterUpdateCutoff(&velocityDLpf, gain);
velocityD = pt1FilterApply(&velocityDLpf, velocityD);
pitchAdjustment = velocityP + velocityD;
if (rescueState.phase == RESCUE_FLY_HOME) {
pitchAdjustment *= 0.7f; // attenuate pitch PIDs during main fly home phase, tighten up in descent.
}
pitchAdjustment += velocityI * velocityIAttenuator;
pitchAdjustment = velocityP + velocityI + velocityD;
pitchAdjustment = constrainf(pitchAdjustment, -pitchAngleLimit, pitchAngleLimit);
// limit to maximum allowed angle
const float movingAvgPitchAdjustment = 0.5f * (previousPitchAdjustment + pitchAdjustment);
// moving average seems to work best here, a lot of sequential up and down in velocity data
previousPitchAdjustment = pitchAdjustment;
pitchAdjustment = movingAvgPitchAdjustment;
// pitchAdjustment is the absolute Pitch angle adjustment value in degrees * 100
// it gets added to the normal level mode Pitch adjustments in pid.c
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 0, lrintf(velocityP));
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 1, lrintf(velocityD));
}
const float pitchAdjustmentFiltered = pt3FilterApply(&pitchLpf, pitchAdjustment);
// upsampling and smoothing of pitch angle steps
float pitchAdjustmentFiltered = pt3FilterApply(&velocityUpsampleLpf, pitchAdjustment);
const float pitchAngleLimit = rescueState.intent.pitchAngleLimitDeg * 100.0f;
gpsRescueAngle[AI_PITCH] = constrainf(pitchAdjustmentFiltered, -pitchAngleLimit, pitchAngleLimit);
gpsRescueAngle[AI_PITCH] = pitchAdjustmentFiltered;
// this angle gets added to the normal pitch Angle Mode control values in pid.c - will be seen in pitch setpoint
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 3, lrintf(rescueState.intent.targetVelocityCmS));
@ -447,7 +451,7 @@ static void performSanityChecks(void)
// if the quad is stuck, or if GPS data packets stop, there will be no change in distance to home
// we can't use rescueState.sensor.currentVelocity because it will be held at the last good value if GPS data updates stop
if (rescueState.phase == RESCUE_FLY_HOME) {
const float velocityToHomeCmS = previousDistanceToHomeCm- rescueState.sensor.distanceToHomeCm; // cm/s
const float velocityToHomeCmS = previousDistanceToHomeCm - rescueState.sensor.distanceToHomeCm; // cm/s
previousDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
rescueState.intent.secondsFailing += (velocityToHomeCmS < 0.5f * rescueState.intent.targetVelocityCmS) ? 1 : -1;
rescueState.intent.secondsFailing = constrain(rescueState.intent.secondsFailing, 0, 15);
@ -540,7 +544,8 @@ static void sensorUpdate(void)
if (rescueState.phase == RESCUE_LANDING) {
// do this at sensor update rate, not the much slower GPS rate, for quick disarm
rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z]) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
rescueState.sensor.accMagnitude = (float) sqrtf(sq(acc.accADC[Z] - acc.dev.acc_1G) + sq(acc.accADC[X]) + sq(acc.accADC[Y])) * acc.dev.acc_1G_rec;
// Note: subtracting 1G from Z assumes the quad is 'flat' with respect to the horizon. A true non-gravity acceleration value, regardless of attitude, may be better.
}
rescueState.sensor.directionToHome = GPS_directionToHome;
@ -562,21 +567,15 @@ static void sensorUpdate(void)
rescueState.sensor.distanceToHomeM = rescueState.sensor.distanceToHomeCm / 100.0f;
rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s
static timeUs_t previousGPSDataTimeUs = 0;
const timeDelta_t gpsDataIntervalUs = cmpTimeUs(currentTimeUs, previousGPSDataTimeUs);
rescueState.sensor.gpsDataIntervalSeconds = constrainf(gpsDataIntervalUs * 0.000001f, 0.01f, 1.0f);
rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds();
// Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
previousGPSDataTimeUs = currentTimeUs;
rescueState.sensor.velocityToHomeCmS = (prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds;
rescueState.sensor.velocityToHomeCmS = ((prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds);
// positive = towards home. First value is useless since prevDistanceToHomeCm was zero.
prevDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
rescueState.sensor.maxPitchStep = rescueState.sensor.gpsDataIntervalSeconds * GPS_RESCUE_MAX_PITCH_RATE;
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 2, lrintf(rescueState.sensor.velocityToHomeCmS));
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 0, lrintf(rescueState.sensor.velocityToHomeCmS));
}
// This function flashes "RESCUE N/A" in the OSD if:
@ -643,15 +642,17 @@ void descend(void)
if (newGPSData) {
const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - (rescueState.intent.targetLandingAltitudeCm / 200.0f);
// considers home to be a circle half landing height around home to avoid overshooting home point
const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToLandingArea;
rescueState.intent.proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
rescueState.intent.velocityPidCutoffModifier = 2.5f - rescueState.intent.proximityToLandingArea;
// 1.5 when starting descent, 2.5 when almost landed; multiplier for velocity step cutoff filter
rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * rescueState.intent.proximityToLandingArea;
// reduce target velocity as we get closer to home. Zero within 2m of home, reducing risk of overshooting.
// if quad drifts further than 2m away from home, should by then have rotated towards home, so pitch is allowed
rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->angle * proximityToLandingArea;
rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->maxRescueAngle * rescueState.intent.proximityToLandingArea;
// reduce roll capability when closer to home, none within final 2m
}
// adjust altitude step for interval between altitude readings
// configure altitude step for descent, considering interval between altitude readings
rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
// descend more slowly if return altitude is less than 20m
@ -665,15 +666,8 @@ void descend(void)
// increase descent rate to max of 3x default above 50m, 2x above 25m, 1.2 at 5m, default by ground level.
}
void altitudeAchieved(void)
{
rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
rescueState.intent.altitudeStep = 0;
rescueState.phase = RESCUE_ROTATE;
}
void gpsRescueUpdate(void)
// this runs a lot faster than the GPS Data update rate, and runs whether or not rescue is active
// runs at gpsRescueTaskIntervalSeconds, and runs whether or not rescue is active
{
if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
rescueStop(); // sets phase to RESCUE_IDLE; does nothing else. RESCUE_IDLE tasks still run.
@ -687,7 +681,8 @@ void gpsRescueUpdate(void)
sensorUpdate(); // always get latest GPS and Altitude data, update ascend and descend rates
bool startedLow = true;
static bool initialAltitudeLow = true;
static bool initialVelocityLow = true;
rescueState.isAvailable = checkGPSRescueIsAvailable();
switch (rescueState.phase) {
@ -700,32 +695,41 @@ void gpsRescueUpdate(void)
// target altitude is always set to current altitude.
case RESCUE_INITIALIZE:
// Things that should abort the start of a Rescue
// Things that should be done at the start of a Rescue
rescueState.intent.targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM;
if (!STATE(GPS_FIX_HOME)) {
// we didn't get a home point on arming
rescueState.failure = RESCUE_NO_HOME_POINT;
// will result in a disarm via the sanity check system, with delay if switch induced
// alternative is to prevent the rescue by returning to IDLE, but this could cause flyaways
} else if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) {
// Attempt to initiate inside minimum activation distance -> landing mode
rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
rescueState.intent.targetVelocityCmS = 0; // zero forward velocity
rescueState.intent.pitchAngleLimitDeg = 0; // flat on pitch
rescueState.intent.rollAngleLimitDeg = 0.0f; // flat on roll also
rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueState.intent.altitudeStep;
rescueState.phase = RESCUE_LANDING;
// start landing from current altitude
if (rescueState.sensor.distanceToHomeM < 5.0f && rescueState.sensor.currentAltitudeCm < rescueState.intent.targetLandingAltitudeCm) {
// attempted initiation within 5m of home, and 'on the ground' -> instant disarm, for safety reasons
rescueState.phase = RESCUE_ABORT;
} else {
// Otherwise, attempted initiation inside minimum activation distance, at any height -> landing mode
rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
rescueState.intent.targetVelocityCmS = 0; // zero forward velocity
rescueState.intent.pitchAngleLimitDeg = 0; // flat on pitch
rescueState.intent.rollAngleLimitDeg = 0.0f; // flat on roll also
rescueState.intent.proximityToLandingArea = 0.0f; // force velocity iTerm to zero
rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueState.intent.altitudeStep;
rescueState.phase = RESCUE_LANDING;
// start landing from current altitude
}
} else {
rescueState.phase = RESCUE_ATTAIN_ALT;
rescueState.intent.secondsFailing = 0; // reset the sanity check timer for the climb
rescueState.intent.targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM;
startedLow = (rescueState.sensor.currentAltitudeCm <= rescueState.intent.returnAltitudeCm);
initialAltitudeLow = (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm);
rescueState.intent.yawAttenuator = 0.0f;
rescueState.intent.targetVelocityCmS = 0.0f; // zero forward velocity while climbing
rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS;
rescueState.intent.pitchAngleLimitDeg = 0.0f; // no pitch
rescueState.intent.rollAngleLimitDeg = 0.0f; // no roll until flying home
rescueState.intent.altitudeStep = 0.0f;
rescueState.intent.descentRateModifier = 0.0f;
rescueState.intent.velocityPidCutoffModifier = 1.0f; // normal cutoff until descending when increases 150->250% during descent
rescueState.intent.proximityToLandingArea = 0.0f; // force velocity iTerm to zero
rescueState.intent.velocityItermRelax = 0.0f; // and don't accumulate any
}
break;
@ -733,47 +737,62 @@ void gpsRescueUpdate(void)
// gradually increment the target altitude until the craft reaches target altitude
// note that this can mean the target altitude may increase above returnAltitude if the craft lags target
// sanity check will abort if altitude gain is blocked for a cumulative period
if (startedLow) {
if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) {
rescueState.intent.altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->ascendRate;
} else if (rescueState.sensor.currentAltitudeCm > rescueState.intent.returnAltitudeCm) {
altitudeAchieved();
}
rescueState.intent.altitudeStep = ((initialAltitudeLow) ? gpsRescueConfig()->ascendRate : -1.0f * gpsRescueConfig()->descendRate) * rescueState.sensor.gpsRescueTaskIntervalSeconds;
const bool currentAltitudeLow = rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm;
if (initialAltitudeLow == currentAltitudeLow) {
// we started low, and still are low; also true if we started high, and still are too high
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
} else {
if (rescueState.intent.targetAltitudeCm > rescueState.intent.returnAltitudeCm) {
rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
} else if (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm) {
altitudeAchieved();
}
// target altitude achieved - move on to ROTATE phase, returning at target altitude
rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
rescueState.intent.altitudeStep = 0.0f;
rescueState.phase = RESCUE_ROTATE;
}
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS;
// gives velocity P and I no error that otherwise would be present due to velocity drift at the start of the rescue
break;
case RESCUE_ROTATE:
if (rescueState.intent.yawAttenuator < 1.0f) { // gradually acquire yaw authority
rescueState.intent.yawAttenuator += 0.01f;
if (rescueState.intent.yawAttenuator < 1.0f) { // acquire yaw authority over one second
rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
}
if (rescueState.sensor.absErrorAngle < 30.0f) {
rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->angle; // allow pitch
rescueState.intent.pitchAngleLimitDeg = gpsRescueConfig()->maxRescueAngle; // allow pitch
rescueState.phase = RESCUE_FLY_HOME; // enter fly home phase
rescueState.intent.secondsFailing = 0; // reset sanity timer for flight home
rescueState.intent.proximityToLandingArea = 1.0f; // velocity iTerm activated, initialise proximity for descent phase at 1.0
}
initialVelocityLow = rescueState.sensor.velocityToHomeCmS < gpsRescueConfig()->rescueGroundspeed; // used to set direction of velocity target change
rescueState.intent.targetVelocityCmS = rescueState.sensor.velocityToHomeCmS;
break;
case RESCUE_FLY_HOME:
if (rescueState.intent.yawAttenuator < 1.0f) { // be sure to accumulate full yaw authority
rescueState.intent.yawAttenuator += 0.01f;
rescueState.intent.yawAttenuator += rescueState.sensor.gpsRescueTaskIntervalSeconds;
}
// steadily increase target velocity target until full return velocity is acquired
if (rescueState.intent.targetVelocityCmS < gpsRescueConfig()->rescueGroundspeed) {
rescueState.intent.targetVelocityCmS += 0.01f * gpsRescueConfig()->rescueGroundspeed;
}
// acquire full roll authority slowly when pointing to home
if (rescueState.sensor.absErrorAngle < 10.0f && rescueState.intent.rollAngleLimitDeg < gpsRescueConfig()->angle) {
// roll is primarily intended to deal with wind drift causing small yaw errors during return
rescueState.intent.rollAngleLimitDeg += 0.1f;
// velocity PIDs are now active
// update target velocity gradually, aiming for rescueGroundspeed with a time constant of 1.0s
const float targetVelocityError = gpsRescueConfig()->rescueGroundspeed - rescueState.intent.targetVelocityCmS;
const float velocityTargetStep = rescueState.sensor.gpsRescueTaskIntervalSeconds * targetVelocityError;
// velocityTargetStep is positive when starting low, negative when starting high
const bool targetVelocityIsLow = rescueState.intent.targetVelocityCmS < gpsRescueConfig()->rescueGroundspeed;
if (initialVelocityLow == targetVelocityIsLow) {
// also true if started faster than target velocity and target is still high
rescueState.intent.targetVelocityCmS += velocityTargetStep;
}
rescueState.intent.velocityItermRelax += 0.5f * rescueState.sensor.gpsRescueTaskIntervalSeconds * (1.0f - rescueState.intent.velocityItermRelax);
// slowly introduce velocity iTerm accumulation at start, goes 0 ->1 with time constant 2.0s
// there is always a lot of lag at the start
rescueState.intent.velocityPidCutoffModifier = 2.0f - rescueState.intent.velocityItermRelax;
// higher velocity cutoff for initial few seconds to improve accuracy; can be smoother later
rescueState.intent.rollAngleLimitDeg = 0.5f * rescueState.intent.velocityItermRelax * gpsRescueConfig()->maxRescueAngle;
// gradually gain roll capability to max of half of max pitch angle
if (newGPSData) {
if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) {
rescueState.phase = RESCUE_DESCENT;
@ -795,6 +814,7 @@ void gpsRescueUpdate(void)
case RESCUE_LANDING:
// Reduce altitude target steadily until impact, then disarm.
// control yaw angle and throttle and pitch, attenuate velocity, roll and pitch iTerm
// increase velocity smoothing cutoff as we get closer to ground
descend();
disarmOnImpact();
break;
@ -806,6 +826,7 @@ void gpsRescueUpdate(void)
case RESCUE_ABORT:
setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
disarm(DISARM_REASON_FAILSAFE);
rescueState.intent.secondsFailing = 0; // reset sanity timers so we can re-arm
rescueStop();
break;

47
src/main/io/gps.c
View file

@ -82,7 +82,6 @@ uint16_t GPS_distanceToHome; // distance to home point in meters
uint32_t GPS_distanceToHomeCm;
int16_t GPS_directionToHome; // direction to home or hol point in degrees * 10
uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters
int16_t GPS_verticalSpeedInCmS; // vertical speed in cm/s
int16_t nav_takeoff_bearing;
#define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 0.54 km/h 0.335 mph
@ -107,7 +106,7 @@ uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N];
#define UBLOX_ACK_TIMEOUT_MAX_COUNT (25)
static serialPort_t *gpsPort;
static float gpsSampleRateHz;
static float gpsDataIntervalSeconds;
typedef struct gpsInitData_s {
uint8_t index;
@ -327,8 +326,7 @@ static void gpsSetState(gpsState_e state)
void gpsInit(void)
{
gpsSampleRateHz = 0.0f;
gpsDataIntervalSeconds = 0.1f;
gpsData.baudrateIndex = 0;
gpsData.errors = 0;
gpsData.timeouts = 0;
@ -1843,19 +1841,15 @@ void GPS_calc_longitude_scaling(int32_t lat)
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate the distance flown and vertical speed from gps position data
// Calculate the distance flown from gps position data
//
static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
static void GPS_calculateDistanceFlown(bool initialize)
{
static int32_t lastCoord[2] = { 0, 0 };
static int32_t lastAlt;
static int32_t lastMillis;
int currentMillis = millis();
if (initialize) {
GPS_distanceFlownInCm = 0;
GPS_verticalSpeedInCmS = 0;
} else {
if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) {
uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed;
@ -1870,13 +1864,10 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
GPS_distanceFlownInCm += dist;
}
}
GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
}
lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon;
lastCoord[GPS_LATITUDE] = gpsSol.llh.lat;
lastAlt = gpsSol.llh.altCm;
lastMillis = currentMillis;
}
void GPS_reset_home_position(void)
@ -1895,7 +1886,7 @@ void GPS_reset_home_position(void)
// PS: to test for gyro cal, check for !ARMED, since we cannot be here while disarmed other than via gyro cal
}
}
GPS_calculateDistanceFlownVerticalSpeed(true); // Initialize
GPS_calculateDistanceFlown(true); // Initialize
}
////////////////////////////////////////////////////////////////////////////////////
@ -1920,8 +1911,8 @@ void GPS_calculateDistanceAndDirectionToHome(void)
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir);
GPS_distanceToHome = dist / 100; // m/s
GPS_distanceToHomeCm = dist; // cm/sec
GPS_distanceToHome = dist / 100; // m
GPS_distanceToHomeCm = dist; // cm
GPS_directionToHome = dir / 10; // degrees * 10 or decidegrees
} else {
// If we don't have home set, do not display anything
@ -1933,11 +1924,21 @@ void GPS_calculateDistanceAndDirectionToHome(void)
void onGpsNewData(void)
{
static timeUs_t timeUs, lastTimeUs = 0;
static timeUs_t lastTimeUs = 0;
const timeUs_t timeUs = micros();
// calculate GPS solution interval
// !!! TOO MUCH JITTER TO BE USEFUL - need an exact time !!!
const float gpsDataIntervalS = cmpTimeUs(timeUs, lastTimeUs) / 1e6f;
// dirty hack to remove jitter from interval
if (gpsDataIntervalS < 0.15f) {
gpsDataIntervalSeconds = 0.1f;
} else if (gpsDataIntervalS < 0.4f) {
gpsDataIntervalSeconds = 0.2f;
} else {
gpsDataIntervalSeconds = 1.0f;
}
// Detect current sample rate of GPS solution
timeUs = micros();
gpsSampleRateHz = 1e6f / cmpTimeUs(timeUs, lastTimeUs);
lastTimeUs = timeUs;
if (!(STATE(GPS_FIX) && gpsSol.numSat >= GPS_MIN_SAT_COUNT)) {
@ -1947,7 +1948,7 @@ void onGpsNewData(void)
GPS_calculateDistanceAndDirectionToHome();
if (ARMING_FLAG(ARMED)) {
GPS_calculateDistanceFlownVerticalSpeed(false);
GPS_calculateDistanceFlown(false);
}
#ifdef USE_GPS_RESCUE
@ -1965,9 +1966,9 @@ void gpsSetFixState(bool state)
}
}
float gpsGetSampleRateHz(void)
float getGpsDataIntervalSeconds(void)
{
return gpsSampleRateHz;
return gpsDataIntervalSeconds;
}
#endif // USE_GPS

3
src/main/io/gps.h
View file

@ -150,7 +150,6 @@ extern uint16_t GPS_distanceToHome; // distance to home point in meters
extern uint32_t GPS_distanceToHomeCm; // distance to home point in cm
extern int16_t GPS_directionToHome; // direction to home or hol point in degrees
extern uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters
extern int16_t GPS_verticalSpeedInCmS; // vertical speed in cm/s
extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction
extern float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles
extern int16_t nav_takeoff_bearing;
@ -214,4 +213,4 @@ void GPS_reset_home_position(void);
void GPS_calc_longitude_scaling(int32_t lat);
void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing);
void gpsSetFixState(bool state);
float gpsGetSampleRateHz(void);
float getGpsDataIntervalSeconds(void);

4
src/main/msp/msp.c
View file

@ -1535,7 +1535,7 @@ case MSP_NAME:
#ifdef USE_GPS_RESCUE
case MSP_GPS_RESCUE:
sbufWriteU16(dst, gpsRescueConfig()->angle);
sbufWriteU16(dst, gpsRescueConfig()->maxRescueAngle);
sbufWriteU16(dst, gpsRescueConfig()->initialAltitudeM);
sbufWriteU16(dst, gpsRescueConfig()->descentDistanceM);
sbufWriteU16(dst, gpsRescueConfig()->rescueGroundspeed);
@ -2798,7 +2798,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#ifdef USE_GPS_RESCUE
case MSP_SET_GPS_RESCUE:
gpsRescueConfigMutable()->angle = sbufReadU16(src);
gpsRescueConfigMutable()->maxRescueAngle = sbufReadU16(src);
gpsRescueConfigMutable()->initialAltitudeM = sbufReadU16(src);
gpsRescueConfigMutable()->descentDistanceM = sbufReadU16(src);
gpsRescueConfigMutable()->rescueGroundspeed = sbufReadU16(src);

20
src/main/pg/gps_rescue.c
View file

@ -29,26 +29,28 @@
#include "gps_rescue.h"
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 3);
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 4);
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.minRescueDth = 30,
.altitudeMode = GPS_RESCUE_ALT_MODE_MAX,
.rescueAltitudeBufferM = 10,
.ascendRate = 500, // cm/s, for altitude corrections on ascent
.ascendRate = 750, // cm/s, for altitude corrections on ascent
.initialAltitudeM = 30,
.rescueGroundspeed = 500,
.angle = 40,
.rescueGroundspeed = 750,
.maxRescueAngle = 70,
.rollMix = 150,
.pitchCutoffHz = 75,
.descentDistanceM = 20,
.descendRate = 100, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
.descendRate = 150, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
.targetLandingAltitudeM = 4,
.disarmThreshold = 20,
.throttleMin = 1100,
.throttleMax = 1600,
.throttleMax = 1700,
.throttleHover = 1275,
.allowArmingWithoutFix = false,
@ -57,10 +59,10 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.throttleP = 15,
.throttleI = 15,
.throttleD = 15,
.throttleD = 20,
.velP = 8,
.velI = 30,
.velD = 20,
.velI = 40,
.velD = 12,
.yawP = 20,
.useMag = GPS_RESCUE_USE_MAG

4
src/main/pg/gps_rescue.h
View file

@ -26,7 +26,7 @@
typedef struct gpsRescue_s {
uint16_t angle; // degrees
uint16_t maxRescueAngle; // degrees
uint16_t initialAltitudeM; // meters
uint16_t descentDistanceM; // meters
uint16_t rescueGroundspeed; // centimeters per second
@ -47,6 +47,8 @@ typedef struct gpsRescue_s {
uint16_t descendRate;
uint16_t rescueAltitudeBufferM; // meters
uint8_t rollMix;
uint8_t disarmThreshold;
uint8_t pitchCutoffHz;
} gpsRescueConfig_t;

38
src/main/rx/rx.c
View file

@ -600,7 +600,7 @@ static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample)
static uint16_t getRxfailValue(uint8_t channel)
{
const rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel);
const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
switch (channelFailsafeConfig->mode) {
case RX_FAILSAFE_MODE_AUTO:
@ -621,7 +621,7 @@ static uint16_t getRxfailValue(uint8_t channel)
default:
case RX_FAILSAFE_MODE_INVALID:
case RX_FAILSAFE_MODE_HOLD:
if (failsafeAuxSwitch) {
if (boxFailsafeSwitchIsOn) {
return rcRaw[channel]; // current values are allowed through on held channels with switch induced failsafe
} else {
return rcData[channel]; // last good value
@ -672,41 +672,46 @@ static void readRxChannelsApplyRanges(void)
void detectAndApplySignalLossBehaviour(void)
{
const uint32_t currentTimeMs = millis();
const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
rxFlightChannelsValid = rxSignalReceived && !failsafeAuxSwitch;
// set rxFlightChannelsValid false when a packet is bad or we use a failsafe switch
const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
rxFlightChannelsValid = rxSignalReceived && !boxFailsafeSwitchIsOn;
// rxFlightChannelsValid is false after 100ms of no packets, or as soon as use the BOXFAILSAFE switch is actioned
// rxFlightChannelsValid is true the instant we get a good packet or the BOXFAILSAFE switch is reverted
// can also go false with good packets but where one flight channel is bad > 300ms (PPM type receiver error)
for (int channel = 0; channel < rxChannelCount; channel++) {
float sample = rcRaw[channel]; // sample has latest RC value, rcData has last 'accepted valid' value
const bool thisChannelValid = rxFlightChannelsValid && isPulseValid(sample);
// if the whole packet is bad, consider all channels bad
// if the whole packet is bad, or BOXFAILSAFE switch is actioned, consider all channels bad
if (thisChannelValid) {
// reset the invalid pulse period timer for every good channel
validRxSignalTimeout[channel] = currentTimeMs + MAX_INVALID_PULSE_TIME_MS;
}
if (ARMING_FLAG(ARMED) && failsafeIsActive()) {
// while in failsafe Stage 2, whether Rx loss or switch induced, pass valid incoming flight channel values
// this allows GPS Rescue to detect the 30% requirement for termination
if (failsafeIsActive()) {
// we are in failsafe Stage 2, whether Rx loss or BOXFAILSAFE induced
// pass valid incoming flight channel values to FC,
// so that GPS Rescue can get the 30% requirement for termination of the rescue
if (channel < NON_AUX_CHANNEL_COUNT) {
if (!thisChannelValid) {
if (channel == THROTTLE ) {
sample = failsafeConfig()->failsafe_throttle; // stage 2 failsafe throttle value
sample = failsafeConfig()->failsafe_throttle;
// stage 2 failsafe throttle value. In GPS Rescue Flight mode, gpsRescueGetThrottle overrides, late in mixer.c
} else {
sample = rxConfig()->midrc;
}
}
} else {
// During Stage 2, set aux channels as per Stage 1 configuration
// set aux channels as per Stage 1 failsafe hold/set values, allow all for Failsafe and GPS rescue MODE switches
sample = getRxfailValue(channel);
}
} else {
if (failsafeAuxSwitch) {
// we are normal, or in failsafe stage 1
if (boxFailsafeSwitchIsOn) {
// BOXFAILSAFE active, but not in stage 2 yet, use stage 1 values
sample = getRxfailValue(channel);
// set channels to Stage 1 values immediately failsafe switch is activated
} else if (!thisChannelValid) {
// everything was normal and this channel was invalid
// everything is normal but this channel was invalid
if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) {
// first 300ms of Stage 1 failsafe
sample = rcData[channel];
@ -721,6 +726,7 @@ void detectAndApplySignalLossBehaviour(void)
// set channels that are invalid for more than 300ms to Stage 1 values
}
}
// everything is normal, ie rcData[channel] will be set to rcRaw(channel) via 'sample'
}
sample = constrainf(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
@ -740,10 +746,10 @@ void detectAndApplySignalLossBehaviour(void)
if (rxFlightChannelsValid) {
failsafeOnValidDataReceived();
// --> start the timer to exit stage 2 failsafe
// --> start the timer to exit stage 2 failsafe 100ms after losing all packets or the BOXFAILSAFE switch is actioned
} else {
failsafeOnValidDataFailed();
// -> start timer to enter stage2 failsafe
// -> start timer to enter stage2 failsafe the instant we get a good packet or the BOXFAILSAFE switch is reverted
}
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]);

19
src/test/unit/arming_prevention_unittest.cc
View file

@ -97,8 +97,10 @@ extern "C" {
bool mockIsUpright = false;
uint8_t activePidLoopDenom = 1;
float gpsGetSampleRateHz(void) { return 10.0f; }
float getGpsDataIntervalSeconds(void) { return 0.1f; }
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k) { filter->k = k; }
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) { filter->k = k; }
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k) { filter->k = k; }
}
uint32_t simulationFeatureFlags = 0;
@ -1143,11 +1145,18 @@ extern "C" {
UNUSED(throttleDLpf);
return 0.0f;
}
void pt3FilterInit(pt3Filter_t *pitchLpf, float) {
UNUSED(pitchLpf);
void pt1FilterInit(pt1Filter_t *velocityDLpf, float) {
UNUSED(velocityDLpf);
}
float pt3FilterApply(pt3Filter_t *pitchLpf, float) {
UNUSED(pitchLpf);
float pt1FilterApply(pt1Filter_t *velocityDLpf, float) {
UNUSED(velocityDLpf);
return 0.0f;
}
void pt3FilterInit(pt3Filter_t *velocityUpsampleLpf, float) {
UNUSED(velocityUpsampleLpf);
}
float pt3FilterApply(pt3Filter_t *velocityUpsampleLpf, float) {
UNUSED(velocityUpsampleLpf);
return 0.0f;
}
void getRcDeflectionAbs(void) {}

1
src/test/unit/flight_imu_unittest.cc
View file

@ -213,7 +213,6 @@ extern "C" {
mag_t mag;
gpsSolutionData_t gpsSol;
int16_t GPS_verticalSpeedInCmS;
uint8_t debugMode;
int16_t debug[DEBUG16_VALUE_COUNT];