Mag heading optionally ignored while GPS Rescue is running
If flyaway condition is met and a mag is in use, mag is disabled and countdown is reset Minor cleanup
This commit is contained in:
parent
d8e8d8374d
commit
25a499cc9a
|
@ -140,6 +140,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
|||
.minSats = 8,
|
||||
.minRescueDth = 100,
|
||||
.allowArmingWithoutFix = false,
|
||||
.useMag = true
|
||||
);
|
||||
|
||||
static uint16_t rescueThrottle;
|
||||
|
@ -150,6 +151,7 @@ uint16_t hoverThrottle = 0;
|
|||
float averageThrottle = 0.0;
|
||||
float altitudeError = 0.0;
|
||||
uint32_t throttleSamples = 0;
|
||||
bool magForceDisable = false;
|
||||
|
||||
static bool newGPSData = false;
|
||||
|
||||
|
@ -368,7 +370,14 @@ static void performSanityChecks()
|
|||
lastDistanceToHomeM = rescueState.sensor.distanceToHomeM;
|
||||
|
||||
if (secondsFlyingAway == 10) {
|
||||
rescueState.failure = RESCUE_FLYAWAY;
|
||||
//If there is a mag and has not been disabled, we have to assume is healthy and has been used in imu.c
|
||||
if (sensors(SENSOR_MAG) && gpsRescueConfig()->useMag && !magForceDisable) {
|
||||
//Try again with mag disabled
|
||||
magForceDisable = true;
|
||||
secondsFlyingAway = 0;
|
||||
} else {
|
||||
rescueState.failure = RESCUE_FLYAWAY;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -414,7 +423,7 @@ static void sensorUpdate()
|
|||
// 3. GPS number of satellites is less than the minimum configured for GPS rescue.
|
||||
// Note: this function does not take into account the distance from homepoint etc. (gps_rescue_min_dth) and
|
||||
// is also independent of the gps_rescue_sanity_checks configuration
|
||||
static bool gpsRescueIsAvailable(void)
|
||||
static bool checkGPSRescueIsAvailable(void)
|
||||
{
|
||||
static uint32_t previousTimeUs = 0; // Last time LowSat was checked
|
||||
const uint32_t currentTimeUs = micros();
|
||||
|
@ -473,7 +482,7 @@ void updateGPSRescueState(void)
|
|||
|
||||
sensorUpdate();
|
||||
|
||||
rescueState.isAvailable = gpsRescueIsAvailable();
|
||||
rescueState.isAvailable = checkGPSRescueIsAvailable();
|
||||
|
||||
switch (rescueState.phase) {
|
||||
case RESCUE_IDLE:
|
||||
|
@ -604,9 +613,14 @@ bool gpsRescueIsConfigured(void)
|
|||
return failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE || isModeActivationConditionPresent(BOXGPSRESCUE);
|
||||
}
|
||||
|
||||
bool isGPSRescueAvailable(void)
|
||||
bool gpsRescueIsAvailable(void)
|
||||
{
|
||||
return rescueState.isAvailable;
|
||||
}
|
||||
|
||||
bool gpsRescueDisableMag(void)
|
||||
{
|
||||
return ((!gpsRescueConfig()->useMag || magForceDisable) && (rescueState.phase >= RESCUE_INITIALIZE) && (rescueState.phase <= RESCUE_LANDING));
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
@ -34,6 +34,7 @@ typedef struct gpsRescue_s {
|
|||
uint16_t minRescueDth; //meters
|
||||
uint8_t sanityChecks;
|
||||
uint8_t allowArmingWithoutFix;
|
||||
uint8_t useMag;
|
||||
} gpsRescueConfig_t;
|
||||
|
||||
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
|
||||
|
@ -46,4 +47,5 @@ void rescueNewGpsData(void);
|
|||
float gpsRescueGetYawRate(void);
|
||||
float gpsRescueGetThrottle(void);
|
||||
bool gpsRescueIsConfigured(void);
|
||||
bool isGPSRescueAvailable(void);
|
||||
bool gpsRescueIsAvailable(void);
|
||||
bool gpsRescueDisableMag(void);
|
||||
|
|
|
@ -38,6 +38,7 @@
|
|||
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/gps_rescue.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
|
@ -441,7 +442,11 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
|||
previousIMUUpdateTime = currentTimeUs;
|
||||
|
||||
#ifdef USE_MAG
|
||||
if (sensors(SENSOR_MAG) && compassIsHealthy()) {
|
||||
if (sensors(SENSOR_MAG) && compassIsHealthy()
|
||||
#ifdef USE_GPS_RESCUE
|
||||
&& !gpsRescueDisableMag()
|
||||
#endif
|
||||
) {
|
||||
useMag = true;
|
||||
}
|
||||
#endif
|
||||
|
@ -452,14 +457,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
|||
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
|
||||
useCOG = true;
|
||||
} else {
|
||||
// If GPS rescue mode is active and we can use it, go for it. When we're close to home we will
|
||||
// probably stop re calculating GPS heading data. Other future modes can also use this extern
|
||||
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
|
||||
|
||||
if (canUseGPSHeading) {
|
||||
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
|
||||
|
||||
useCOG = true;
|
||||
}
|
||||
useCOG = true;
|
||||
}
|
||||
|
||||
if (useCOG && shouldInitializeGPSHeading()) {
|
||||
|
|
|
@ -851,6 +851,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
|
||||
{ "gps_rescue_min_dth", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 50, 1000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minRescueDth) },
|
||||
{ "gps_rescue_allow_arming_without_fix", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowArmingWithoutFix) },
|
||||
{ "gps_rescue_use_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
|
|
@ -996,7 +996,7 @@ static bool osdDrawSingleElement(uint8_t item)
|
|||
if (osdWarnGetState(OSD_WARNING_GPS_RESCUE_UNAVAILABLE) &&
|
||||
ARMING_FLAG(ARMED) &&
|
||||
gpsRescueIsConfigured() &&
|
||||
!isGPSRescueAvailable()) {
|
||||
!gpsRescueIsAvailable()) {
|
||||
osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "NO GPS RESC");
|
||||
SET_BLINK(OSD_WARNINGS);
|
||||
break;
|
||||
|
|
|
@ -245,4 +245,5 @@ int32_t baroCalculateAltitude(void) { return 0; }
|
|||
bool gyroGetAccumulationAverage(float *) { return false; }
|
||||
bool accGetAccumulationAverage(float *) { return false; }
|
||||
void mixerSetThrottleAngleCorrection(int) {};
|
||||
bool gpsRescueIsRunning(void) { return false; }
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue