Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 08:45:31 +03:00
Code Issues Wiki Activity

Take into account the climb_first setting in RTH estimations

Browse source
This commit is contained in:
Michel Pastor 2018-07-24 17:01:29 +02:00
parent 14738b5c90
commit 6bccc66ac3
1 changed files with 1 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/flight/rth_estimator.c
View file

@ -114,7 +114,7 @@ static uint16_t estimateRTHAltitudeChangeEnergy(float altitudeChange, float vert
// *heading is in degrees
static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChange, float horizontalWindSpeed, float windHeading, float verticalWindSpeed, float *heading) {
float estimatedAltitudeChangeGroundDistance = estimateRTHAltitudeChangeGroundDistance(altitudeChange, horizontalWindSpeed, windHeading, verticalWindSpeed);
if (altitudeChange > 0) {
if (navConfig()->general.flags.rth_climb_first && (altitudeChange > 0)) {
float headingDiff = DEGREES_TO_RADIANS(DECIDEGREES_TO_DEGREES((float)attitude.values.yaw) - GPS_directionToHome);
float triangleAltitude = GPS_distanceToHome * sin_approx(headingDiff);
float triangleAltitudeToReturnStart = estimatedAltitudeChangeGroundDistance - GPS_distanceToHome * cos_approx(headingDiff);