distance_sensor add timestamp_sample. AFBR set timestamp_sample from …

…callback time
PX4 · May 1, 2024 · 4222d06 · 4222d06
1 parent b6da0b1
commit 4222d06
Show file tree

Hide file tree

Showing 4 changed files with 8 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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
@@ -69,7 +69,7 @@ void PX4Rangefinder::update(const hrt_abstime &timestamp_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 &timestamp_sample, const float dis
 		}
 	}
 
+	report.timestamp = hrt_absolute_time();
+
 	_distance_sensor_pub.update();
 }