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

Refactor and fix RoC/RoD -> altitude controller; Remove surface tracking controller

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-05-13 00:56:47 +10:00
parent 29df2b4f0f
commit ba5c870e94
4 changed files with 45 additions and 67 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -989,12 +989,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
} }
if (navConfig()->general.flags.rth_allow_landing) { 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 // 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) { if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surface < 50.0f) {
// land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // 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. // 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); descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
updateAltitudeTargetFromClimbRate(descentVelLimited, CLIMB_RATE_RESET_SURFACE_TARGET);
} }
else { else {
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. // 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); descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f);
// Do not allow descent velocity slower than -50cm/s so the landing detector works. // 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); descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f);
updateAltitudeTargetFromClimbRate(descentVelLimited, CLIMB_RATE_RESET_SURFACE_TARGET);
} }
updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL);
} }
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
@ -1028,7 +1030,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation
{ {
// Stay in this state // Stay in this state
UNUSED(previousState); 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; return NAV_FSM_EVENT_NONE;
} }
@ -1801,7 +1803,8 @@ void setDesiredPosition(const t_fp_vector * pos, int32_t yaw, navSetWaypointFlag
// Z-position // Z-position
if ((useMask & NAV_POS_UPDATE_Z) != 0) { if ((useMask & NAV_POS_UPDATE_Z) != 0) {
posControl.desiredState.surface = -1; // When we directly set altitude target we must reset surface tracking 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; 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) { else if ((useMask & NAV_POS_UPDATE_BEARING_TAIL_FIRST) != 0) {
posControl.desiredState.yaw = wrap_36000(calculateBearingToDestination(pos) - 18000); 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) void calculateFarAwayTarget(t_fp_vector * farAwayPos, int32_t yaw, int32_t distance)
@ -1862,32 +1858,33 @@ bool isLandingDetected(void)
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Z-position controller * 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 static timeUs_t lastUpdateTimeUs;
// Calculate new altitude target timeUs_t currentTimeUs = micros();
/* Move surface tracking setpoint if it is set */ if (mode == ROC_TO_ALT_RESET) {
if (mode == CLIMB_RATE_RESET_SURFACE_TARGET) { lastUpdateTimeUs = currentTimeUs;
posControl.desiredState.surface = -1; posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z;
} }
else { else {
if (posControl.flags.isTerrainFollowEnabled) { if (STATE(FIXED_WING)) {
if (posControl.flags.hasValidSurfaceSensor && (mode == CLIMB_RATE_UPDATE_SURFACE_TARGET)) { // Fixed wing climb rate controller is open-loop. We simply move the known altitude 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); 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 { 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);
} }
lastUpdateTimeUs = currentTimeUs;
} }
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
} }
static void resetAltitudeController(void) static void resetAltitudeController(void)
@ -1967,11 +1964,6 @@ static bool adjustPositionFromRCInput(void)
retValue = adjustMulticopterPositionFromRCInput(); 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; return retValue;
} }
@ -2303,6 +2295,11 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6); if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6);
if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7); if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7);
if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 8); 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 #endif
} }

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

@ -83,10 +83,14 @@ bool adjustFixedWingAltitudeFromRCInput(void)
if (rcAdjustment) { if (rcAdjustment) {
// set velocity proportional to stick movement // set velocity proportional to stick movement
float rcClimbRate = -rcAdjustment * navConfig()->general.max_manual_climb_rate / (500.0f - rcControlsConfig()->alt_hold_deadband); 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; return true;
} }
else { 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; return false;
} }
} }

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

@ -58,27 +58,6 @@ static int16_t altHoldThrottleRCZero = 1500;
static pt1Filter_t altholdThrottleFilterState; static pt1Filter_t altholdThrottleFilterState;
static bool prepareForTakeoffOnReset = false; 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 // Position to velocity controller for Z axis
static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) 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); 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; return true;
} }
else { else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick // Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) { if (posControl.flags.isAdjustingAltitude) {
updateAltitudeTargetFromClimbRate(0, CLIMB_RATE_UPDATE_SURFACE_TARGET); updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
} }
return false; return false;
@ -188,7 +167,7 @@ void resetMulticopterAltitudeController(void)
if (prepareForTakeoffOnReset) { if (prepareForTakeoffOnReset) {
/* If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump */ /* 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; 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; posControl.pids.vel[Z].integrator = -500.0f;
pt1FilterReset(&altholdThrottleFilterState, -500.0f); pt1FilterReset(&altholdThrottleFilterState, -500.0f);
prepareForTakeoffOnReset = false; prepareForTakeoffOnReset = false;
@ -222,7 +201,6 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
// Check if last correction was too log ago - ignore this update // Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
updateSurfaceTrackingAltitudeSetpoint(deltaMicrosPositionUpdate);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate); updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_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 // Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { 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); updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
} }

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

@ -53,10 +53,9 @@ typedef enum {
} navSetWaypointFlags_t; } navSetWaypointFlags_t;
typedef enum { typedef enum {
CLIMB_RATE_KEEP_SURFACE_TARGET, ROC_TO_ALT_RESET,
CLIMB_RATE_RESET_SURFACE_TARGET, ROC_TO_ALT_NORMAL
CLIMB_RATE_UPDATE_SURFACE_TARGET, } climbRateToAltitudeControllerMode_e;
} navUpdateAltitudeFromRateMode_e;
typedef struct navigationFlags_s { typedef struct navigationFlags_s {
bool horizontalPositionDataNew; bool horizontalPositionDataNew;
@ -300,7 +299,6 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD);
void navPInit(pController_t *p, float _kP); void navPInit(pController_t *p, float _kP);
bool isThrustFacingDownwards(void); bool isThrustFacingDownwards(void);
void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRateMode_e mode);
uint32_t calculateDistanceToDestination(const t_fp_vector * destinationPos); uint32_t calculateDistanceToDestination(const t_fp_vector * destinationPos);
int32_t calculateBearingToDestination(const t_fp_vector * destinationPos); int32_t calculateBearingToDestination(const t_fp_vector * destinationPos);
void resetLandingDetector(void); 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 setDesiredPosition(const t_fp_vector * pos, int32_t yaw, navSetWaypointFlags_t useMask);
void setDesiredSurfaceOffset(float surfaceOffset); void setDesiredSurfaceOffset(float surfaceOffset);
void setDesiredPositionToFarAwayTarget(int32_t yaw, int32_t distance, navSetWaypointFlags_t useMask); 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 isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome);
bool isWaypointMissed(const navWaypointPosition_t * waypoint); bool isWaypointMissed(const navWaypointPosition_t * waypoint);