diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-28 16:29:14 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-28 16:29:14 +0100 |
commit | 2728889f7886e3ab2fea16941d29e60ece0d4449 (patch) | |
tree | ca9994d71205731ee4bb404175c2cf8f13fcc539 /src/modules/position_estimator_inav/position_estimator_inav_main.c | |
parent | f23e603d02ba416ae250770cdaad6a859d6bae69 (diff) | |
parent | 1dcc5c49cc75778bcdde770f2d6c2700dd2bec2e (diff) | |
download | px4-firmware-2728889f7886e3ab2fea16941d29e60ece0d4449.tar.gz px4-firmware-2728889f7886e3ab2fea16941d29e60ece0d4449.tar.bz2 px4-firmware-2728889f7886e3ab2fea16941d29e60ece0d4449.zip |
Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge_attctlposctl
Diffstat (limited to 'src/modules/position_estimator_inav/position_estimator_inav_main.c')
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 3 |
1 files changed, 3 insertions, 0 deletions
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 6de283396..ec297e7f0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -877,6 +877,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; @@ -907,6 +908,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ @@ -991,6 +993,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { |