rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #7320 from TonyBlit/gps_rescue_arm 

Allow arming without GPS
This commit is contained in:
Michael Keller 2019-01-11 03:48:34 +13:00 committed by GitHub
parent 970e8fe46b 007e14f348
commit 6e42e3f68f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 165 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/main/fc/core.c
View File

@ -279,7 +279,7 @@ void updateArmingStatus(void)
#ifdef USE_GPS_RESCUE
if (gpsRescueIsConfigured()) {
if (!gpsRescueConfig()->minSats || STATE(GPS_FIX) || ARMING_FLAG(WAS_EVER_ARMED)) {
if (gpsRescueConfig()->allowArmingWithoutFix || STATE(GPS_FIX) || ARMING_FLAG(WAS_EVER_ARMED)) {
unsetArmingDisabled(ARMING_DISABLED_GPS);
} else {
setArmingDisabled(ARMING_DISABLED_GPS);
@ -455,8 +455,10 @@ void tryArm(void)
lastArmingDisabledReason = 0;
//beep to indicate arming
#ifdef USE_GPS
GPS_reset_home_position();
//beep to indicate arming
if (featureIsEnabled(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) {
beeper(BEEPER_ARMING_GPS_FIX);
} else {
@ -1127,7 +1129,6 @@ FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
}
}
bool isFlipOverAfterCrashActive(void)
{
return flipOverAfterCrashActive;

10
src/main/flight/gps_rescue.c
View File

@ -138,7 +138,8 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.throttleHover = 1280,
.sanityChecks = RESCUE_SANITY_ON,
.minSats = 8,
.minRescueDth = 100
.minRescueDth = 100,
.allowArmingWithoutFix = false,
);
static uint16_t rescueThrottle;
@ -422,7 +423,7 @@ static bool gpsRescueIsAvailable(void)
static bool noGPSfix = false;
bool result = true;
if (!gpsIsHealthy() ) {
if (!gpsIsHealthy() || !STATE(GPS_FIX_HOME)) {
return false;
}
@ -483,6 +484,11 @@ void updateGPSRescueState(void)
hoverThrottle = gpsRescueConfig()->throttleHover;
}
if (!STATE(GPS_FIX_HOME)) {
setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
disarm();
}
// Minimum distance detection.
if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) {
rescueState.failure = RESCUE_TOO_CLOSE;

1
src/main/flight/gps_rescue.h
View File

@ -33,6 +33,7 @@ typedef struct gpsRescue_s {
uint8_t minSats;
uint16_t minRescueDth; //meters
uint8_t sanityChecks;
uint8_t allowArmingWithoutFix;
} gpsRescueConfig_t;
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);

3
src/main/interface/settings.c
View File

@ -848,8 +848,9 @@ const clivalue_t valueTable[] = {
{ "gps_rescue_throttle_max", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) },
{ "gps_rescue_throttle_hover", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleHover) },
{ "gps_rescue_sanity_checks", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) },
{ "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
{ "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) },
#endif
#endif

73
src/main/io/gps.c
View File

@ -1271,6 +1271,39 @@ void GPS_calc_longitude_scaling(int32_t lat)
GPS_scaleLonDown = cos_approx(rads);
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate the distance flown and vertical speed from gps position data
//
static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
{
static int32_t lastCoord[2] = { 0, 0 };
static int16_t lastAlt;
static int32_t lastMillis;
int currentMillis = millis();
if (initialize) {
GPS_distanceFlownInCm = 0;
GPS_verticalSpeedInCmS = 0;
} else {
if (STATE(GPS_FIX_HOME)) {
// Only add up movement when speed is faster than minimum threshold
if (gpsSol.groundSpeed > GPS_DISTANCE_FLOWN_MIN_GROUND_SPEED_THRESHOLD_CM_S) {
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir);
GPS_distanceFlownInCm += dist;
}
}
GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500.0f, 1500.0f);
}
lastCoord[LON] = gpsSol.llh.lon;
lastCoord[LAT] = gpsSol.llh.lat;
lastAlt = gpsSol.llh.altCm;
lastMillis = currentMillis;
}
void GPS_reset_home_position(void)
{
@ -1281,6 +1314,7 @@ void GPS_reset_home_position(void)
// Set ground altitude
ENABLE_STATE(GPS_FIX_HOME);
}
GPS_calculateDistanceFlownVerticalSpeed(true); //Initialize
}
////////////////////////////////////////////////////////////////////////////////////
@ -1313,49 +1347,12 @@ void GPS_calculateDistanceAndDirectionToHome(void)
}
}
////////////////////////////////////////////////////////////////////////////////////
// Calculate the distance flown from gps position data
//
static void GPS_calculateDistanceFlown(bool initialize)
{
static int32_t lastCoord[2] = { 0, 0 };
static int16_t lastAlt;
static int32_t lastMillis;
int currentMillis = millis();
if (initialize) {
GPS_distanceFlownInCm = 0;
GPS_verticalSpeedInCmS = 0;
} else {
// Only add up movement when speed is faster than minimum threshold
if (gpsSol.groundSpeed > GPS_DISTANCE_FLOWN_MIN_GROUND_SPEED_THRESHOLD_CM_S) {
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir);
GPS_distanceFlownInCm += dist;
}
GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500.0f, 1500.0f);
}
lastCoord[LON] = gpsSol.llh.lon;
lastCoord[LAT] = gpsSol.llh.lat;
lastAlt = gpsSol.llh.altCm;
lastMillis = currentMillis;
}
void onGpsNewData(void)
{
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
return;
}
if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) {
GPS_reset_home_position();
GPS_calculateDistanceFlown(true);
}
// Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
@ -1395,7 +1392,7 @@ void onGpsNewData(void)
GPS_calculateDistanceAndDirectionToHome();
if (ARMING_FLAG(ARMED)) {
GPS_calculateDistanceFlown(false);
GPS_calculateDistanceFlownVerticalSpeed(false);
}
#ifdef USE_GPS_RESCUE

116
src/test/unit/arming_prevention_unittest.cc
View File

@ -616,7 +616,7 @@ TEST(ArmingPreventionTest, WhenUsingSwitched3DModeThenNormalThrottleArmingCondit
EXPECT_EQ(0, getArmingDisableFlags());
}
TEST(ArmingPreventionTest, Rescue)
TEST(ArmingPreventionTest, GPSRescueWithoutFixPreventsArm)
{
// given
simulationFeatureFlags = 0;
@ -637,6 +637,120 @@ TEST(ArmingPreventionTest, Rescue)
// and
rxConfigMutable()->mincheck = 1050;
// given
rcData[THROTTLE] = 1000;
rcData[AUX1] = 1000;
rcData[AUX2] = 1000;
ENABLE_STATE(SMALL_ANGLE);
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_FALSE(ARMING_FLAG(ARMED));
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_GPS, getArmingDisableFlags());
EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
// given
// arm
rcData[AUX1] = 1800;
// when
tryArm();
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_FALSE(ARMING_FLAG(ARMED));
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH|ARMING_DISABLED_GPS, getArmingDisableFlags());
EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
// given
// disarm
rcData[AUX1] = 1000;
// when
disarm();
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_FALSE(ARMING_FLAG(ARMED));
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_GPS, getArmingDisableFlags());
EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
// given
// receive GPS fix
ENABLE_STATE(GPS_FIX);
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_FALSE(ARMING_FLAG(ARMED));
EXPECT_FALSE(isArmingDisabled());
EXPECT_EQ(0, getArmingDisableFlags());
EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
// given
// arm
rcData[AUX1] = 1800;
// when
tryArm();
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_TRUE(ARMING_FLAG(ARMED));
EXPECT_FALSE(isArmingDisabled());
EXPECT_EQ(0, getArmingDisableFlags());
EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
// given
// disarm
rcData[AUX1] = 1000;
// when
disarm();
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_FALSE(ARMING_FLAG(ARMED));
EXPECT_FALSE(isArmingDisabled());
EXPECT_EQ(0, getArmingDisableFlags());
EXPECT_FALSE(IS_RC_MODE_ACTIVE(BOXGPSRESCUE));
}
TEST(ArmingPreventionTest, GPSRescueSwitchPreventsArm)
{
// given
simulationFeatureFlags = 0;
simulationTime = 0;
gyroCalibDone = true;
gpsSol.numSat = 5;
ENABLE_STATE(GPS_FIX);
// and
modeActivationConditionsMutable(0)->auxChannelIndex = 0;
modeActivationConditionsMutable(0)->modeId = BOXARM;
modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
modeActivationConditionsMutable(1)->auxChannelIndex = 1;
modeActivationConditionsMutable(1)->modeId = BOXGPSRESCUE;
modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
rcControlsInit();
// and
rxConfigMutable()->mincheck = 1050;
// given
rcData[THROTTLE] = 1000;
rcData[AUX1] = 1000;