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

improve landing altitude 

fix some comments

some requested fixes

initialLandingAltitude converted in meters

removed landing altitude setting from msp, added missing comma

re-added gps_rescue_landing_alt in settings.c

made target landing distance configurable, renamed descentDistance

added slow down distance and improved return speed

changed newSpeed formula

moved newSpeed variable in gps rescue inizialization phase

newSpeed declared as int32_t

modified slow down distance to constant

added some comments

fix type-casting bug

removed extra space

changed slow down distance for better landing

changed default targetLandingAltitudeM to 5 meters
This commit is contained in:
Nicola De Pasquale 2019-03-19 19:52:13 +01:00
parent 7d86cd6523
commit 3a282d1d3e
3 changed files with 32 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -875,6 +875,8 @@ const clivalue_t valueTable[] = {
{ "gps_rescue_angle", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, angle) },
{ "gps_rescue_initial_alt", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 20, 100 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, initialAltitudeM) },
{ "gps_rescue_descent_dist", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, descentDistanceM) },
{ "gps_rescue_landing_alt", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 3, 10 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingAltitudeM) },
{ "gps_rescue_landing_dist", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 15 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, targetLandingDistanceM) },
{ "gps_rescue_ground_speed", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 30, 3000 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, rescueGroundspeed) },
{ "gps_rescue_throttle_p", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleP) },
{ "gps_rescue_throttle_i", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) },

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

@ -116,8 +116,9 @@ typedef struct {
bool isAvailable;
} rescueState_s;
#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_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
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1);
@ -140,7 +141,9 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
.minSats = 8,
.minRescueDth = 100,
.allowArmingWithoutFix = false,
.useMag = true
.useMag = true,
.targetLandingAltitudeM = 5,
.targetLandingDistanceM = 10,
);
static uint16_t rescueThrottle;
@ -470,8 +473,11 @@ static bool checkGPSRescueIsAvailable(void)
*/
void updateGPSRescueState(void)
{
static uint16_t descentDistance;
static uint16_t newDescentDistanceM;
static float_t lineSlope;
static float_t lineOffsetM;
static int32_t newSpeed;
if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
rescueStop();
} else if (FLIGHT_MODE(GPS_RESCUE_MODE) && rescueState.phase == RESCUE_IDLE) {
@ -513,12 +519,17 @@ void updateGPSRescueState(void)
// When not in failsafe mode: leave it up to the sanity check setting.
}
newSpeed = gpsRescueConfig()->rescueGroundspeed;
//set new descent distance if actual distance to home is lower
if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->descentDistanceM) {
descentDistance = rescueState.sensor.distanceToHomeM - 5;
newDescentDistanceM = rescueState.sensor.distanceToHomeM - 5;
} else {
descentDistance = gpsRescueConfig()->descentDistanceM;
newDescentDistanceM = gpsRescueConfig()->descentDistanceM;
}
//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;
rescueState.phase = RESCUE_ATTAIN_ALT;
FALLTHROUGH;
@ -535,7 +546,7 @@ void updateGPSRescueState(void)
rescueState.intent.maxAngleDeg = 15;
break;
case RESCUE_CROSSTRACK:
if (rescueState.sensor.distanceToHomeM < descentDistance) {
if (rescueState.sensor.distanceToHomeM <= newDescentDistanceM) {
rescueState.phase = RESCUE_LANDING_APPROACH;
}
@ -549,19 +560,23 @@ void updateGPSRescueState(void)
break;
case RESCUE_LANDING_APPROACH:
// We are getting close to home in the XY plane, get Z where it needs to be to move to landing phase
if (rescueState.sensor.distanceToHomeM < 10 && rescueState.sensor.currentAltitudeCm <= 1000) {
if (rescueState.sensor.distanceToHomeM <= gpsRescueConfig()->targetLandingDistanceM && rescueState.sensor.currentAltitudeCm <= gpsRescueConfig()->targetLandingAltitudeM * 100) {
rescueState.phase = RESCUE_LANDING;
}
// Only allow new altitude and new speed to be equal or lower than the current values (to prevent parabolic movement on overshoot)
const int32_t newAlt = gpsRescueConfig()->initialAltitudeM * 100 * rescueState.sensor.distanceToHomeM / descentDistance;
const int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHomeM / descentDistance;
const int32_t newAlt = (lineSlope * rescueState.sensor.distanceToHomeM + lineOffsetM) * 100;
// Start to decrease proportionally the quad's speed when the distance to home is less or equal than GPS_RESCUE_SLOWDOWN_DISTANCE_M
if (rescueState.sensor.distanceToHomeM <= GPS_RESCUE_SLOWDOWN_DISTANCE_M) {
newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHomeM / GPS_RESCUE_SLOWDOWN_DISTANCE_M;
}
rescueState.intent.targetAltitudeCm = constrain(newAlt, 100, rescueState.intent.targetAltitudeCm);
rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed);
rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 10;
rescueState.intent.maxAngleDeg = 20;
rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle;
break;
case RESCUE_LANDING:
// We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data.

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

@ -25,7 +25,7 @@ typedef struct gpsRescue_s {
uint16_t angle; //degrees
uint16_t initialAltitudeM; //meters
uint16_t descentDistanceM; //meters
uint16_t rescueGroundspeed; // centimeters per second
uint16_t rescueGroundspeed; //centimeters per second
uint16_t throttleP, throttleI, throttleD;
uint16_t yawP;
uint16_t throttleMin;
@ -37,6 +37,8 @@ typedef struct gpsRescue_s {
uint8_t sanityChecks;
uint8_t allowArmingWithoutFix;
uint8_t useMag;
uint16_t targetLandingAltitudeM; //meters
uint16_t targetLandingDistanceM; //meters
} gpsRescueConfig_t;
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);