From 8545164869479486ec68e78a1ae887a10babd7fb Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Mon, 17 Oct 2022 08:23:36 -0600 Subject: [PATCH] sensors/vehicle_optical_flow: only get gyro if optical flow delta angle not available (#20416) --- src/modules/logger/logged_topics.cpp | 6 +++--- .../sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp | 8 +++++++- .../sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp | 2 ++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 7d78da5d5b..cb00c1ebb6 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -202,13 +202,13 @@ void LoggedTopics::add_default_topics() add_topic_multi("sensor_gps", 1000, 2); add_topic_multi("sensor_gnss_relative", 1000, 1); add_optional_topic_multi("sensor_gyro", 1000, 4); - add_optional_topic_multi("sensor_mag", 1000, 4); - add_optional_topic_multi("sensor_optical_flow", 1000, 2); + add_topic_multi("sensor_mag", 1000, 4); + add_topic_multi("sensor_optical_flow", 1000, 2); add_topic_multi("vehicle_imu", 500, 4); add_topic_multi("vehicle_imu_status", 1000, 4); add_optional_topic_multi("vehicle_magnetometer", 500, 4); - add_optional_topic("vehicle_optical_flow", 500); + add_topic("vehicle_optical_flow", 500); //add_optional_topic("vehicle_optical_flow_vel", 100); add_optional_topic("pps_capture"); diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp index 11d43ee0b9..a6a7499a78 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -102,7 +102,10 @@ void VehicleOpticalFlow::Run() ParametersUpdate(); UpdateDistanceSensor(); - UpdateSensorGyro(); + + if (!_delta_angle_available) { + UpdateSensorGyro(); + } sensor_optical_flow_s sensor_optical_flow; @@ -129,8 +132,11 @@ void VehicleOpticalFlow::Run() ) { // passthrough integrated gyro if available _delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle}; + _delta_angle_available = true; } else { + _delta_angle_available = false; + // integrate synchronized gyro gyroSample gyro_sample; diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp index 611cf7c2e8..186af594a8 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -122,6 +122,8 @@ private: int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations hrt_abstime _last_range_sensor_update{0}; + bool _delta_angle_available{false}; + struct gyroSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) matrix::Vector3f data{};