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

first build

Browse source
This commit is contained in:
breadoven 2023-03-24 13:40:49 +00:00
parent 4fa7e508ed
commit f8da237e28
4 changed files with 38 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1286,7 +1286,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
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);
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);
@ -1396,7 +1396,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);
@ -1424,7 +1424,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
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);
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, tmpHomePos->z, ROC_TO_ALT_TARGET);
}
}
else {
@ -1471,7 +1471,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;
}
@ -1494,7 +1494,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,7 +1704,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
// 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);
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);
}
@ -2786,7 +2786,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;
}
@ -2877,28 +2877,34 @@ 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
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;
}
else { // ROC_TO_ALT_NORMAL
if (mode != ROC_TO_ALT_RESET && desiredClimbRate) { // ROC_TO_ALT_CONSTANT & ROC_TO_ALT_TARGET
/* */
if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) {
int8_t direction = desiredClimbRate > 0 ? 1 : -1;
float absClimbRate = fabsf(desiredClimbRate);
uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? absClimbRate * 5 : absClimbRate;
float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude,
direction * -500.0f, direction * -maxRateCutoffAlt, MIN_TARGET_CLIMB_RATE, absClimbRate);
desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate);
}
/*
* 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;
}
@ -2924,9 +2930,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);
}
lastUpdateTimeUs = currentTimeUs;
} else { // ROC_TO_ALT_RESET or zero desired climbrate
posControl.desiredState.pos.z = altitudeToUse;
}
lastUpdateTimeUs = currentTimeUs;
}
static void resetAltitudeController(bool useTerrainFollowing)