From aabdc4125b0230637dbfc2200c10401563b79fcd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 3 Mar 2017 08:26:24 +0100 Subject: [PATCH] ekf2: publish ekf2_timestamps topic --- src/modules/ekf2/ekf2_main.cpp | 65 ++++++++++++++++++++ src/modules/sensors/voted_sensors_update.cpp | 8 ++- 2 files changed, 70 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index ba070c4c94..096fb93083 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -75,6 +75,7 @@ #include #include #include +#include #include #include #include @@ -162,6 +163,7 @@ private: orb_advert_t _estimator_status_pub; orb_advert_t _estimator_innovations_pub; orb_advert_t _replay_pub; + orb_advert_t _ekf2_timestamps_pub; /* Low pass filter for attitude rates */ math::LowPassFilter2p _lp_roll_rate; @@ -299,6 +301,7 @@ Ekf2::Ekf2(): _estimator_status_pub(nullptr), _estimator_innovations_pub(nullptr), _replay_pub(nullptr), + _ekf2_timestamps_pub(nullptr), _lp_roll_rate(250.0f, 30.0f), _lp_pitch_rate(250.0f, 30.0f), _lp_yaw_rate(250.0f, 20.0f), @@ -1010,6 +1013,68 @@ void Ekf2::task_main() _prev_landed = vehicle_land_detected.landed; + // publish ekf2_timestamps (using 0.1 ms relative timestamps) + { + ekf2_timestamps_s ekf2_timestamps; + ekf2_timestamps.timestamp = sensors.timestamp; + + if (gps_updated) { + // divide individually to get consistent rounding behavior + ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); + + } else { + ekf2_timestamps.gps_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; + } + + if (optical_flow_updated) { + ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); + + } else { + ekf2_timestamps.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; + } + + if (range_finder_updated) { + ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)range_finder.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); + + } else { + ekf2_timestamps.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; + } + + if (airspeed_updated) { + ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); + + } else { + ekf2_timestamps.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; + } + + if (vision_position_updated) { + ekf2_timestamps.vision_position_timestamp_rel = (int16_t)((int64_t)ev_pos.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); + + } else { + ekf2_timestamps.vision_position_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; + } + + if (vision_attitude_updated) { + ekf2_timestamps.vision_attitude_timestamp_rel = (int16_t)((int64_t)ev_att.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); + + } else { + ekf2_timestamps.vision_attitude_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID; + } + + if (_ekf2_timestamps_pub == nullptr) { + _ekf2_timestamps_pub = orb_advertise(ORB_ID(ekf2_timestamps), &ekf2_timestamps); + + } else { + orb_publish(ORB_ID(ekf2_timestamps), _ekf2_timestamps_pub, &ekf2_timestamps); + } + } + + // publish replay message if in replay mode bool publish_replay_message = (bool)_param_record_replay_msg.get(); diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index a8d8da7320..bc6e44221b 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -1045,15 +1045,17 @@ void VotedSensorsUpdate::check_failover() void VotedSensorsUpdate::set_relative_timestamps(sensor_combined_s &raw) { if (_last_accel_timestamp[_accel.last_best_vote]) { - raw.accelerometer_timestamp_relative = (int32_t)(_last_accel_timestamp[_accel.last_best_vote] - raw.timestamp); + raw.accelerometer_timestamp_relative = (int32_t)((int64_t)_last_accel_timestamp[_accel.last_best_vote] - + (int64_t)raw.timestamp); } if (_last_mag_timestamp[_mag.last_best_vote]) { - raw.magnetometer_timestamp_relative = (int32_t)(_last_mag_timestamp[_mag.last_best_vote] - raw.timestamp); + raw.magnetometer_timestamp_relative = (int32_t)((int64_t)_last_mag_timestamp[_mag.last_best_vote] - + (int64_t)raw.timestamp); } if (_last_baro_timestamp[_baro.last_best_vote]) { - raw.baro_timestamp_relative = (int32_t)(_last_baro_timestamp[_baro.last_best_vote] - raw.timestamp); + raw.baro_timestamp_relative = (int32_t)((int64_t)_last_baro_timestamp[_baro.last_best_vote] - (int64_t)raw.timestamp); } }