From 339838026230314de12ea6557e9c55f7bd52f173 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Sun, 7 Aug 2022 18:23:28 -0600 Subject: [PATCH] Switch to async AFBR measurement calls and use schedule. --- .../distance_sensor/broadcom/afbrs50/AFBRS50.cpp | 15 +++++++++------ .../broadcom/afbrs50/API/Src/irq.c | 15 +++++++++++++-- 2 files changed, 22 insertions(+), 8 deletions(-) diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp index af1d5a1eb5..01e9634073 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp @@ -234,7 +234,6 @@ void AFBRS50::Run() break; case STATE::CONFIGURE: { - //status_t status = Argus_SetConfigurationFrameTime(_hnd, _measure_interval); status_t status = set_rate(SHORT_RANGE_MODE_HZ); if (status != STATUS_OK) { @@ -259,8 +258,6 @@ void AFBRS50::Run() _mode = ARGUS_MODE_B; set_mode(_mode); - status = Argus_StartMeasurementTimer(_hnd, measurement_ready_callback); - if (status != STATUS_OK) { PX4_ERR("CONFIGURE status not okay: %i", (int)status); ScheduleNow(); @@ -273,7 +270,14 @@ void AFBRS50::Run() break; case STATE::COLLECT: { - // currently handeled by measurement_ready_callback + // Only start a new measurement if one is not ongoing + if (Argus_GetStatus(_hnd) == STATUS_IDLE) { + status_t status = Argus_TriggerMeasurement(_hnd, measurement_ready_callback); + + if (status != STATUS_OK) { + PX4_ERR("Argus_TriggerMeasurement status not okay: %i", (int)status); + } + } UpdateMode(); } @@ -290,8 +294,7 @@ void AFBRS50::Run() break; } - // backup schedule - ScheduleDelayed(100_ms); + ScheduleDelayed(_measure_interval); } void AFBRS50::UpdateMode() diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c index 833cf9d486..d1787c0d25 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c @@ -2,6 +2,7 @@ #include static volatile irqstate_t irqstate_flags; +static volatile size_t _lock_count = 0; /*!*************************************************************************** * @brief Enable IRQ Interrupts @@ -10,7 +11,13 @@ static volatile irqstate_t irqstate_flags; *****************************************************************************/ void IRQ_UNLOCK(void) { - leave_critical_section(irqstate_flags); + if (_lock_count > 0) { + _lock_count--; + + if (_lock_count == 0) { + leave_critical_section(irqstate_flags); + } + } } /*!*************************************************************************** @@ -20,5 +27,9 @@ void IRQ_UNLOCK(void) *****************************************************************************/ void IRQ_LOCK(void) { - irqstate_flags = enter_critical_section(); + if (_lock_count == 0) { + irqstate_flags = enter_critical_section(); + } + + _lock_count++; }