mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-20 14:55:18 +03:00
Surface estimtion refactoring
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
cf73ab9a39
commit
c52501aade
3 changed files with 41 additions and 47 deletions
|
|
@ -1169,7 +1169,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati
|
|||
|
||||
if (navConfig()->general.flags.rth_allow_landing) {
|
||||
// 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) {
|
||||
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);
|
||||
|
|
@ -1679,13 +1679,25 @@ void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, flo
|
|||
posControl.actualState.surface = surfaceDistance;
|
||||
posControl.actualState.surfaceVel = surfaceVelocity;
|
||||
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
if (surfaceDistance > 0) {
|
||||
if (posControl.actualState.surfaceMin > 0) {
|
||||
posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, surfaceDistance);
|
||||
// Update validity
|
||||
// Update validity
|
||||
if (hasValidSensor) {
|
||||
posControl.flags.hasValidSurfaceSensor = true;
|
||||
posControl.flags.surfaceDistanceDataNew = 1;
|
||||
}
|
||||
else {
|
||||
posControl.actualState.surfaceMin = surfaceDistance;
|
||||
posControl.flags.hasValidSurfaceSensor = false;
|
||||
posControl.flags.surfaceDistanceDataNew = 0;
|
||||
}
|
||||
|
||||
// Update minimum surface distance (landing detection threshold)
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
if (hasValidSensor && posControl.actualState.surface > 0) {
|
||||
if (posControl.actualState.surfaceMin > 0) {
|
||||
posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, posControl.actualState.surface);
|
||||
}
|
||||
else {
|
||||
posControl.actualState.surfaceMin = posControl.actualState.surface;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1693,15 +1705,6 @@ void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, flo
|
|||
posControl.actualState.surfaceMin = -1;
|
||||
}
|
||||
|
||||
posControl.flags.hasValidSurfaceSensor = hasValidSensor;
|
||||
|
||||
if (hasValidSensor) {
|
||||
posControl.flags.surfaceDistanceDataNew = 1;
|
||||
}
|
||||
else {
|
||||
posControl.flags.surfaceDistanceDataNew = 0;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navActualSurface = surfaceDistance;
|
||||
#endif
|
||||
|
|
@ -2049,7 +2052,7 @@ void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRat
|
|||
}
|
||||
else {
|
||||
if (posControl.flags.isTerrainFollowEnabled) {
|
||||
if (posControl.actualState.surface >= 0.0f && posControl.flags.hasValidSurfaceSensor && (mode == CLIMB_RATE_UPDATE_SURFACE_TARGET)) {
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -63,7 +63,7 @@ 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.actualState.surface >= 0 && posControl.flags.hasValidSurfaceSensor) {
|
||||
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;
|
||||
|
|
@ -524,7 +524,7 @@ bool isMulticopterLandingDetected(void)
|
|||
navDebug[2] = (currentTimeUs - landingTimer) / 1000;
|
||||
|
||||
// If we have surface sensor (for example sonar) - use it to detect touchdown
|
||||
if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surface >= 0 && posControl.actualState.surfaceMin >= 0) {
|
||||
if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surfaceMin >= 0) {
|
||||
// TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety.
|
||||
// TODO: Out of range sonar may give reading that looks like we landed, find a way to check if sonar is healthy.
|
||||
// surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed
|
||||
|
|
|
|||
|
|
@ -113,17 +113,19 @@ typedef struct {
|
|||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
float alt; // Raw altitude measurement (cm)
|
||||
float vel;
|
||||
} navPositionEstimatorSONAR_t;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
/* 3D position, velocity and confidence */
|
||||
t_fp_vector pos;
|
||||
t_fp_vector vel;
|
||||
float surface;
|
||||
float surfaceVel;
|
||||
float eph;
|
||||
float epv;
|
||||
/* Surface offset */
|
||||
float surface;
|
||||
float surfaceVel;
|
||||
bool surfaceValid;
|
||||
} navPositionEstimatorESTIMATE_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -179,8 +181,8 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
|
|||
|
||||
.w_z_baro_p = 0.35f,
|
||||
|
||||
.w_z_sonar_p = 3.000f,
|
||||
.w_z_sonar_v = 4.700f,
|
||||
.w_z_sonar_p = 3.500f,
|
||||
.w_z_sonar_v = 6.100f,
|
||||
|
||||
.w_z_gps_p = 0.2f,
|
||||
.w_z_gps_v = 0.5f,
|
||||
|
|
@ -482,23 +484,8 @@ void updatePositionEstimator_SonarTopic(timeUs_t currentTimeUs)
|
|||
|
||||
/* Apply predictive filter to sonar readings */
|
||||
if (newSonarAlt > 0 && newSonarAlt <= positionEstimationConfig()->max_sonar_altitude) {
|
||||
float sonarDt = (currentTimeUs - posEstimator.sonar.lastUpdateTime) * 1e-6;
|
||||
posEstimator.sonar.lastUpdateTime = currentTimeUs;
|
||||
|
||||
if (sonarDt > 0.25f) {
|
||||
posEstimator.sonar.vel = 0.0f;
|
||||
posEstimator.sonar.alt = newSonarAlt;
|
||||
}
|
||||
else {
|
||||
/* Predictive stage */
|
||||
posEstimator.sonar.alt = posEstimator.sonar.alt + posEstimator.sonar.vel * sonarDt;
|
||||
|
||||
const float sonarResidual = newSonarAlt - posEstimator.sonar.alt;
|
||||
const float bellCurveScaler = scaleRangef(bellCurve(sonarResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
|
||||
posEstimator.sonar.alt += sonarResidual * positionEstimationConfig()->w_z_sonar_p * bellCurveScaler * sonarDt;
|
||||
posEstimator.sonar.vel += sonarResidual * positionEstimationConfig()->w_z_sonar_v * sq(bellCurveScaler) * sonarDt;
|
||||
}
|
||||
posEstimator.sonar.lastUpdateTime = currentTimeUs;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -806,17 +793,26 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
|
||||
/* Surface offset */
|
||||
#if defined(SONAR)
|
||||
posEstimator.est.surface = posEstimator.est.surface + posEstimator.est.surfaceVel * dt;
|
||||
|
||||
if (isSonarValid) {
|
||||
posEstimator.est.surface = posEstimator.sonar.alt;
|
||||
posEstimator.est.surfaceVel = posEstimator.sonar.vel;
|
||||
const float sonarResidual = posEstimator.sonar.alt - posEstimator.est.surface;
|
||||
const float bellCurveScaler = scaleRangef(bellCurve(sonarResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
|
||||
posEstimator.est.surface += sonarResidual * positionEstimationConfig()->w_z_sonar_p * bellCurveScaler * dt;
|
||||
posEstimator.est.surfaceVel += sonarResidual * positionEstimationConfig()->w_z_sonar_v * sq(bellCurveScaler) * dt;
|
||||
}
|
||||
else {
|
||||
posEstimator.est.surface = -1;
|
||||
posEstimator.est.surfaceVel = 0;
|
||||
posEstimator.est.surfaceVel = 0; // Zero out velocity to prevent estimate to drift away
|
||||
posEstimator.est.surfaceValid = true;
|
||||
}
|
||||
|
||||
debug[0] = posEstimator.est.surface;
|
||||
debug[1] = posEstimator.est.surfaceVel;
|
||||
#else
|
||||
posEstimator.est.surface = -1;
|
||||
posEstimator.est.surfaceVel = 0;
|
||||
posEstimator.est.surfaceValid = false;
|
||||
#endif
|
||||
|
||||
/* Update uncertainty */
|
||||
|
|
@ -854,12 +850,7 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
/* Publish surface distance */
|
||||
if (posEstimator.est.surface > 0) {
|
||||
updateActualSurfaceDistance(true, posEstimator.est.surface, posEstimator.est.surfaceVel);
|
||||
}
|
||||
else {
|
||||
updateActualSurfaceDistance(false, -1, 0);
|
||||
}
|
||||
updateActualSurfaceDistance(posEstimator.est.surfaceValid, posEstimator.est.surface, posEstimator.est.surfaceVel);
|
||||
|
||||
/* Store history data */
|
||||
posEstimator.history.pos[posEstimator.history.index] = posEstimator.est.pos;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue