diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-20 23:44:04 +0100 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-20 23:44:04 +0100 |
commit | 2e472cf918074cd2c4addc7d32abc09933d4ee2d (patch) | |
tree | c83ea8e7680f9c9debe13f9cf0aff6aa928e2562 /src/modules/attitude_estimator_ekf | |
parent | 7956c8b08e53be876e96ddd434799b96ce5b7661 (diff) | |
download | px4-firmware-2e472cf918074cd2c4addc7d32abc09933d4ee2d.tar.gz px4-firmware-2e472cf918074cd2c4addc7d32abc09933d4ee2d.tar.bz2 px4-firmware-2e472cf918074cd2c4addc7d32abc09933d4ee2d.zip |
attitude_estimator_ekf: acc comp bug fixed, estimated gravity vector logging
Diffstat (limited to 'src/modules/attitude_estimator_ekf')
-rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 6 |
1 files changed, 5 insertions, 1 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 7cf100014..6a1bec153 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos_updated && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; @@ -532,6 +532,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; + att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); + att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); + att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); + /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); |