From d528005626791341d6a452547762e9aa75e8acb2 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Mon, 28 Mar 2016 22:42:14 +1000 Subject: [PATCH] SURFACE: Improved surface offset handling logic --- src/main/flight/navigation_rewrite.c | 29 ++++++++++++++---------- src/main/flight/navigation_rewrite_fixedwing.c | 2 +- src/main/flight/navigation_rewrite_multicopter.c | 22 +++++++++--------- src/main/flight/navigation_rewrite_private.h | 8 ++++++- 4 files changed, 36 insertions(+), 25 deletions(-) diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 93ee97b4a..f23383a71 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -927,18 +927,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati // A safeguard - if sonar is available and it is reading < 50cm altitude - drop to low descend speed if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surface >= 0 && posControl.actualState.surface < 50.0f) { // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown - updateAltitudeTargetFromClimbRate(-0.15f * posControl.navConfig->land_descent_rate); + updateAltitudeTargetFromClimbRate(-0.15f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); } else { // Gradually reduce descent speed depending on actual altitude. if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 1500.0f)) { - updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->land_descent_rate); + updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); } else if (posControl.actualState.pos.V.Z > (posControl.homePosition.pos.V.Z + 500.0f)) { - updateAltitudeTargetFromClimbRate(-0.5f * posControl.navConfig->land_descent_rate); + updateAltitudeTargetFromClimbRate(-0.5f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); } else { - updateAltitudeTargetFromClimbRate(-0.25f * posControl.navConfig->land_descent_rate); + updateAltitudeTargetFromClimbRate(-0.25f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); } } @@ -949,7 +949,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState) { UNUSED(previousState); - updateAltitudeTargetFromClimbRate(-0.3f * posControl.navConfig->land_descent_rate); // FIXME + updateAltitudeTargetFromClimbRate(-0.3f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); // FIXME return NAV_FSM_EVENT_SUCCESS; } @@ -957,7 +957,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHED(navigat { // Stay in this state UNUSED(previousState); - updateAltitudeTargetFromClimbRate(-0.3f * posControl.navConfig->land_descent_rate); // FIXME + updateAltitudeTargetFromClimbRate(-0.3f * posControl.navConfig->land_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); // FIXME return NAV_FSM_EVENT_NONE; } @@ -1593,19 +1593,24 @@ bool isLandingDetected(void) /*----------------------------------------------------------- * Z-position controller *-----------------------------------------------------------*/ -void updateAltitudeTargetFromClimbRate(float climbRate) +void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRateMode_e mode) { // FIXME: On FIXED_WING and multicopter this should work in a different way // Calculate new altitude target /* Move surface tracking setpoint if it is set */ - if (posControl.flags.isTerrainFollowEnabled) { - if (posControl.actualState.surface >= 0.0f && posControl.flags.hasValidSurfaceSensor) { - posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / posControl.pids.pos[Z].param.kP), 1.0f, INAV_SONAR_MAX_DISTANCE); - } + if (mode == CLIMB_RATE_RESET_SURFACE_TARGET) { + posControl.desiredState.surface = -1; } else { - posControl.desiredState.surface = -1; + if (posControl.flags.isTerrainFollowEnabled) { + if (posControl.actualState.surface >= 0.0f && posControl.flags.hasValidSurfaceSensor && (mode == CLIMB_RATE_UPDATE_SURFACE_TARGET)) { + posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / posControl.pids.pos[Z].param.kP), 1.0f, INAV_SONAR_MAX_DISTANCE); + } + } + else { + posControl.desiredState.surface = -1; + } } posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + (climbRate / posControl.pids.pos[Z].param.kP); diff --git a/src/main/flight/navigation_rewrite_fixedwing.c b/src/main/flight/navigation_rewrite_fixedwing.c index c2ee44727..78cb04cbc 100755 --- a/src/main/flight/navigation_rewrite_fixedwing.c +++ b/src/main/flight/navigation_rewrite_fixedwing.c @@ -75,7 +75,7 @@ bool adjustFixedWingAltitudeFromRCInput(void) if (rcAdjustment) { // set velocity proportional to stick movement float rcClimbRate = -rcAdjustment * posControl.navConfig->max_manual_climb_rate / (500.0f - posControl.rcControlsConfig->alt_hold_deadband); - updateAltitudeTargetFromClimbRate(rcClimbRate); + updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_RESET_SURFACE_TARGET); return true; } else { diff --git a/src/main/flight/navigation_rewrite_multicopter.c b/src/main/flight/navigation_rewrite_multicopter.c index cfdf925da..5aa3442e9 100755 --- a/src/main/flight/navigation_rewrite_multicopter.c +++ b/src/main/flight/navigation_rewrite_multicopter.c @@ -62,23 +62,23 @@ static int16_t rcCommandAdjustedThrottle; static int16_t altHoldThrottleRCZero = 1500; static filterStatePt1_t altholdThrottleFilterState; -#define SURFACE_TRACKING_GAIN 5.0f +#define SURFACE_TRACKING_GAIN 25.0f /* Calculate global altitude setpoint based on surface setpoint */ static void updateSurfaceTrackingAltitudeSetpoint_MC(uint32_t deltaMicros) { /* If we have a surface offset target and a valid surface offset reading - recalculate altitude target */ - if (posControl.flags.isTerrainFollowEnabled) { - if (posControl.desiredState.surface > 0 && (posControl.actualState.surface > 0 && posControl.flags.hasValidSurfaceSensor)) { + if (posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface >= 0) { + if (posControl.actualState.surface >= 0 && posControl.flags.hasValidSurfaceSensor) { float surfaceOffsetError = posControl.desiredState.surface - posControl.actualState.surface; - float altitudeError = posControl.desiredState.pos.V.Z - posControl.actualState.pos.V.Z; float trackingWeight = SURFACE_TRACKING_GAIN * US2S(deltaMicros); - posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + surfaceOffsetError * trackingWeight + altitudeError * (1 - trackingWeight); + posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + surfaceOffsetError * trackingWeight; + } + else { + // TODO: We are possible above valid range, should we descend down to attempt to get back within range? + updateAltitudeTargetFromClimbRate(-0.5f * posControl.navConfig->emerg_descent_rate, CLIMB_RATE_KEEP_SURFACE_TARGET); } - } - else { - // TODO: We are possible above valid range, should we descend down to attempt to get back within range? } } @@ -129,14 +129,14 @@ bool adjustMulticopterAltitudeFromRCInput(void) rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (altHoldThrottleRCZero - posControl.escAndServoConfig->minthrottle); } - updateAltitudeTargetFromClimbRate(rcClimbRate); + updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_UPDATE_SURFACE_TARGET); return true; } else { // Adjusting finished - reset desired position to stay exactly where pilot released the stick if (posControl.flags.isAdjustingAltitude) { - updateAltitudeTargetFromClimbRate(0); + updateAltitudeTargetFromClimbRate(0, CLIMB_RATE_UPDATE_SURFACE_TARGET); } return false; @@ -524,7 +524,7 @@ static void applyMulticopterEmergencyLandingController(uint32_t currentTime) // Check if last correction was too log ago - ignore this update if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { - updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->emerg_descent_rate); + updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->emerg_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET); updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); } diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index 8089d7855..85582aa80 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/src/main/flight/navigation_rewrite_private.h @@ -47,6 +47,12 @@ typedef enum { NAV_POS_UPDATE_BEARING_TAIL_FIRST = 1 << 4, } navSetWaypointFlags_t; +typedef enum { + CLIMB_RATE_KEEP_SURFACE_TARGET, + CLIMB_RATE_RESET_SURFACE_TARGET, + CLIMB_RATE_UPDATE_SURFACE_TARGET, +} navUpdateAltitudeFromRateMode_e; + typedef struct navigationFlags_s { bool horizontalPositionNewData; bool verticalPositionNewData; @@ -275,7 +281,7 @@ 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); +void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRateMode_e mode); uint32_t calculateDistanceToDestination(t_fp_vector * destinationPos); int32_t calculateBearingToDestination(t_fp_vector * destinationPos); void resetLandingDetector(void); -- 2.11.4.GIT