Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +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[] = { static const char * const lookupTableRescueSanityType[] = {
"RESCUE_SANITY_OFF", "RESCUE_SANITY_ON", "RESCUE_SANITY_FS_ONLY" "RESCUE_SANITY_OFF", "RESCUE_SANITY_ON", "RESCUE_SANITY_FS_ONLY"
}; };
static const char * const lookupTableRescueAltitudeMode[] = {
"MAX_ALT", "FIXED_ALT", "CURRENT_ALT"
};
#endif #endif
#ifdef USE_MAX7456 #ifdef USE_MAX7456
@ -472,6 +475,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableGPSSBASMode), LOOKUP_TABLE_ENTRY(lookupTableGPSSBASMode),
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE
LOOKUP_TABLE_ENTRY(lookupTableRescueSanityType), LOOKUP_TABLE_ENTRY(lookupTableRescueSanityType),
LOOKUP_TABLE_ENTRY(lookupTableRescueAltitudeMode),
#endif #endif
#endif #endif
#ifdef USE_BLACKBOX #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_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_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_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_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_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_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 #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) }, { "gps_rescue_use_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, useMag) },
#endif #endif

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

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

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

@ -116,6 +116,12 @@ typedef struct {
bool isAvailable; bool isAvailable;
} rescueState_s; } 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_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_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 #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, .useMag = GPS_RESCUE_USE_MAG,
.targetLandingAltitudeM = 5, .targetLandingAltitudeM = 5,
.targetLandingDistanceM = 10, .targetLandingDistanceM = 10,
.allowClimbToMaxReachedAlt = true, .altitudeMode = MAX_ALT,
); );
static uint16_t rescueThrottle; static uint16_t rescueThrottle;
@ -166,6 +172,7 @@ bool magForceDisable = false;
static bool newGPSData = false; static bool newGPSData = false;
rescueState_s rescueState; rescueState_s rescueState;
altitudeMode_e altitudeMode;
/* /*
If we have new GPS data, update home heading If we have new GPS data, update home heading
@ -538,10 +545,16 @@ void updateGPSRescueState(void)
newDescentDistanceM = gpsRescueConfig()->descentDistanceM; newDescentDistanceM = gpsRescueConfig()->descentDistanceM;
} }
if (gpsRescueConfig()->allowClimbToMaxReachedAlt) { switch (altitudeMode) {
newAltitude = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500); case MAX_ALT:
} else { newAltitude = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500);
newAltitude = gpsRescueConfig()->initialAltitudeM * 100; 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 //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; uint8_t useMag;
uint16_t targetLandingAltitudeM; //meters uint16_t targetLandingAltitudeM; //meters
uint16_t targetLandingDistanceM; //meters uint16_t targetLandingDistanceM; //meters
uint8_t allowClimbToMaxReachedAlt; uint8_t altitudeMode;
} gpsRescueConfig_t; } gpsRescueConfig_t;
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig); PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);