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

[RTH] Refactor linear RTH descent; Fix initial altitude bug

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2019-06-01 19:42:06 +02:00
parent 929bbd6314
commit 76ecb84bd4
3 changed files with 23 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -138,7 +138,7 @@ static float estimateRTHDistanceAndHeadingAfterAltitudeChange(float altitudeChan
// output in Watt
static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float speedToHome) {
const float timeToHome = distanceToHome / speedToHome; // seconds
const float altitudeChangeDescentToHome = CENTIMETERS_TO_METERS(navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT ? MAX(0, getEstimatedActualPosition(Z) - RTHAltitude()) : 0);
const float altitudeChangeDescentToHome = CENTIMETERS_TO_METERS(navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT ? MAX(0, getEstimatedActualPosition(Z) - calculateRTHAltitude()) : 0);
const float pitchToHome = MIN(RADIANS_TO_DEGREES(atan2_approx(altitudeChangeDescentToHome, distanceToHome)), navConfig()->fw.max_dive_angle);
return estimatePitchPower(pitchToHome) * timeToHome / 3600;
}
@ -156,7 +156,7 @@ static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
))
return -1;
const float RTH_initial_altitude_change = MAX(0, (RTHAltitude() - getEstimatedActualPosition(Z)) / 100);
const float RTH_initial_altitude_change = MAX(0, (calculateRTHAltitude() - getEstimatedActualPosition(Z)) / 100);
float RTH_heading; // degrees
#ifdef USE_WIND_ESTIMATOR

26
src/main/navigation/navigation.c
View file

@ -1190,15 +1190,24 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
}
else {
if (navConfig()->general.flags.rth_alt_control_mode == NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT) {
// Calculate required travel and actual travel distance
float rthTotalDistanceToTravel = posControl.rthInitialHomeDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0);
float rthTravelledDistance = MIN((posControl.rthInitialHomeDistance - posControl.homeDistance), rthTotalDistanceToTravel);
// At this point `posControl.homeWaypointAbove.pos.z` is initial altitude at RTH start
// while (posControl.homePosition.pos.z + navConfig()->general.rth_altitude) is a target altitude above home point
// We will scale gradually between the two, but only if rthTotalDistanceToTravel is more than 1m (otherwise probably no point)
if (rthTotalDistanceToTravel >= 100) {
fpVector3_t pos;
uint16_t loiterDistanceFromHome = STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0;
uint32_t distanceToLoiterToTravelFromRTHStart = posControl.rthInitialHomeDistance - loiterDistanceFromHome;
uint32_t distanceToLoiterTraveled = constrain((int32_t)posControl.rthInitialHomeDistance - posControl.homeDistance, 0, distanceToLoiterToTravelFromRTHStart);
float RTHStartAltitude = posControl.homeWaypointAbove.pos.z;
float RTHFinalAltitude = posControl.homePosition.pos.z + navConfig()->general.rth_altitude;
pos.z = RTHStartAltitude - scaleRange(distanceToLoiterTraveled, 0, distanceToLoiterToTravelFromRTHStart, 0, RTHStartAltitude - RTHFinalAltitude);
float ratioTravelled = rthTravelledDistance / rthTotalDistanceToTravel;
pos.z = posControl.homeWaypointAbove.pos.z -
(posControl.homeWaypointAbove.pos.z - (posControl.homePosition.pos.z + navConfig()->general.rth_altitude)) * ratioTravelled;
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z);
}
}
return NAV_FSM_EVENT_NONE;
}
@ -2006,7 +2015,7 @@ static void updateHomePositionCompatibility(void)
GPS_directionToHome = posControl.homeDirection / 100;
}
float RTHAltitude() {
float calculateRTHAltitude(void) {
switch (navConfig()->general.flags.rth_alt_control_mode) {
case NAV_RTH_NO_ALT:
return(posControl.actualState.abs.pos.z);
@ -2015,6 +2024,7 @@ float RTHAltitude() {
case NAV_RTH_MAX_ALT:
return(MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z));
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
case NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT:
return(MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z));
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
default:
@ -2029,7 +2039,7 @@ static void updateDesiredRTHAltitude(void)
{
if (ARMING_FLAG(ARMED)) {
if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
posControl.homeWaypointAbove.pos.z = RTHAltitude();
posControl.homeWaypointAbove.pos.z = calculateRTHAltitude();
}
}
else {

2
src/main/navigation/navigation.h
View file

@ -416,7 +416,7 @@ void resetWaypointList(void);
bool loadNonVolatileWaypointList(void);
bool saveNonVolatileWaypointList(void);
float RTHAltitude(void);
float calculateRTHAltitude(void);
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch);
/* Geodetic functions */