Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

ekf2 replay: use correct timestamp for attitude publication

Browse Source
This commit is contained in:
Beat Küng
2018-08-04 16:19:24 +02:00
parent e0dc5ae7f4
commit e4485fc8cf
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+4 -4
View File
@@ -121,7 +121,7 @@ private:
void update_mag_bias(Param &mag_bias_param, int axis_index); void update_mag_bias(Param &mag_bias_param, int axis_index);
template<typename Param> template<typename Param>
bool update_mag_decl(Param &mag_decl_param); bool update_mag_decl(Param &mag_decl_param);
bool publish_attitude(const sensor_combined_s &sensors); bool publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now);
bool publish_wind_estimate(const hrt_abstime &timestamp); bool publish_wind_estimate(const hrt_abstime &timestamp);
const Vector3f get_vel_body_wind(); const Vector3f get_vel_body_wind();
@@ -798,7 +798,7 @@ void Ekf2::run()
_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt, gyro_integral, accel_integral); _ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt, gyro_integral, accel_integral);
// publish attitude immediately (uses quaternion from output predictor) // publish attitude immediately (uses quaternion from output predictor)
publish_attitude(sensors); publish_attitude(sensors, now);
// read mag data // read mag data
bool magnetometer_updated = false; bool magnetometer_updated = false;
@@ -1643,12 +1643,12 @@ int Ekf2::getRangeSubIndex(const int *subs)
return -1; return -1;
} }
bool Ekf2::publish_attitude(const sensor_combined_s &sensors) bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now)
{ {
if (_ekf.attitude_valid()) { if (_ekf.attitude_valid()) {
// generate vehicle attitude quaternion data // generate vehicle attitude quaternion data
vehicle_attitude_s att; vehicle_attitude_s att;
att.timestamp = hrt_absolute_time(); att.timestamp = now;
const Quatf q{_ekf.calculate_quaternion()}; const Quatf q{_ekf.calculate_quaternion()};
q.copyTo(att.q); q.copyTo(att.q);