forked from Archive/PX4-Autopilot
ekf2: publish ekf2_timestamps topic
This commit is contained in:
parent
df3ef3660b
commit
aabdc4125b
|
@ -75,6 +75,7 @@
|
|||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/ekf2_innovations.h>
|
||||
#include <uORB/topics/ekf2_replay.h>
|
||||
#include <uORB/topics/ekf2_timestamps.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
|
@ -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();
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue