fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 14:58:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Switch to async AFBR measurement calls and use schedule.

Browse Source
This commit is contained in:
alexklimaj
2022-09-12 13:00:25 -04:00
committed by Daniel Agar
parent 3dffa5e6df
commit 3398380262
2 changed files with 22 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
+9 -6
View File
@@ -234,7 +234,6 @@ void AFBRS50::Run()
break; break;
case STATE::CONFIGURE: { case STATE::CONFIGURE: {
//status_t status = Argus_SetConfigurationFrameTime(_hnd, _measure_interval);
status_t status = set_rate(SHORT_RANGE_MODE_HZ); status_t status = set_rate(SHORT_RANGE_MODE_HZ);
if (status != STATUS_OK) { if (status != STATUS_OK) {
@@ -259,8 +258,6 @@ void AFBRS50::Run()
_mode = ARGUS_MODE_B; _mode = ARGUS_MODE_B;
set_mode(_mode); set_mode(_mode);
status = Argus_StartMeasurementTimer(_hnd, measurement_ready_callback);
if (status != STATUS_OK) { if (status != STATUS_OK) {
PX4_ERR("CONFIGURE status not okay: %i", (int)status); PX4_ERR("CONFIGURE status not okay: %i", (int)status);
ScheduleNow(); ScheduleNow();
@@ -273,7 +270,14 @@ void AFBRS50::Run()
break; break;
case STATE::COLLECT: { 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(); UpdateMode();
} }
@@ -290,8 +294,7 @@ void AFBRS50::Run()
break; break;
} }
// backup schedule ScheduleDelayed(_measure_interval);
ScheduleDelayed(100_ms);
} }
void AFBRS50::UpdateMode() void AFBRS50::UpdateMode()
src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c
+13 -2
View File
@@ -2,6 +2,7 @@
#include <nuttx/irq.h> #include <nuttx/irq.h>
static volatile irqstate_t irqstate_flags; static volatile irqstate_t irqstate_flags;
static volatile size_t _lock_count = 0;
/*!*************************************************************************** /*!***************************************************************************
* @brief Enable IRQ Interrupts * @brief Enable IRQ Interrupts
@@ -10,7 +11,13 @@ static volatile irqstate_t irqstate_flags;
*****************************************************************************/ *****************************************************************************/
void IRQ_UNLOCK(void) 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) void IRQ_LOCK(void)
{ {
irqstate_flags = enter_critical_section(); if (_lock_count == 0) {
irqstate_flags = enter_critical_section();
}
_lock_count++;
} }