dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Switch to async AFBR measurement calls and use schedule.

This commit is contained in:
alexklimaj 2022-08-07 18:23:28 -06:00 committed by Daniel Agar
parent 3dffa5e6df
commit 3398380262
2 changed files with 22 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
View File

@ -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()

15
src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c
View File

@ -2,6 +2,7 @@
#include <nuttx/irq.h>
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++;
}