diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-11-25 19:47:33 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-11-25 19:47:33 +0400 |
| commit | 98ebcb626d4a8f84b61fbbedaa594a0a3b8df17d (patch) | |
| tree | 3f901c4849679aa0e2e8f9761757e7ec405016e8 /src/modules/position_estimator_inav | |
| parent | 20ac3c815547716ea7ad201c56e3e64ba742555f (diff) | |
| download | px4-firmware-98ebcb626d4a8f84b61fbbedaa594a0a3b8df17d.tar.gz px4-firmware-98ebcb626d4a8f84b61fbbedaa594a0a3b8df17d.tar.bz2 px4-firmware-98ebcb626d4a8f84b61fbbedaa594a0a3b8df17d.zip | |
position_estimator_inav: minor comments and code style fixes
Diffstat (limited to 'src/modules/position_estimator_inav')
| -rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 13 |
1 files changed, 7 insertions, 6 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 d95de11c4..d99895a64 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -388,7 +388,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (sensor.accelerometer_counter > accel_counter) { if (att.R_valid) { - /* correct accel bias, now only for Z */ + /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; sensor.accelerometer_m_s2[1] -= acc_bias[1]; sensor.accelerometer_m_s2[2] -= acc_bias[2]; @@ -664,11 +664,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* baro offset correction */ - if (use_gps_z) { - float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; - baro_offset += offs_corr; - baro_counter += offs_corr; - } + if (use_gps_z) { + float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; + baro_offset += offs_corr; + baro_counter += offs_corr; + } /* accelerometer bias correction */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; @@ -679,6 +679,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; } + if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; } |