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

update

Browse source
This commit is contained in:
breadoven 2023-03-26 23:24:26 +01:00
parent f8da237e28
commit 5a454a3400
3 changed files with 18 additions and 35 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1283,17 +1283,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, 0, ROC_TO_ALT_CONSTANT);
} 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) {
@ -1415,21 +1410,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, tmpHomePos->z, ROC_TO_ALT_TARGET);
}
}
else {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
}
return NAV_FSM_EVENT_NONE;
}
@ -1702,12 +1683,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, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
} else {
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_Z);
}
posControl.wpAltitudeReached = isWaypointAltitudeReached();
@ -2887,9 +2863,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
// Terrain following uses different altitude measurement
const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z;
if (mode != ROC_TO_ALT_RESET && desiredClimbRate) { // ROC_TO_ALT_CONSTANT & ROC_TO_ALT_TARGET
if (mode != ROC_TO_ALT_RESET && desiredClimbRate) {
/* ROC_TO_ALT_CONSTANT - constant climb rate always
* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min value 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) {
int8_t direction = desiredClimbRate > 0 ? 1 : -1;
float absClimbRate = fabsf(desiredClimbRate);

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

@ -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 climb
if (isNavHoldPositionActive()) {
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,7 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
if (posControl.flags.estAltStatus >= EST_USABLE) {
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, 0, ROC_TO_ALT_TARGET);
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000, 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));

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

@ -842,7 +842,7 @@ 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, 0, ROC_TO_ALT_TARGET);
updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500, ROC_TO_ALT_TARGET);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}