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

Merge pull request #4603 from iNavFlight/de_yg_surface_mode_experimental

Browse source 
Add accelerometer weight to position estimation
This commit is contained in:
Konstantin Sharlaimov 2019-05-04 21:26:14 +02:00 • committed by GitHub
commit ff7ca472ca
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 31 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/fc/settings.yaml
View file

@ -1189,19 +1189,19 @@ groups:
- name: inav_w_z_surface_p
field: w_z_surface_p
min: 0
max: 10
max: 100
- name: inav_w_z_surface_v
field: w_z_surface_v
min: 0
max: 10
max: 100
- name: inav_w_xy_flow_p
field: w_xy_flow_p
min: 0
max: 10
max: 100
- name: inav_w_xy_flow_v
field: w_xy_flow_v
min: 0
max: 10
max: 100
- name: inav_w_z_baro_p
field: w_z_baro_p
min: 0
@ -1230,6 +1230,10 @@ groups:
field: w_xy_res_v
min: 0
max: 10
- name: inav_w_xyz_acc_p
field: w_xyz_acc_p
min: 0
max: 1
- name: inav_w_acc_bias
field: w_acc_bias
min: 0

7
src/main/io/rangefinder_benewake.c
View file

@ -123,13 +123,6 @@ static void benewakeRangefinderUpdate(void)
if (sensorData == 0 || qual <= BENEWAKE_MIN_QUALITY) {
sensorData = -1;
}
/*
debug[0] = ((tfminiPacket_t *) &buffer[0])->distL;
debug[1] = ((tfminiPacket_t *) &buffer[0])->distH;
debug[2] = ((tfminiPacket_t *) &buffer[0])->strengthH;
debug[3] = ((tfminiPacket_t *) &buffer[0])->strengthL;
*/
}
// Prepare for new packet

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

@ -2687,6 +2687,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2);
if (posControl.flags.isTerrainFollowEnabled) navFlags |= (1 << 3);
#if defined(NAV_GPS_GLITCH_DETECTION)
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
#endif

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

@ -129,6 +129,7 @@ typedef struct positionEstimationConfig_s {
float w_xy_res_v;
float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
float w_xyz_acc_p;
float max_eph_epv; // Max estimated position error acceptable for estimation (cm)
float baro_epv; // Baro position error

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

@ -53,7 +53,7 @@
navigationPosEstimator_t posEstimator;
PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 3);
PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 4);
PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
// Inertial position estimator parameters
@ -66,6 +66,8 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
.max_surface_altitude = 200,
.w_xyz_acc_p = 1.0f,
.w_z_baro_p = 0.35f,
.w_z_surface_p = 3.500f,
@ -433,6 +435,12 @@ static bool navIsHeadingUsable(void)
}
}
float navGetAccelerometerWeight(void)
{
// TODO(digitalentity): consider accelerometer health in weight calculation
return positionEstimationConfig()->w_xyz_acc_p;
}
static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
{
/* Figure out if we have valid position data from our data sources */
@ -474,11 +482,13 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
static void estimationPredict(estimationContext_t * ctx)
{
const float accWeight = navGetAccelerometerWeight();
/* Prediction step: Z-axis */
if ((ctx->newFlags & EST_Z_VALID)) {
posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
}
/* Prediction step: XY-axis */
@ -489,10 +499,10 @@ static void estimationPredict(estimationContext_t * ctx)
// If heading is valid, accelNEU is valid as well. Account for acceleration
if (navIsHeadingUsable() && navIsAccelerationUsable()) {
posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f;
posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f;
posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt;
posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt;
posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight);
posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(accWeight);
}
}
}

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

@ -149,9 +149,10 @@ void estimationCalculateAGL(estimationContext_t * ctx)
}
// Update estimate
const float accWeight = navGetAccelerometerWeight();
posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt;
posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt;
posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
// Apply correction
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {

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

@ -182,5 +182,5 @@ typedef struct {
extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w);
extern void estimationCalculateAGL(estimationContext_t * ctx);
extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx);
extern float navGetAccelerometerWeight(void);