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 // output in Watt
static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float speedToHome) { static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float speedToHome) {
const float timeToHome = distanceToHome / speedToHome; // seconds 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); const float pitchToHome = MIN(RADIANS_TO_DEGREES(atan2_approx(altitudeChangeDescentToHome, distanceToHome)), navConfig()->fw.max_dive_angle);
return estimatePitchPower(pitchToHome) * timeToHome / 3600; return estimatePitchPower(pitchToHome) * timeToHome / 3600;
} }
@ -156,7 +156,7 @@ static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
)) ))
return -1; 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 float RTH_heading; // degrees
#ifdef USE_WIND_ESTIMATOR #ifdef USE_WIND_ESTIMATOR

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

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

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

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