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

cleanup + add roc to alt flag

Browse source
This commit is contained in:
breadoven 2023-11-08 22:48:33 +00:00
parent 0246892708
commit 7ae41ad928
4 changed files with 24 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -2927,7 +2927,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
// Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) {
updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, pos->z, ROC_TO_ALT_TARGET);
updateClimbRateToAltitudeController(0, pos->z, ROC_TO_ALT_TARGET);
}
// Heading
@ -3060,11 +3060,12 @@ int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
if (STATE(MULTIROTOR)) {
targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros);
} else {
targetVel = scaleRangef(targetAltitudeError, 0.0f, maxClimbRate * 5, 0.0f, maxClimbRate);
// 0.2f relates to climb rate starting to reduce from maxClimbRate at a distance of 5 x maxClimbRate from targetAltitude
targetVel = 0.2f * targetAltitudeError;
}
if (emergLandingIsActive && targetAltitudeError > -50) {
return -100;
return -100; // maintain 1 m/s descent during emerg landing when approaching T/O altitude
} else {
return constrainf(targetVel, -maxClimbRate, maxClimbRate);
}
@ -3072,11 +3073,14 @@ int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode)
{
/* ROC_TO_ALT_CONSTANT - constant climb rate. desiredClimbRate direction (+ or -) is required
* 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.
* desiredClimbRate direction isn't required, set by target altitude direction instead
* NAV_IMPOSSIBLE_ALTITUDE_TARGET simply uses posControl.desiredState.pos.z to set a constant climb rate */
/* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to 0 when reached.
* Rate reduction starts at distance from target altitude of 5 x climb rate for Fixed wing.
* No climb rate required, set by target altitude direction instead.
*
* ROC_TO_ALT_RESET - similar to ROC_TO_ALT_TARGET except target altitude set to current altitude.
* No climb rate or altitude target required.
*
* ROC_TO_ALT_CONSTANT - constant climb rate. Climb rate and direction required. Target alt not required. */
// Terrain following uses different altitude measurement
const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
@ -3085,10 +3089,10 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
posControl.desiredState.pos.z = altitudeToUse;
} else if (mode == ROC_TO_ALT_TARGET) {
posControl.desiredState.pos.z = targetAltitude;
} else { // ROC_TO_ALT_CONSTANT
posControl.desiredState.pos.z = NAV_IMPOSSIBLE_ALTITUDE_TARGET;
}
posControl.flags.rocToAltMode = mode;
/*
* If max altitude is set and altitude limit reached reset altitude target to max altitude unless desired climb rate is -ve
*/
@ -3096,7 +3100,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
}
posControl.desiredState.vel.z = desiredClimbRate;
posControl.desiredState.vel.z = desiredClimbRate; // only used for ROC_TO_ALT_CONSTANT
}
static void resetAltitudeController(bool useTerrainFollowing)

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

@ -123,7 +123,7 @@ bool adjustFixedWingAltitudeFromRCInput(void)
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
}
return false;
}
@ -136,7 +136,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
float desiredClimbRate = posControl.desiredState.vel.z;
if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) {
if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
}
@ -757,7 +757,7 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
if (posControl.flags.estAltStatus >= EST_USABLE) {
// target min descent rate 10m above takeoff altitude
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000.0f, ROC_TO_ALT_TARGET);
updateClimbRateToAltitudeController(0, 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));

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

@ -78,7 +78,7 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
{
float targetVel = posControl.desiredState.vel.z;
if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) {
if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
}
@ -130,7 +130,7 @@ 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(navConfig()->general.max_manual_climb_rate, altTarget, ROC_TO_ALT_TARGET);
updateClimbRateToAltitudeController(0, altTarget, ROC_TO_ALT_TARGET);
}
else {
updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT);
@ -162,7 +162,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, 0, ROC_TO_ALT_RESET);
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
}
return false;
@ -249,7 +249,6 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
if (prepareForTakeoffOnReset) {
const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity();
posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
posControl.pids.vel[Z].integrator = -500.0f;
pt1FilterReset(&altholdThrottleFilterState, -500.0f);
@ -930,7 +929,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
// Check if last correction was not too long ago
if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
// target min descent rate 5m above takeoff altitude
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500.0f, ROC_TO_ALT_TARGET);
updateClimbRateToAltitudeController(0, 500.0f, ROC_TO_ALT_TARGET);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}

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

@ -47,8 +47,6 @@
#define NAV_RTH_TRACKBACK_POINTS 50 // max number RTH trackback points
#define NAV_IMPOSSIBLE_ALTITUDE_TARGET 10000000 // 100km in cm. Serves as a flag for vertical velocity control
#define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro
_Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!");
@ -95,6 +93,8 @@ typedef struct navigationFlags_s {
navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane
bool gpsCfEstimatedAltitudeMismatch; // Indicates a mismatch between GPS altitude and estimated altitude
climbRateToAltitudeControllerMode_e rocToAltMode;
bool isAdjustingPosition;
bool isAdjustingAltitude;
bool isAdjustingHeading;