From 57f0e32bfc1793e861a59c7df78a489129d55a41 Mon Sep 17 00:00:00 2001 From: Konstantin Sharlaimov Date: Sun, 24 Jul 2016 15:13:49 +0300 Subject: [PATCH] Use GPS for vertical velocity estimation on multirotors (#346) --- src/main/config/config.c | 2 +- src/main/flight/navigation_rewrite_pos_estimator.c | 23 +++++++++++++++------- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 145d672fa..05375056f 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -252,7 +252,7 @@ void resetNavConfig(navConfig_t * navConfig) navConfig->inav.w_z_baro_p = 0.35f; navConfig->inav.w_z_gps_p = 0.2f; - navConfig->inav.w_z_gps_v = 0.2f; + navConfig->inav.w_z_gps_v = 0.5f; navConfig->inav.w_xy_gps_p = 1.0f; navConfig->inav.w_xy_gps_v = 2.0f; diff --git a/src/main/flight/navigation_rewrite_pos_estimator.c b/src/main/flight/navigation_rewrite_pos_estimator.c index 962d32512..e66773ae6 100755 --- a/src/main/flight/navigation_rewrite_pos_estimator.c +++ b/src/main/flight/navigation_rewrite_pos_estimator.c @@ -517,7 +517,10 @@ static void updateEstimatedTopic(uint32_t currentTime) #endif /* Apply GPS altitude corrections only on fixed wing aircrafts */ - bool useGpsZ = STATE(FIXED_WING) && isGPSValid; + bool useGpsZPos = STATE(FIXED_WING) && isGPSValid; + + /* GPS correction for velocity might be used on all aircrafts */ + bool useGpsZVel = isGPSValid; /* Pre-calculate history index for GPS delay compensation */ int gpsHistoryIndex = (posEstimator.history.index - 1) - constrain(((int)posControl.navConfig->inav.gps_delay_ms / (1000 / INAV_POSITION_PUBLISH_RATE_HZ)), 0, INAV_HISTORY_BUF_SIZE - 1); @@ -538,10 +541,14 @@ static void updateEstimatedTopic(uint32_t currentTime) accelBiasCorr.V.Y -= (posEstimator.gps.pos.V.Y - posEstimator.history.pos[gpsHistoryIndex].V.Y) * sq(posControl.navConfig->inav.w_xy_gps_p); accelBiasCorr.V.Y -= (posEstimator.gps.vel.V.Y - posEstimator.history.vel[gpsHistoryIndex].V.Y) * posControl.navConfig->inav.w_xy_gps_v; - if (useGpsZ) { + if (useGpsZPos) { accelBiasCorr.V.Z -= (posEstimator.gps.pos.V.Z - posEstimator.history.pos[gpsHistoryIndex].V.Z) * sq(posControl.navConfig->inav.w_z_gps_p); + } + + if (useGpsZVel) { accelBiasCorr.V.Z -= (posEstimator.gps.vel.V.Z - posEstimator.history.vel[gpsHistoryIndex].V.Z) * posControl.navConfig->inav.w_z_gps_v; } + } /* accelerometer bias correction for baro */ @@ -559,7 +566,7 @@ static void updateEstimatedTopic(uint32_t currentTime) } /* Estimate Z-axis */ - if ((posEstimator.est.epv < posControl.navConfig->inav.max_eph_epv) || useGpsZ || isBaroValid) { + if ((posEstimator.est.epv < posControl.navConfig->inav.max_eph_epv) || useGpsZPos || isBaroValid) { /* Predict position/velocity based on acceleration */ inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.V.Z); @@ -576,13 +583,15 @@ static void updateEstimatedTopic(uint32_t currentTime) #endif /* Apply GPS correction to altitude */ - if (useGpsZ) { + if (useGpsZPos) { inavFilterCorrectPos(Z, dt, posEstimator.gps.pos.V.Z - posEstimator.history.pos[gpsHistoryIndex].V.Z, posControl.navConfig->inav.w_z_gps_p); - inavFilterCorrectVel(Z, dt, posEstimator.gps.vel.V.Z - posEstimator.history.vel[gpsHistoryIndex].V.Z, posControl.navConfig->inav.w_z_gps_v); - - /* Adjust EPV */ posEstimator.est.epv = MIN(posEstimator.est.epv, posEstimator.gps.epv); } + + /* Apply GPS correction to climb rate */ + if (useGpsZVel) { + inavFilterCorrectVel(Z, dt, posEstimator.gps.vel.V.Z - posEstimator.history.vel[gpsHistoryIndex].V.Z, posControl.navConfig->inav.w_z_gps_v); + } } else { inavFilterCorrectVel(Z, dt, 0.0f - posEstimator.est.vel.V.Z, posControl.navConfig->inav.w_z_res_v); -- 2.11.4.GIT