From 4222d06124cb332efb14d59fd8b7010818fcafbe Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Tue, 30 Apr 2024 19:39:34 -0600 Subject: [PATCH] distance_sensor add timestamp_sample. AFBR set timestamp_sample from callback time --- msg/DistanceSensor.msg | 1 + src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp | 6 +++--- src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp | 2 +- src/lib/drivers/rangefinder/PX4Rangefinder.cpp | 4 +++- 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/msg/DistanceSensor.msg b/msg/DistanceSensor.msg index dd08e4b5898b..0d946a45ce14 100644 --- a/msg/DistanceSensor.msg +++ b/msg/DistanceSensor.msg @@ -1,6 +1,7 @@ # DISTANCE_SENSOR message data uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp index 91e3366d5ec8..49fbe3ab7961 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp @@ -75,7 +75,7 @@ status_t AFBRS50::measurement_ready_callback(status_t status, argus_hnd_t *hnd) if (!up_interrupt_context()) { if (status == STATUS_OK) { if (g_dev) { - g_dev->ProcessMeasurement(hnd); + g_dev->ProcessMeasurement(hnd, hrt_absolute_time()); } } else { @@ -86,7 +86,7 @@ status_t AFBRS50::measurement_ready_callback(status_t status, argus_hnd_t *hnd) return status; } -void AFBRS50::ProcessMeasurement(argus_hnd_t *hnd) +void AFBRS50::ProcessMeasurement(argus_hnd_t *hnd, hrt_abstime timestamp_sample) { perf_count(_sample_perf); @@ -112,7 +112,7 @@ void AFBRS50::ProcessMeasurement(argus_hnd_t *hnd) _current_distance = result_m; _current_quality = quality; - _px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality); + _px4_rangefinder.update(timestamp_sample, result_m, quality); } } diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp index 7b24c5b4baeb..c165ad617aca 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp @@ -80,7 +80,7 @@ class AFBRS50 : public ModuleParams, public px4::ScheduledWorkItem void Evaluate_rate(); - void ProcessMeasurement(argus_hnd_t *hnd); + void ProcessMeasurement(argus_hnd_t *hnd, hrt_abstime timestamp_sample); static status_t measurement_ready_callback(status_t status, argus_hnd_t *hnd); diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index 63937f8f59e3..3f628cef3bb3 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -69,7 +69,7 @@ void PX4Rangefinder::update(const hrt_abstime ×tamp_sample, const float dis { distance_sensor_s &report = _distance_sensor_pub.get(); - report.timestamp = timestamp_sample; + report.timestamp_sample = timestamp_sample; report.current_distance = distance; report.signal_quality = quality; @@ -80,5 +80,7 @@ void PX4Rangefinder::update(const hrt_abstime ×tamp_sample, const float dis } } + report.timestamp = hrt_absolute_time(); + _distance_sensor_pub.update(); }