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

Merge pull request #8920 from breadoven/abo_climb_rate_controller_improvement

Browse source 
Nav altitude control improvements
This commit is contained in:
breadoven 2023-08-07 12:21:57 +01:00 • committed by GitHub
commit 5f1a10ad74
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 51 additions and 57 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1281,17 +1281,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
}
// Climb to safe altitude and turn to correct direction
if (STATE(FIXED_WING_LEGACY)) {
if (navConfig()->general.flags.rth_climb_first == RTH_CLIMB_ON_FW_SPIRAL) {
float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
} else {
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
}
} else {
// Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
// it in a reasonable time. Immediately after we finish this phase - target the original altitude.
if (STATE(FIXED_WING_LEGACY)) {
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
} else {
tmpHomePos->z += MR_RTH_CLIMB_OVERSHOOT_CM;
if (navConfig()->general.flags.rth_tail_first) {
@ -1403,7 +1398,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
if ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
resetLandingDetector(); // force reset landing detector just in case
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
} else {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL);
@ -1422,21 +1417,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
}
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
if (navConfig()->general.rth_home_altitude) {
float timeToReachHomeAltitude = fabsf(tmpHomePos->z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate;
if (timeToReachHomeAltitude < 1) {
// we almost reached the target home altitude so set the desired altitude to the desired home altitude
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
} else {
float altitudeChangeDirection = tmpHomePos->z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
}
}
else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
}
return NAV_FSM_EVENT_NONE;
}
@ -1478,7 +1459,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd);
}
updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL);
updateClimbRateToAltitudeController(-descentVelLimited, 0, ROC_TO_ALT_CONSTANT);
return NAV_FSM_EVENT_NONE;
}
@ -1501,7 +1482,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
UNUSED(previousState);
if (STATE(ALTITUDE_CONTROL)) {
updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL); // FIXME
updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, 0, ROC_TO_ALT_CONSTANT); // FIXME
}
// Prevent I-terms growing when already landed
@ -1704,12 +1685,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
// Adjust altitude to waypoint setting
if (STATE(AIRPLANE)) {
int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
} else {
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z);
}
posControl.wpAltitudeReached = isWaypointAltitudeReached();
@ -2788,7 +2764,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
// Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) {
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller
posControl.desiredState.pos.z = pos->z;
}
@ -2879,28 +2855,36 @@ bool isFlightDetected(void)
/*-----------------------------------------------------------
* Z-position controller
*-----------------------------------------------------------*/
void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode)
void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
{
#define MIN_TARGET_CLIMB_RATE 100.0f // cm/s
static timeUs_t lastUpdateTimeUs;
timeUs_t currentTimeUs = micros();
// Terrain following uses different altitude measurement
const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
if (mode == ROC_TO_ALT_RESET) {
lastUpdateTimeUs = currentTimeUs;
posControl.desiredState.pos.z = altitudeToUse;
if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {
/* ROC_TO_ALT_CONSTANT - constant climb rate
* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached
* Rate reduction starts at distance from target altitude of 5 x climb rate for FW, 1 x climb rate for MC */
if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
const int8_t direction = desiredClimbRate > 0 ? 1 : -1;
const float absClimbRate = fabsf(desiredClimbRate);
const uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate);
desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
}
else { // ROC_TO_ALT_NORMAL
/*
* If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
* In other words, when altitude is reached, allow it only to shrink
*/
if (navConfig()->general.max_altitude > 0 &&
altitudeToUse >= navConfig()->general.max_altitude &&
desiredClimbRate > 0
) {
if (navConfig()->general.max_altitude > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) {
desiredClimbRate = 0;
}
@ -2926,9 +2910,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
// Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD
posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
}
} else { // ROC_TO_ALT_RESET or zero desired climbrate
posControl.desiredState.pos.z = altitudeToUse;
}
lastUpdateTimeUs = currentTimeUs;
}
}
static void resetAltitudeController(bool useTerrainFollowing)

14
src/main/navigation/navigation_fixedwing.c
View file

@ -117,13 +117,13 @@ bool adjustFixedWingAltitudeFromRCInput(void)
if (rcAdjustment) {
// set velocity proportional to stick movement
float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband);
updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
return true;
}
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
}
return false;
}
@ -157,9 +157,14 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
const float pitchGainInv = 1.0f / 1.0f;
// Here we use negative values for dive for better clarity
const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
// Reduce max allowed climb pitch if performing loiter (stall prevention)
if (needToCalculateCircularLoiter) {
maxClimbDeciDeg = maxClimbDeciDeg * 0.67f;
}
// PID controller to translate energy balance error [J] into pitch angle [decideg]
float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f);
@ -760,7 +765,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) {
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
// target min descent rate 10m above takeoff altitude
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000.0f, ROC_TO_ALT_TARGET);
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));

11
src/main/navigation/navigation_multicopter.c
View file

@ -133,11 +133,11 @@ bool adjustMulticopterAltitudeFromRCInput(void)
// In terrain follow mode we apply different logic for terrain control
if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) {
// We have solid terrain sensor signal - directly map throttle to altitude
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
posControl.desiredState.pos.z = altTarget;
}
else {
updateClimbRateToAltitudeController(-50.0f, ROC_TO_ALT_NORMAL);
updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT);
}
// In surface tracking we always indicate that we're adjusting altitude
@ -159,14 +159,14 @@ bool adjustMulticopterAltitudeFromRCInput(void)
rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband);
}
updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT);
return true;
}
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
}
return false;
@ -889,7 +889,8 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
// Check if last correction was not too long ago
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
// target min descent rate 5m above takeoff altitude
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500.0f, ROC_TO_ALT_TARGET);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}

5
src/main/navigation/navigation_private.h
View file

@ -61,7 +61,8 @@ typedef enum {
typedef enum {
ROC_TO_ALT_RESET,
ROC_TO_ALT_NORMAL
ROC_TO_ALT_CONSTANT,
ROC_TO_ALT_TARGET
} climbRateToAltitudeControllerMode_e;
typedef enum {
@ -447,7 +448,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFla
void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlags_t useMask);
void setDesiredSurfaceOffset(float surfaceOffset);
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); // NOT USED
void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode);
void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode);
bool isNavHoldPositionActive(void);
bool isLastMissionWaypoint(void);