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

Surface estimation tweaks

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-03-08 22:07:26 +10:00
parent 30bdeae47d
commit cf73ab9a39
7 changed files with 21 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/common/maths.c
View file

@ -187,7 +187,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
return ((a / b) - (destMax - destMin)) + destMax;
}
int scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) {
float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) {
float a = (destMax - destMin) * (x - srcMin);
float b = srcMax - srcMin;
return ((a / b) - (destMax - destMin)) + destMax;

2
src/main/common/maths.h
View file

@ -144,7 +144,7 @@ float devStandardDeviation(stdev_t *dev);
float degreesToRadians(int16_t degrees);
int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
int scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax);
float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax);
void normalizeV(struct fp_vector *src, struct fp_vector *dest);

4
src/main/fc/cli.c
View file

@ -783,6 +783,10 @@ static const clivalue_t valueTable[] = {
{ "inav_gps_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, gps_delay_ms) },
{ "inav_reset_altitude", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_NAV_RESET_ALTITUDE }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, use_gps_velned) },
{ "inav_max_sonar_altitude", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, max_sonar_altitude) },
{ "inav_w_z_sonar_p", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_sonar_p) },
{ "inav_w_z_sonar_v", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_sonar_v) },
{ "inav_w_z_baro_p", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_baro_p) },
{ "inav_w_z_gps_p", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_gps_p) },
{ "inav_w_z_gps_v", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_gps_v) },

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

@ -2728,7 +2728,7 @@ void navigationUsePIDs(void)
// Initialize surface tracking PID
navPidInit(&posControl.pids.surface, 2.0f,
1.0f,
0.0f,
0.0f);
/** Airplane PIDs */

2
src/main/navigation/navigation.h
View file

@ -72,6 +72,8 @@ typedef struct positionEstimationConfig_s {
uint8_t use_gps_velned;
uint16_t gps_delay_ms;
uint16_t max_sonar_altitude;
float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements
float w_z_sonar_p; // Weight (cutoff frequency) for sonar altitude measurements

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

@ -65,13 +65,12 @@ static void updateSurfaceTrackingAltitudeSetpoint(timeDelta_t deltaMicros)
if (posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface >= 0) {
if (posControl.actualState.surface >= 0 && 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), -5.0f, +35.0f, 0);
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(-0.10f * navConfig()->emerg_descent_rate, CLIMB_RATE_KEEP_SURFACE_TARGET);
updateAltitudeTargetFromClimbRate(-20.0f, CLIMB_RATE_KEEP_SURFACE_TARGET);
updateAltitudeTargetFromClimbRate(-50.0f, CLIMB_RATE_KEEP_SURFACE_TARGET);
}
}

20
src/main/navigation/navigation_pos_estimator.c
View file

@ -80,9 +80,6 @@
#define INAV_BARO_TIMEOUT_MS 200 // Baro timeout
#define INAV_SONAR_TIMEOUT_MS 300 // Sonar timeout (missed 3 readings in a row)
#define INAV_SONAR_W1 3.000f
#define INAV_SONAR_W2 4.700f
#define INAV_HISTORY_BUF_SIZE (INAV_POSITION_PUBLISH_RATE_HZ / 2) // Enough to hold 0.5 sec historical data
typedef struct {
@ -168,7 +165,7 @@ typedef struct {
static navigationPosEstimator_s posEstimator;
PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 1);
PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
// Inertial position estimator parameters
@ -178,8 +175,13 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
.accz_unarmed_cal = 1,
.use_gps_velned = 1, // "Disabled" is mandatory with gps_dyn_model = Pedestrian
.max_sonar_altitude = 200,
.w_z_baro_p = 0.35f,
.w_z_sonar_p = 3.000f,
.w_z_sonar_v = 4.700f,
.w_z_gps_p = 0.2f,
.w_z_gps_v = 0.5f,
@ -479,7 +481,7 @@ void updatePositionEstimator_SonarTopic(timeUs_t currentTimeUs)
newSonarAlt = rangefinderCalculateAltitude(newSonarAlt, calculateCosTiltAngle());
/* Apply predictive filter to sonar readings */
if (newSonarAlt > 0 && newSonarAlt <= INAV_SONAR_MAX_DISTANCE) {
if (newSonarAlt > 0 && newSonarAlt <= positionEstimationConfig()->max_sonar_altitude) {
float sonarDt = (currentTimeUs - posEstimator.sonar.lastUpdateTime) * 1e-6;
posEstimator.sonar.lastUpdateTime = currentTimeUs;
@ -492,14 +494,12 @@ void updatePositionEstimator_SonarTopic(timeUs_t currentTimeUs)
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 * INAV_SONAR_W1 * sonarDt;
posEstimator.sonar.vel += sonarResidual * INAV_SONAR_W2 * sonarDt;
posEstimator.sonar.alt += sonarResidual * positionEstimationConfig()->w_z_sonar_p * bellCurveScaler * sonarDt;
posEstimator.sonar.vel += sonarResidual * positionEstimationConfig()->w_z_sonar_v * sq(bellCurveScaler) * sonarDt;
}
}
debug[0] = posEstimator.sonar.alt;
debug[1] = posEstimator.sonar.vel;
}
#endif