From 09cc3120e24f911b26cd36e12430ed3330574b7a Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 22 Oct 2020 11:41:35 +0200 Subject: [PATCH] OpticalFlow: add optical flow velocity logging This is important to align the flow with the IMU data and verify that the compensation is properly done --- msg/CMakeLists.txt | 1 + msg/estimator_optical_flow_vel.msg | 8 ++++++++ msg/tools/uorb_rtps_message_ids.yaml | 2 ++ src/lib/ecl | 2 +- src/modules/ekf2/EKF2.cpp | 22 ++++++++++++++++++++++ src/modules/ekf2/EKF2.hpp | 5 +++++ src/modules/logger/logged_topics.cpp | 1 + 7 files changed, 40 insertions(+), 1 deletion(-) create mode 100644 msg/estimator_optical_flow_vel.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 263d12f8ec..9b563274e4 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -56,6 +56,7 @@ set(msg_files esc_report.msg esc_status.msg estimator_innovations.msg + estimator_optical_flow_vel.msg estimator_sensor_bias.msg estimator_states.msg estimator_status.msg diff --git a/msg/estimator_optical_flow_vel.msg b/msg/estimator_optical_flow_vel.msg new file mode 100644 index 0000000000..38ee0d7f68 --- /dev/null +++ b/msg/estimator_optical_flow_vel.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[2] vel_body # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s) +float32[2] vel_ne # same as vel_body but in local frame (m/s) +float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad) +float32[2] flow_compensated_integral # integrated optical flow measurement compensated for angular motion (rad) +float32[3] gyro_rate_integral # gyro measurement integrated to flow rate and synchronized with flow measurements (rad) diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index e64331aa0d..a382012e54 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -285,6 +285,8 @@ rtps: id: 136 - msg: navigator_mission_item id: 137 + - msg: estimator_optical_flow_vel + id: 138 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/src/lib/ecl b/src/lib/ecl index f666ebb995..dd3ffc4192 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit f666ebb99552eed796e82a20bb60e813d45eee03 +Subproject commit dd3ffc419238bfc7d9b7e3be92a178c2cf7756f1 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 92f958b472..154175f3e5 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -159,6 +159,7 @@ EKF2::EKF2(bool replay_mode): _estimator_innovation_test_ratios_pub.advertise(); _estimator_innovation_variances_pub.advertise(); _estimator_innovations_pub.advertise(); + _estimator_optical_flow_vel_pub.advertise(); _estimator_sensor_bias_pub.advertise(); _estimator_states_pub.advertise(); _estimator_status_pub.advertise(); @@ -489,6 +490,7 @@ void EKF2::Run() } if (_optical_flow_sub.updated()) { + _new_optical_flow_data_received = true; optical_flow_s optical_flow; if (_optical_flow_sub.copy(&optical_flow)) { @@ -1042,6 +1044,11 @@ void EKF2::Run() publish_yaw_estimator_status(now); + if (_new_optical_flow_data_received) { + publish_estimator_optical_flow_vel(now); + _new_optical_flow_data_received = false; + } + if (!_mag_decl_saved && (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) { _mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl); } @@ -1333,6 +1340,21 @@ void EKF2::publish_wind_estimate(const hrt_abstime ×tamp) } } +void EKF2::publish_estimator_optical_flow_vel(const hrt_abstime ×tamp) +{ + estimator_optical_flow_vel_s flow_vel{}; + flow_vel.timestamp_sample = timestamp; + + _ekf.getFlowVelBody().copyTo(flow_vel.vel_body); + _ekf.getFlowVelNE().copyTo(flow_vel.vel_ne); + _ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral); + _ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral); + _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral); + flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); + + _estimator_optical_flow_vel_pub.publish(flow_vel); +} + float EKF2::filter_altitude_ellipsoid(float amsl_hgt) { float height_diff = static_cast(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 6aa7aae2f2..7b4e9833e3 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -81,6 +81,7 @@ #include #include #include +#include #include "Utility/PreFlightChecker.hpp" @@ -125,6 +126,7 @@ private: void publish_odometry(const hrt_abstime ×tamp, const imuSample &imu, const vehicle_local_position_s &lpos); void publish_wind_estimate(const hrt_abstime ×tamp); void publish_yaw_estimator_status(const hrt_abstime ×tamp); + void publish_estimator_optical_flow_vel(const hrt_abstime ×tamp); /* * Calculate filtered WGS84 height from estimated AMSL height @@ -169,6 +171,8 @@ private: bool new_ev_data_received = false; vehicle_odometry_s _ev_odom{}; + bool _new_optical_flow_data_received{false}; + uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; @@ -201,6 +205,7 @@ private: uORB::Publication _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::Publication _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::Publication _estimator_innovations_pub{ORB_ID(estimator_innovations)}; + uORB::Publication _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::Publication _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; uORB::Publication _estimator_states_pub{ORB_ID(estimator_states)}; uORB::PublicationData _estimator_status_pub{ORB_ID(estimator_status)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 82daf3f702..fc73194c36 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -60,6 +60,7 @@ void LoggedTopics::add_default_topics() add_topic("estimator_innovation_test_ratios", 200); add_topic("estimator_innovation_variances", 200); add_topic("estimator_innovations", 200); + add_topic("estimator_optical_flow_vel"); add_topic("estimator_sensor_bias", 1000); add_topic("estimator_states", 1000); add_topic("estimator_status", 200);