fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_optical_flow: only get gyro if optical flow delta angle not available (#20416)

Browse Source
This commit is contained in:
Alex Klimaj 2022-10-17 08:23:36 -06:00 committed by GitHub
parent 116bb6049f
commit 8545164869
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/logger/logged_topics.cpp
View File

@ -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");

8
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
View File

@ -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;

2
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp
View File

@ -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{};