mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 01:05:21 +03:00
Refactor and fix RoC/RoD -> altitude controller; Remove surface tracking controller
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
29df2b4f0f
commit
ba5c870e94
4 changed files with 45 additions and 67 deletions
|
|
@ -989,12 +989,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
}
|
||||
|
||||
if (navConfig()->general.flags.rth_allow_landing) {
|
||||
float descentVelLimited = 0;
|
||||
|
||||
// A safeguard - if sonar is available and it is reading < 50cm altitude - drop to low descend speed
|
||||
if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surface < 50.0f) {
|
||||
// land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
|
||||
// Do not allow descent velocity slower than -30cm/s so the landing detector works.
|
||||
float descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
|
||||
updateAltitudeTargetFromClimbRate(descentVelLimited, CLIMB_RATE_RESET_SURFACE_TARGET);
|
||||
descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
|
||||
}
|
||||
else {
|
||||
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
|
||||
|
|
@ -1004,9 +1005,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
|
||||
|
||||
// Do not allow descent velocity slower than -50cm/s so the landing detector works.
|
||||
float descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f);
|
||||
updateAltitudeTargetFromClimbRate(descentVelLimited, CLIMB_RATE_RESET_SURFACE_TARGET);
|
||||
descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f);
|
||||
}
|
||||
|
||||
updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL);
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
|
|
@ -1028,7 +1030,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
|
|||
{
|
||||
// Stay in this state
|
||||
UNUSED(previousState);
|
||||
updateAltitudeTargetFromClimbRate(-0.3f * navConfig()->general.land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); // FIXME
|
||||
updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL); // FIXME
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
|
|
@ -1801,6 +1803,7 @@ void setDesiredPosition(const t_fp_vector * 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
|
||||
posControl.desiredState.surface = -1; // When we directly set altitude target we must reset surface tracking
|
||||
posControl.desiredState.pos.V.Z = pos->V.Z;
|
||||
}
|
||||
|
|
@ -1816,13 +1819,6 @@ void setDesiredPosition(const t_fp_vector * pos, int32_t yaw, navSetWaypointFlag
|
|||
else if ((useMask & NAV_POS_UPDATE_BEARING_TAIL_FIRST) != 0) {
|
||||
posControl.desiredState.yaw = wrap_36000(calculateBearingToDestination(pos) - 18000);
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.V.X), -32678, 32767);
|
||||
navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.V.Y), -32678, 32767);
|
||||
navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767);
|
||||
navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
void calculateFarAwayTarget(t_fp_vector * farAwayPos, int32_t yaw, int32_t distance)
|
||||
|
|
@ -1862,32 +1858,33 @@ bool isLandingDetected(void)
|
|||
/*-----------------------------------------------------------
|
||||
* Z-position controller
|
||||
*-----------------------------------------------------------*/
|
||||
void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRateMode_e mode)
|
||||
void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode)
|
||||
{
|
||||
// FIXME: On FIXED_WING and multicopter this should work in a different way
|
||||
// Calculate new altitude target
|
||||
static timeUs_t lastUpdateTimeUs;
|
||||
timeUs_t currentTimeUs = micros();
|
||||
|
||||
/* Move surface tracking setpoint if it is set */
|
||||
if (mode == CLIMB_RATE_RESET_SURFACE_TARGET) {
|
||||
posControl.desiredState.surface = -1;
|
||||
if (mode == ROC_TO_ALT_RESET) {
|
||||
lastUpdateTimeUs = currentTimeUs;
|
||||
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z;
|
||||
}
|
||||
else {
|
||||
if (posControl.flags.isTerrainFollowEnabled) {
|
||||
if (posControl.flags.hasValidSurfaceSensor && (mode == CLIMB_RATE_UPDATE_SURFACE_TARGET)) {
|
||||
posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / (posControl.pids.pos[Z].param.kP * posControl.pids.surface.param.kP)), 1.0f, INAV_SURFACE_MAX_DISTANCE);
|
||||
if (STATE(FIXED_WING)) {
|
||||
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
|
||||
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
|
||||
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
|
||||
posControl.desiredState.pos.V.Z = desiredClimbRate * timeDelta;
|
||||
posControl.desiredState.pos.V.Z = constrainf(posControl.desiredState.pos.V.Z, // FIXME: calculate sanity limits in a smarter way
|
||||
posControl.actualState.pos.V.Z - 500,
|
||||
posControl.actualState.pos.V.Z + 500);
|
||||
}
|
||||
}
|
||||
else {
|
||||
posControl.desiredState.surface = -1;
|
||||
}
|
||||
// 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.V.Z = posControl.actualState.pos.V.Z + (desiredClimbRate / posControl.pids.pos[Z].param.kP);
|
||||
}
|
||||
|
||||
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + (climbRate / posControl.pids.pos[Z].param.kP);
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767);
|
||||
navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
|
||||
#endif
|
||||
lastUpdateTimeUs = currentTimeUs;
|
||||
}
|
||||
}
|
||||
|
||||
static void resetAltitudeController(void)
|
||||
|
|
@ -1967,11 +1964,6 @@ static bool adjustPositionFromRCInput(void)
|
|||
retValue = adjustMulticopterPositionFromRCInput();
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.V.X), -32678, 32767);
|
||||
navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.V.Y), -32678, 32767);
|
||||
#endif
|
||||
|
||||
return retValue;
|
||||
}
|
||||
|
||||
|
|
@ -2303,6 +2295,11 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6);
|
||||
if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7);
|
||||
if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 8);
|
||||
|
||||
navTargetPosition[X] = constrain(lrintf(posControl.desiredState.pos.V.X), -32678, 32767);
|
||||
navTargetPosition[Y] = constrain(lrintf(posControl.desiredState.pos.V.Y), -32678, 32767);
|
||||
navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767);
|
||||
navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -83,10 +83,14 @@ 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);
|
||||
updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_RESET_SURFACE_TARGET);
|
||||
updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
|
||||
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);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -58,27 +58,6 @@ static int16_t altHoldThrottleRCZero = 1500;
|
|||
static pt1Filter_t altholdThrottleFilterState;
|
||||
static bool prepareForTakeoffOnReset = false;
|
||||
|
||||
/* Calculate global altitude setpoint based on surface setpoint */
|
||||
static void updateSurfaceTrackingAltitudeSetpoint(timeDelta_t deltaMicros)
|
||||
{
|
||||
/* If we have a surface offset target and a valid surface offset reading - recalculate altitude target */
|
||||
if (posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface >= 0) {
|
||||
if (posControl.flags.hasValidSurfaceSensor) {
|
||||
// We better overshoot a little bit than undershoot
|
||||
const float targetAltitudeError = navPidApply2(&posControl.pids.surface, posControl.desiredState.surface, posControl.actualState.surface, US2S(deltaMicros), -35.0f, +35.0f, 0);
|
||||
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + targetAltitudeError;
|
||||
}
|
||||
else {
|
||||
// TODO: We are possible above valid range, we now descend down to attempt to get back within range
|
||||
updateAltitudeTargetFromClimbRate(-50.0f, CLIMB_RATE_KEEP_SURFACE_TARGET);
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Position to velocity controller for Z axis
|
||||
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
|
||||
{
|
||||
|
|
@ -137,14 +116,14 @@ bool adjustMulticopterAltitudeFromRCInput(void)
|
|||
rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (altHoldThrottleRCZero - motorConfig()->minthrottle - rcControlsConfig()->alt_hold_deadband);
|
||||
}
|
||||
|
||||
updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_UPDATE_SURFACE_TARGET);
|
||||
updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
|
||||
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
|
||||
if (posControl.flags.isAdjustingAltitude) {
|
||||
updateAltitudeTargetFromClimbRate(0, CLIMB_RATE_UPDATE_SURFACE_TARGET);
|
||||
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
|
||||
}
|
||||
|
||||
return false;
|
||||
|
|
@ -188,7 +167,7 @@ void resetMulticopterAltitudeController(void)
|
|||
if (prepareForTakeoffOnReset) {
|
||||
/* If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump */
|
||||
posControl.desiredState.vel.V.Z = -navConfig()->general.max_manual_climb_rate;
|
||||
updateAltitudeTargetFromClimbRate(-navConfig()->general.max_manual_climb_rate, CLIMB_RATE_UPDATE_SURFACE_TARGET);
|
||||
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
|
||||
posControl.pids.vel[Z].integrator = -500.0f;
|
||||
pt1FilterReset(&altholdThrottleFilterState, -500.0f);
|
||||
prepareForTakeoffOnReset = false;
|
||||
|
|
@ -222,7 +201,6 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
|
|||
|
||||
// Check if last correction was too log ago - ignore this update
|
||||
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
|
||||
updateSurfaceTrackingAltitudeSetpoint(deltaMicrosPositionUpdate);
|
||||
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
|
||||
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
|
||||
}
|
||||
|
|
@ -579,7 +557,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
|
||||
// Check if last correction was too log ago - ignore this update
|
||||
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
|
||||
updateAltitudeTargetFromClimbRate(-1.0f * navConfig()->general.emerg_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET);
|
||||
updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
|
||||
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
|
||||
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -53,10 +53,9 @@ typedef enum {
|
|||
} navSetWaypointFlags_t;
|
||||
|
||||
typedef enum {
|
||||
CLIMB_RATE_KEEP_SURFACE_TARGET,
|
||||
CLIMB_RATE_RESET_SURFACE_TARGET,
|
||||
CLIMB_RATE_UPDATE_SURFACE_TARGET,
|
||||
} navUpdateAltitudeFromRateMode_e;
|
||||
ROC_TO_ALT_RESET,
|
||||
ROC_TO_ALT_NORMAL
|
||||
} climbRateToAltitudeControllerMode_e;
|
||||
|
||||
typedef struct navigationFlags_s {
|
||||
bool horizontalPositionDataNew;
|
||||
|
|
@ -300,7 +299,6 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD);
|
|||
void navPInit(pController_t *p, float _kP);
|
||||
|
||||
bool isThrustFacingDownwards(void);
|
||||
void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRateMode_e mode);
|
||||
uint32_t calculateDistanceToDestination(const t_fp_vector * destinationPos);
|
||||
int32_t calculateBearingToDestination(const t_fp_vector * destinationPos);
|
||||
void resetLandingDetector(void);
|
||||
|
|
@ -312,6 +310,7 @@ void setHomePosition(const t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t
|
|||
void setDesiredPosition(const t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t useMask);
|
||||
void setDesiredSurfaceOffset(float surfaceOffset);
|
||||
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask);
|
||||
void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAltitudeControllerMode_e mode);
|
||||
|
||||
bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome);
|
||||
bool isWaypointMissed(const navWaypointPosition_t * waypoint);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue