fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-22 01:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_optical_flow: don't publish interval lower than configured rate

Browse Source
This commit is contained in:
Daniel Agar
2023-09-15 14:02:14 -04:00
parent d98f3554da
commit 5e8ee98bf9
2 changed files with 7 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
+7 -13
View File
@@ -112,15 +112,16 @@ void VehicleOpticalFlow::Run()
if (_sensor_flow_sub.update(&sensor_optical_flow)) { if (_sensor_flow_sub.update(&sensor_optical_flow)) {
// clear data accumulation if there's a gap in data // clear data accumulation if there's a gap in data
if (((sensor_optical_flow.timestamp_sample - _flow_timestamp_sample_last) const uint64_t integration_gap_threshold_us = sensor_optical_flow.integration_timespan_us * 2;
> sensor_optical_flow.integration_timespan_us * 1.5f)
|| (_accumulated_count > 0 && _quality_sum == 0)) { if ((sensor_optical_flow.timestamp_sample >= _flow_timestamp_sample_last + integration_gap_threshold_us)
|| (_accumulated_count > 0 && (sensor_optical_flow.quality > 0) && _quality_sum == 0)) {
ClearAccumulatedData(); ClearAccumulatedData();
} }
const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - lroundf( const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - sensor_optical_flow.integration_timespan_us;
sensor_optical_flow.integration_timespan_us);
const hrt_abstime timestamp_newest = sensor_optical_flow.timestamp; const hrt_abstime timestamp_newest = sensor_optical_flow.timestamp;
// delta angle // delta angle
@@ -203,12 +204,7 @@ void VehicleOpticalFlow::Run()
const float interval_us = 1e6f / _param_sens_flow_rate.get(); const float interval_us = 1e6f / _param_sens_flow_rate.get();
// don't allow publishing faster than SENS_FLOW_RATE // don't allow publishing faster than SENS_FLOW_RATE
if (sensor_optical_flow.timestamp_sample < _last_publication_timestamp + interval_us) { if (_integration_timespan_us < interval_us) {
publish = false;
}
// integrate for full interval unless we haven't published recently
if ((hrt_elapsed_time(&_last_publication_timestamp) < 1_ms) && (_integration_timespan_us < interval_us)) {
publish = false; publish = false;
} }
} }
@@ -271,8 +267,6 @@ void VehicleOpticalFlow::Run()
vehicle_optical_flow.timestamp = hrt_absolute_time(); vehicle_optical_flow.timestamp = hrt_absolute_time();
_vehicle_optical_flow_pub.publish(vehicle_optical_flow); _vehicle_optical_flow_pub.publish(vehicle_optical_flow);
_last_publication_timestamp = vehicle_optical_flow.timestamp_sample;
// vehicle_optical_flow_vel if distance is available (for logging) // vehicle_optical_flow_vel if distance is available (for logging)
if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) { if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) {
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp
-2
View File
@@ -117,8 +117,6 @@ private:
uint16_t _quality_sum{0}; uint16_t _quality_sum{0};
uint8_t _accumulated_count{0}; uint8_t _accumulated_count{0};
hrt_abstime _last_publication_timestamp{0};
int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations
hrt_abstime _last_range_sensor_update{0}; hrt_abstime _last_range_sensor_update{0};