From 3db5a35005f1e57aa04e5e17d21c3cfb517bb605 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 21 Dec 2022 14:21:10 +0100 Subject: [PATCH] HACK: add new topic to log local position w/o reference Signed-off-by: Silvan Fuhrer --- msg/vehicle_local_position.msg | 2 +- src/modules/ekf2/EKF2.cpp | 10 ++++++++++ src/modules/ekf2/EKF2.hpp | 2 ++ src/modules/logger/logged_topics.cpp | 2 ++ 4 files changed, 15 insertions(+), 1 deletion(-) diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index a638bbb61c..c27da065d9 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -72,5 +72,5 @@ float32 vz_max # maximum vertical speed - set to 0 when limiting not required float32 hagl_min # minimum height above ground level - set to 0 when limiting not required (meters) float32 hagl_max # maximum height above ground level - set to 0 when limiting not required (meters) -# TOPICS vehicle_local_position vehicle_local_position_groundtruth +# TOPICS vehicle_local_position vehicle_local_position_groundtruth vehicle_local_position_wo_ref # TOPICS estimator_local_position diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 05798f375c..cf854def3d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -56,6 +56,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _global_position_pub(multi_mode ? ORB_ID(estimator_global_position) : ORB_ID(vehicle_global_position)), _odometry_pub(multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)), _wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)), + _local_position_wo_ref_pub(ORB_ID(vehicle_local_position_wo_ref)), _params(_ekf.getParamHandle()), _param_ekf2_predict_us(_params->filter_update_interval_us), _param_ekf2_mag_delay(_params->mag_delay_ms), @@ -168,6 +169,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): // advertise expected minimal topic set immediately to ensure logging _attitude_pub.advertise(); _local_position_pub.advertise(); + _local_position_wo_ref_pub.advertise(); _estimator_event_flags_pub.advertise(); _estimator_innovation_test_ratios_pub.advertise(); @@ -1041,6 +1043,14 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) // publish vehicle local position data lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _local_position_pub.publish(lpos); + + // publish vehicle_local_position_wo_ref (without lat/lon ref) + vehicle_local_position_s vehicle_local_position_wo_ref; + vehicle_local_position_wo_ref = lpos; + vehicle_local_position_wo_ref.ref_lat = static_cast(NAN); + vehicle_local_position_wo_ref.ref_lon = static_cast(NAN); + vehicle_local_position_wo_ref.ref_alt = NAN; + _local_position_wo_ref_pub.publish(vehicle_local_position_wo_ref); } void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 0f5e97481b..61861b0d4b 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -337,6 +337,8 @@ private: uORB::PublicationMulti _odometry_pub; uORB::PublicationMulti _wind_pub; + uORB::Publication _local_position_wo_ref_pub; + PreFlightChecker _preflt_checker; Ekf _ekf; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 087bb62ac1..197acfffd3 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -112,6 +112,8 @@ void LoggedTopics::add_default_topics() add_optional_topic("vtol_vehicle_status", 200); add_topic("wind", 1000); + add_topic("vehicle_local_position_wo_ref", 100); + if (!_do_not_log_position_data) { add_optional_topic("camera_capture"); add_topic("follow_target_status", 100);