Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 00:35:39 +03:00
Code Issues Wiki Activity

better code

Browse source
This commit is contained in:
Nicola De Pasquale 2019-05-19 03:10:07 +02:00
parent c5adc0b549
commit 40d390cfdc
1 changed files with 9 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -487,6 +487,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();
@ -537,6 +538,12 @@ void updateGPSRescueState(void)
newDescentDistanceM = gpsRescueConfig()->descentDistanceM;
}
if (gpsRescueConfig()->allowClimbToMaxReachedAlt) {
newAltitude = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500);
} else {
newAltitude = gpsRescueConfig()->initialAltitudeM * 100;
}
//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;
@ -550,11 +557,7 @@ void updateGPSRescueState(void)
}
rescueState.intent.targetGroundspeed = 500;
if (gpsRescueConfig()->allowClimbToMaxReachedAlt) {
rescueState.intent.targetAltitudeCm = MAX(gpsRescueConfig()->initialAltitudeM * 100, rescueState.sensor.maxAltitudeCm + 1500);
} else {
rescueState.intent.targetAltitudeCm = gpsRescueConfig()->initialAltitudeM * 100;
}
rescueState.intent.targetAltitudeCm = newAltitude;
rescueState.intent.crosstrack = true;
rescueState.intent.minAngleDeg = 10;
rescueState.intent.maxAngleDeg = 15;
@ -567,7 +570,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;