Skip to content

Commit

Permalink
improvements
Browse files Browse the repository at this point in the history
  • Loading branch information
@breadoven
breadoven committed May 3, 2024
1 parent 5126043 commit d40bc0d
Show file tree
Hide file tree
Showing 3 changed files with 23 additions and 27 deletions.
29 changes: 16 additions & 13 deletions src/main/navigation/navigation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1898,16 +1898,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);

// Use linear climb between WPs arriving at WP altitude when within 10% of total distance to WP
// Update climbrate until within 25% of total distance to WP, then hold constant
static float climbRate = 0;
if (posControl.wpDistance > 0.25f * posControl.wpInitialDistance) {
// Update climb rate until within 100cm of total climb xy distance to WP, then hold constant
float climbRate = 0.0f;
if (posControl.wpDistance - 0.1f * posControl.wpInitialDistance > 100.0f) {
climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) /
(posControl.wpDistance - 0.1f * posControl.wpInitialDistance);
}

if (fabsf(climbRate) >= (STATE(AIRPLANE) ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate)) {
climbRate = 0;
}
updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);

if(STATE(MULTIROTOR)) {
Expand Down Expand Up @@ -3401,14 +3397,20 @@ bool isProbablyStillFlying(void)
float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
{
const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();

float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate;

if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) {
if (posControl.flags.isAdjustingAltitude) {
maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate;
}

return constrainf(posControl.desiredState.climbRateDemand, -maxClimbRate, maxClimbRate);
}

if (posControl.desiredState.climbRateDemand) {
maxClimbRate = ABS(posControl.desiredState.climbRateDemand);
maxClimbRate = constrainf(ABS(posControl.desiredState.climbRateDemand), 0.0f, maxClimbRate);
} else if (emergLandingIsActive) {
maxClimbRate = navConfig()->general.emerg_descent_rate;
} else if (posControl.flags.isAdjustingAltitude) {
maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate;
}

const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z;
Expand All @@ -3420,8 +3422,8 @@ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
targetVel = pidProfile()->fwAltControlResponseFactor * targetAltitudeError / 100.0f;
}

if (emergLandingIsActive && targetAltitudeError > -50) {
return -100; // maintain 1 m/s descent during emerg landing when approaching T/O altitude
if (emergLandingIsActive && targetAltitudeError > -50.0f) {
return -100.0f; // maintain 1 m/s descent during emerg landing when within 50cm of min speed landing altitude target
} else {
return constrainf(targetVel, -maxClimbRate, maxClimbRate);
}
Expand All @@ -3439,6 +3441,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt

if (mode == ROC_TO_ALT_CURRENT) {
posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
desiredClimbRate = 0.0f;
} else if (mode == ROC_TO_ALT_TARGET) {
posControl.desiredState.pos.z = targetAltitude;
}
Expand Down
10 changes: 3 additions & 7 deletions src/main/navigation/navigation_fixedwing.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,11 +133,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
{
static pt1Filter_t velzFilterState;

float desiredClimbRate = posControl.desiredState.climbRateDemand;

if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
}
float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);

// Reduce max allowed climb pitch if performing loiter (stall prevention)
if (needToCalculateCircularLoiter && desiredClimbRate > 0.0f) {
Expand Down Expand Up @@ -770,8 +766,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);

if (posControl.flags.estAltStatus >= EST_USABLE) {
// target min descent rate 10m above takeoff altitude
updateClimbRateToAltitudeController(0, 1000.0f, ROC_TO_ALT_TARGET);
// target min descent rate at distance 2 x emerg descent rate above takeoff altitude
updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, 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));
Expand Down
11 changes: 4 additions & 7 deletions src/main/navigation/navigation_multicopter.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,11 +76,7 @@ float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros)
// Position to velocity controller for Z axis
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
{
float targetVel = posControl.desiredState.climbRateDemand;

if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
}
float targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);

posControl.pids.pos[Z].output_constrained = targetVel; // only used for Blackbox and OSD info

Expand Down Expand Up @@ -141,6 +137,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
}
else {
const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500);

if (rcThrottleAdjustment) {
// set velocity proportional to stick movement
float rcClimbRate;
Expand Down Expand Up @@ -928,8 +925,8 @@ 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(0, 500.0f, ROC_TO_ALT_TARGET);
// target min descent rate at distance 2 x emerg descent rate above takeoff altitude
updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_TARGET);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}
Expand Down

0 comments on commit d40bc0d

Please sign in to comment.