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

Add gps rescue option for not climbing to highest altitude of flight (#8301) 

Add gps rescue option for not climbing to highest altitude of flight
This commit is contained in:
Michael Keller 2019-05-20 19:58:52 +12:00 committed by GitHub
parent 70215cc758 1262dbf06f
commit 1c313a9a02
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 32 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/main/cli/settings.c
View File

@ -356,6 +356,9 @@ static const char * const lookupTableThrottleLimitType[] = {
static const char * const lookupTableRescueSanityType[] = {
"RESCUE_SANITY_OFF", "RESCUE_SANITY_ON", "RESCUE_SANITY_FS_ONLY"
};
static const char * const lookupTableRescueAltitudeMode[] = {
"MAX_ALT", "FIXED_ALT", "CURRENT_ALT"
};
#endif
#ifdef USE_MAX7456
@ -472,6 +475,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableGPSSBASMode),
#ifdef USE_GPS_RESCUE
LOOKUP_TABLE_ENTRY(lookupTableRescueSanityType),
LOOKUP_TABLE_ENTRY(lookupTableRescueAltitudeMode),
#endif
#endif
#ifdef USE_BLACKBOX
@ -904,10 +908,11 @@ const clivalue_t valueTable[] = {
{ "gps_rescue_throttle_min", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMin) },
{ "gps_rescue_throttle_max", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 2000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleMax) },
{ "gps_rescue_throttle_hover", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 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_sanity_checks", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_SANITY_CHECK }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, sanityChecks) },
{ "gps_rescue_min_sats", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 50 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, minSats) },
{ "gps_rescue_min_dth", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 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_alt_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_RESCUE_ALT_MODE }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, altitudeMode) },
#ifdef USE_MAG
{ "gps_rescue_use_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
#endif

3
src/main/cli/settings.h
View File

@ -34,7 +34,8 @@ typedef enum {
TABLE_GPS_SBAS_MODE,
#endif
#ifdef USE_GPS_RESCUE
TABLE_GPS_RESCUE,
TABLE_GPS_RESCUE_SANITY_CHECK,
TABLE_GPS_RESCUE_ALT_MODE,
#endif
#ifdef USE_BLACKBOX
TABLE_BLACKBOX_DEVICE,

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

@ -116,6 +116,12 @@ typedef struct {
bool isAvailable;
} rescueState_s;
typedef enum {
MAX_ALT,
FIXED_ALT,
CURRENT_ALT
} altitudeMode_e;
#define GPS_RESCUE_MAX_YAW_RATE 180 // deg/sec max yaw rate
#define GPS_RESCUE_RATE_SCALE_DEGREES 45 // Scale the commanded yaw rate when the error is less then this angle
#define GPS_RESCUE_SLOWDOWN_DISTANCE_M 200 // distance from home to start decreasing speed
@ -150,6 +156,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.useMag = GPS_RESCUE_USE_MAG,
.targetLandingAltitudeM = 5,
.targetLandingDistanceM = 10,
.altitudeMode = MAX_ALT,
);
static uint16_t rescueThrottle;
@ -165,6 +172,7 @@ bool magForceDisable = false;
static bool newGPSData = false;
rescueState_s rescueState;
altitudeMode_e altitudeMode;
/*
If we have new GPS data, update home heading
@ -486,6 +494,7 @@ void updateGPSRescueState(void)
static float_t lineSlope;
static float_t lineOffsetM;
static int32_t newSpeed;
static uint32_t newAltitude;
if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
rescueStop();
@ -536,6 +545,18 @@ void updateGPSRescueState(void)
newDescentDistanceM = gpsRescueConfig()->descentDistanceM;
}
switch (altitudeMode) {
case MAX_ALT:
newAltitude = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500);
break;
case FIXED_ALT:
newAltitude = gpsRescueConfig()->initialAltitudeM * 100;
break;
case CURRENT_ALT:
newAltitude = rescueState.sensor.currentAltitudeCm;
break;
}
//Calculate angular coefficient and offset for equation of line from 2 points needed for RESCUE_LANDING_APPROACH
lineSlope = ((float)gpsRescueConfig()->initialAltitudeM - gpsRescueConfig()->targetLandingAltitudeM) / (newDescentDistanceM - gpsRescueConfig()->targetLandingDistanceM);
lineOffsetM = gpsRescueConfig()->initialAltitudeM - lineSlope * newDescentDistanceM;
@ -549,7 +570,7 @@ void updateGPSRescueState(void)
}
rescueState.intent.targetGroundspeed = 500;
rescueState.intent.targetAltitudeCm = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500);
rescueState.intent.targetAltitudeCm = newAltitude;
rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 10;
rescueState.intent.maxAngleDeg = 15;
@ -562,7 +583,7 @@ void updateGPSRescueState(void)
// We can assume at this point that we are at or above our RTH height, so we need to try and point to home and tilt while maintaining alt
// Is our altitude way off? We should probably kick back to phase RESCUE_ATTAIN_ALT
rescueState.intent.targetGroundspeed = gpsRescueConfig()->rescueGroundspeed;
rescueState.intent.targetAltitudeCm = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500);
rescueState.intent.targetAltitudeCm = newAltitude;
rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 15;
rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle;

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

@ -39,6 +39,7 @@ typedef struct gpsRescue_s {
uint8_t useMag;
uint16_t targetLandingAltitudeM; //meters
uint16_t targetLandingDistanceM; //meters
uint8_t altitudeMode;
} gpsRescueConfig_t;
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);