From 864eeb2bd3222c3c87ae79ae097d95073b7502db Mon Sep 17 00:00:00 2001
From: Jacob Dahl <dahl.jakejacob@gmail.com>
Date: Wed, 9 Jul 2025 22:53:27 -0800
Subject: [PATCH] switch measurement trigger scheduling

---
 .../broadcom/afbrs50/AFBRS50.cpp              | 44 ++++++++++---------
 .../broadcom/afbrs50/AFBRS50.hpp              |  3 +-
 2 files changed, 26 insertions(+), 21 deletions(-)

diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
index b46c81d3aa..ecd6fd8543 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
@@ -123,7 +123,6 @@ void AFBRS50::processMeasurement()
 		quality = 0;
 	}
 
-	// TODO: I don't think we need this..
 	if (distance > _max_distance) {
 		distance = 0.0;
 		quality = 0;
@@ -264,49 +263,51 @@ void AFBRS50::Run()
 
 			// Enable interrupt on falling edge
 			px4_arch_configgpio(BROADCOM_AFBR_S50_S2PI_IRQ);
-			_state = STATE::TRIGGER;
+
 			// TODO: delay after configure?
-			ScheduleNow();
-			// ScheduleDelayed(50_ms);
+			_state = STATE::TRIGGER;
+			ScheduleDelayed(_measurement_inverval);
 		}
 		break;
 
 	case STATE::TRIGGER: {
 			if (Argus_GetStatus(_hnd) != STATUS_IDLE) {
 				perf_count(_not_ready_perf);
-				ScheduleDelayed(10_ms);
+				ScheduleDelayed(_measurement_inverval / 4);
 				return;
 			}
 
-			// Trigger continuous measurement mode. An hrt_call_after will trigger
-			// measurements periodically -- see API/Src/timer.c
-			status_t status = Argus_StartMeasurementTimer(_hnd, measurementReadyCallback);
+			_trigger_time = hrt_absolute_time();
+			status_t status = Argus_TriggerMeasurement(_hnd, measurementReadyCallback);
 
 			if (status != STATUS_OK) {
-				PX4_ERR("Argus_TriggerMeasurement status not okay: %i", (int)status);
+				// PX4_ERR("Argus_TriggerMeasurement status not okay: %i", (int)status);
 				perf_count(_not_ready_perf);
-				ScheduleDelayed(50_ms);
+				// Reschedule another trigger
+				_state = STATE::TRIGGER;
+				ScheduleDelayed(_measurement_inverval / 4);
 			}
+
+			// We don't reschedule, the trigger callback will schedule a collect
 		}
 		break;
 
 	case STATE::COLLECT: {
+			// Process and publish the measurement
 			processMeasurement();
 
 			// Change measurement rate and mode based on range
 			updateMeasurementRateFromRange();
 
-			// Rechedule watchdog, push back by 2x measurement rate
-			_state = STATE::WATCHDOG;
-			ScheduleDelayed(_measurement_inverval * 2);
-		}
-		break;
-
-	case STATE::WATCHDOG: {
-			PX4_WARN("watchdog triggered, rescheduling");
 			_state = STATE::TRIGGER;
-			// When this occurs the device locks up for ~160ms
-			ScheduleDelayed(160_ms);
+
+			auto elapsed = hrt_elapsed_time(&_trigger_time);
+
+			if (elapsed > _measurement_inverval) {
+				ScheduleNow();
+			} else {
+				ScheduleDelayed(_measurement_inverval - elapsed);
+			}
 		}
 		break;
 
@@ -315,6 +316,9 @@ void AFBRS50::Run()
 	}
 }
 
+// TODO: fix this logic
+// Require 3 readings on same side of fence to mode switch
+// Require 3_s after ground_contact in vehicle_land_detected before switching to long range mode
 void AFBRS50::updateMeasurementRateFromRange()
 {
 	// only update mode if _current_distance is a valid measurement and if the last rate switch was more than 1 second ago
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp
index 9332d3b8a2..25fd58902b 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp
@@ -74,7 +74,6 @@ private:
 		CONFIGURE,
 		TRIGGER,
 		COLLECT,
-		WATCHDOG
 	} _state{STATE::CONFIGURE};
 
 	PX4Rangefinder _px4_rangefinder;
@@ -90,6 +89,8 @@ private:
 	float _max_distance{30.f};
 	uint32_t _current_rate{0};
 
+	hrt_abstime _trigger_time{0};
+
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 
 	uint32_t _measurement_inverval {1000000 / 50}; // 50Hz