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:
parent
7d86cd6523
commit
3a282d1d3e
|
@ -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_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_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_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_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_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) },
|
{ "gps_rescue_throttle_i", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 500 }, PG_GPS_RESCUE, offsetof(gpsRescueConfig_t, throttleI) },
|
||||||
|
|
|
@ -116,8 +116,9 @@ typedef struct {
|
||||||
bool isAvailable;
|
bool isAvailable;
|
||||||
} rescueState_s;
|
} rescueState_s;
|
||||||
|
|
||||||
#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
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1);
|
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 1);
|
||||||
|
|
||||||
|
@ -140,7 +141,9 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
||||||
.minSats = 8,
|
.minSats = 8,
|
||||||
.minRescueDth = 100,
|
.minRescueDth = 100,
|
||||||
.allowArmingWithoutFix = false,
|
.allowArmingWithoutFix = false,
|
||||||
.useMag = true
|
.useMag = true,
|
||||||
|
.targetLandingAltitudeM = 5,
|
||||||
|
.targetLandingDistanceM = 10,
|
||||||
);
|
);
|
||||||
|
|
||||||
static uint16_t rescueThrottle;
|
static uint16_t rescueThrottle;
|
||||||
|
@ -470,8 +473,11 @@ static bool checkGPSRescueIsAvailable(void)
|
||||||
*/
|
*/
|
||||||
void updateGPSRescueState(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)) {
|
if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
|
||||||
rescueStop();
|
rescueStop();
|
||||||
} else if (FLIGHT_MODE(GPS_RESCUE_MODE) && rescueState.phase == RESCUE_IDLE) {
|
} 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.
|
// 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
|
//set new descent distance if actual distance to home is lower
|
||||||
if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->descentDistanceM) {
|
if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->descentDistanceM) {
|
||||||
descentDistance = rescueState.sensor.distanceToHomeM - 5;
|
newDescentDistanceM = rescueState.sensor.distanceToHomeM - 5;
|
||||||
} else {
|
} 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;
|
rescueState.phase = RESCUE_ATTAIN_ALT;
|
||||||
FALLTHROUGH;
|
FALLTHROUGH;
|
||||||
|
@ -535,7 +546,7 @@ void updateGPSRescueState(void)
|
||||||
rescueState.intent.maxAngleDeg = 15;
|
rescueState.intent.maxAngleDeg = 15;
|
||||||
break;
|
break;
|
||||||
case RESCUE_CROSSTRACK:
|
case RESCUE_CROSSTRACK:
|
||||||
if (rescueState.sensor.distanceToHomeM < descentDistance) {
|
if (rescueState.sensor.distanceToHomeM <= newDescentDistanceM) {
|
||||||
rescueState.phase = RESCUE_LANDING_APPROACH;
|
rescueState.phase = RESCUE_LANDING_APPROACH;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -549,19 +560,23 @@ void updateGPSRescueState(void)
|
||||||
break;
|
break;
|
||||||
case RESCUE_LANDING_APPROACH:
|
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
|
// 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;
|
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)
|
// 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 newAlt = (lineSlope * rescueState.sensor.distanceToHomeM + lineOffsetM) * 100;
|
||||||
const int32_t newSpeed = gpsRescueConfig()->rescueGroundspeed * rescueState.sensor.distanceToHomeM / descentDistance;
|
|
||||||
|
// 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.targetAltitudeCm = constrain(newAlt, 100, rescueState.intent.targetAltitudeCm);
|
||||||
rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed);
|
rescueState.intent.targetGroundspeed = constrain(newSpeed, 100, rescueState.intent.targetGroundspeed);
|
||||||
rescueState.intent.crosstrack = true;
|
rescueState.intent.crosstrack = true;
|
||||||
rescueState.intent.minAngleDeg = 10;
|
rescueState.intent.minAngleDeg = 10;
|
||||||
rescueState.intent.maxAngleDeg = 20;
|
rescueState.intent.maxAngleDeg = gpsRescueConfig()->angle;
|
||||||
break;
|
break;
|
||||||
case RESCUE_LANDING:
|
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.
|
// We have reached the XYZ envelope to be considered at "home". We need to land gently and check our accelerometer for abnormal data.
|
||||||
|
|
|
@ -25,7 +25,7 @@ typedef struct gpsRescue_s {
|
||||||
uint16_t angle; //degrees
|
uint16_t angle; //degrees
|
||||||
uint16_t initialAltitudeM; //meters
|
uint16_t initialAltitudeM; //meters
|
||||||
uint16_t descentDistanceM; //meters
|
uint16_t descentDistanceM; //meters
|
||||||
uint16_t rescueGroundspeed; // centimeters per second
|
uint16_t rescueGroundspeed; //centimeters per second
|
||||||
uint16_t throttleP, throttleI, throttleD;
|
uint16_t throttleP, throttleI, throttleD;
|
||||||
uint16_t yawP;
|
uint16_t yawP;
|
||||||
uint16_t throttleMin;
|
uint16_t throttleMin;
|
||||||
|
@ -37,6 +37,8 @@ typedef struct gpsRescue_s {
|
||||||
uint8_t sanityChecks;
|
uint8_t sanityChecks;
|
||||||
uint8_t allowArmingWithoutFix;
|
uint8_t allowArmingWithoutFix;
|
||||||
uint8_t useMag;
|
uint8_t useMag;
|
||||||
|
uint16_t targetLandingAltitudeM; //meters
|
||||||
|
uint16_t targetLandingDistanceM; //meters
|
||||||
} gpsRescueConfig_t;
|
} gpsRescueConfig_t;
|
||||||
|
|
||||||
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
|
PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
|
||||||
|
|
Loading…
Reference in New Issue