Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

added altitude options

Browse source
This commit is contained in:
Nicola De Pasquale 2019-05-19 12:09:27 +02:00
parent 40d390cfdc
commit 1262dbf06f
4 changed files with 27 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

8
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,11 +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_climb_to_max_reached_alt", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, allowClimbToMaxReachedAlt) },
{ "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,

23
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,7 +156,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.useMag = GPS_RESCUE_USE_MAG,
.targetLandingAltitudeM = 5,
.targetLandingDistanceM = 10,
.allowClimbToMaxReachedAlt = true,
.altitudeMode = MAX_ALT,
);
static uint16_t rescueThrottle;
@ -166,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
@ -538,10 +545,16 @@ void updateGPSRescueState(void)
newDescentDistanceM = gpsRescueConfig()->descentDistanceM;
}
if (gpsRescueConfig()->allowClimbToMaxReachedAlt) {
newAltitude = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500);
} else {
newAltitude = gpsRescueConfig()->initialAltitudeM * 100;
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

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

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