Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 14:47:44 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

[bug] AFBRS50 fix scheduling and refactor (#24837)

Browse Source 
* fix scheduling bug for afbrs50

* cleanup and refactor

* format

* clean up

---------

Co-authored-by: Alex Klimaj <alex@arkelectron.com>
This commit is contained in:
Jacob Dahl
2025-06-12 14:59:02 -08:00
committed by GitHub
parent 4c130769bd
commit fae563b35e
4 changed files with 374 additions and 385 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
+264 -302
View File
File diff suppressed because it is too large Load Diff
src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp
+22 -40
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,12 +31,6 @@
*
****************************************************************************/
/**
* @file AFBRS50.hpp
*
* Driver for the Broadcom AFBR-S50 connected via SPI.
*
*/
#pragma once
#include "argus.h"
@@ -51,8 +45,6 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
using namespace time_literals;
class AFBRS50 : public ModuleParams, public px4::ScheduledWorkItem
{
public:
@@ -60,63 +52,53 @@ public:
~AFBRS50() override;
int init();
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
/**50
* Stop the automatic measurement state machine.
*/
void stop();
int test();
bool _testing = false;
void printInfo();
private:
void Run() override;
void Evaluate_rate();
void recordCommsError();
void scheduleCollect();
void processMeasurement();
void updateMeasurementRateFromRange();
void ProcessMeasurement(argus_hnd_t *hnd);
static status_t measurementReadyCallback(status_t status, argus_hnd_t *hnd);
static status_t measurement_ready_callback(status_t status, argus_hnd_t *hnd);
status_t setRateAndDfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode);
argus_mode_t argusModeFromParameter();
void get_info();
status_t set_rate_and_dfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode);
argus_hnd_t *_hnd{nullptr};
private:
argus_hnd_t *_hnd {nullptr};
enum class STATE : uint8_t {
TEST,
CONFIGURE,
TRIGGER,
COLLECT,
STOP
WATCHDOG
} _state{STATE::CONFIGURE};
PX4Rangefinder _px4_rangefinder;
hrt_abstime _measurement_time{0};
hrt_abstime _last_rate_switch{0};
perf_counter_t _sample_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": sample interval")};
perf_counter_t _sample_perf{perf_alloc(PC_COUNT, MODULE_NAME": sample count")};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": comms error")};
perf_counter_t _not_ready_perf{perf_alloc(PC_COUNT, MODULE_NAME": not ready")};
uint32_t _measure_interval{1000000 / 50}; // 50Hz
float _current_distance{0};
int8_t _current_quality{0};
float _max_distance;
float _min_distance;
float _max_distance{30.f};
uint32_t _current_rate{0};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uint32_t _measurement_inverval {1000000 / 50}; // 50Hz
DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_AFBR_MODE>) _p_sens_afbr_mode,
(ParamInt<px4::params::SENS_AFBR_S_RATE>) _p_sens_afbr_s_rate,
(ParamInt<px4::params::SENS_AFBR_L_RATE>) _p_sens_afbr_l_rate,
(ParamInt<px4::params::SENS_AFBR_S_RATE>) _p_sens_afbr_s_rate,
(ParamInt<px4::params::SENS_AFBR_L_RATE>) _p_sens_afbr_l_rate,
(ParamInt<px4::params::SENS_AFBR_THRESH>) _p_sens_afbr_thresh,
(ParamInt<px4::params::SENS_AFBR_HYSTER>) _p_sens_afbr_hyster
(ParamInt<px4::params::SENS_AFBR_HYSTER>) _p_sens_afbr_hyster
);
};
src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c → src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.cpp
+87 -42
View File
@@ -14,6 +14,8 @@
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
/*! A structure to hold all internal data required by the S2PI module. */
typedef struct {
/*! Determines the current driver status. */
@@ -52,11 +54,71 @@ s2pi_handle_t s2pi_ = { .GPIOs = { [ S2PI_CLK ] = BROADCOM_AFBR_S50_S2PI_CLK,
}
};
static struct work_s broadcom_s2pi_transfer_work = {};
static perf_counter_t irq_perf = NULL;
static perf_counter_t s2pi_transfer_perf = NULL;
static perf_counter_t s2pi_transfer_callback_perf = NULL;
static perf_counter_t s2pi_irq_callback_perf = NULL;
class AFBRS50_SPI : public px4::ScheduledWorkItem
{
public:
AFBRS50_SPI();
void schedule_now();
void schedule_clear();
private:
void Run() override;
};
AFBRS50_SPI::AFBRS50_SPI():
// NOTE: we use SPI0 WQ since it is the 2nd highest priority thread (behind rate_ctrl).
// TODO: we should fix how SPI comms work. Async SPI comms is
// undesirable. We should use SPI TX DMA complete callback
// instead of relying on a high priority thread.
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::SPI0)
{
// Anything to do?
}
void AFBRS50_SPI::Run()
{
px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 0);
SPI_EXCHANGE(s2pi_.spidev, s2pi_.spi_tx_data, s2pi_.spi_rx_data, s2pi_.spi_frame_size);
px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 1);
//// WARNING!
// After the last SPI TX we have ~60us to execute the below
// callback otherwise the IRQ will fire and we're screwed.
// The proper way to solve this problem is to either fix
// the API or to configure SPI TX DMA callback complete
// to execute the below callback immediately.
// If we are pre-empted here and the IRQ fires before the
// callback has been invoked -- we're screwed.
IRQ_LOCK();
s2pi_.Status = STATUS_IDLE;
if (s2pi_.Callback != 0) {
s2pi_callback_t callback = s2pi_.Callback;
s2pi_.Callback = 0;
callback(STATUS_OK, s2pi_.CallbackData);
}
IRQ_UNLOCK();
}
void AFBRS50_SPI::schedule_now()
{
ScheduleNow();
}
void AFBRS50_SPI::schedule_clear()
{
ScheduleClear();
}
static AFBRS50_SPI *_spi_iface = nullptr;
/*!***************************************************************************
* @brief Initialize the S2PI module.
@@ -71,18 +133,6 @@ static perf_counter_t s2pi_irq_callback_perf = NULL;
*
* @return Returns the \link #status_t status\endlink (#STATUS_OK on success).
*****************************************************************************/
static int gpio_falling_edge(int irq, void *context, void *arg)
{
if (s2pi_.IrqCallback != 0) {
perf_begin(s2pi_irq_callback_perf);
s2pi_.IrqCallback(s2pi_.IrqCallbackData);
perf_end(s2pi_irq_callback_perf);
}
return 0;
}
status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps)
{
(void)defaultSlave;
@@ -91,12 +141,25 @@ status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps)
s2pi_.spidev = px4_spibus_initialize(BROADCOM_AFBR_S50_S2PI_SPI_BUS);
px4_arch_configgpio(BROADCOM_AFBR_S50_S2PI_IRQ);
px4_arch_gpiosetevent(BROADCOM_AFBR_S50_S2PI_IRQ, false, true, false, &gpio_falling_edge, NULL);
// Falling edge callback
auto callback = [](int irq, void *context, void *arg) -> int {
if (s2pi_.IrqCallback != 0)
{
perf_begin(irq_perf);
s2pi_.IrqCallback(s2pi_.IrqCallbackData);
perf_end(irq_perf);
}
s2pi_transfer_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": transfer");
s2pi_transfer_callback_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": transfer callback");
s2pi_irq_callback_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": irq callback");
return 0;
};
// NOTE: we enable the interrupt event here but do not configure the GPIO.
// We configure the GPIO and enable the interrupt after the device mode
// has been configured. This prevents erroneous interrupts from occuring.
px4_arch_gpiosetevent(BROADCOM_AFBR_S50_S2PI_IRQ, false, true, false, callback, NULL);
irq_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": irq callback");
_spi_iface = new AFBRS50_SPI();
return S2PI_SetBaudRate(baudRate_Bps);
}
@@ -334,25 +397,6 @@ status_t S2PI_CycleCsPin(s2pi_slave_t slave)
* was not started.
*****************************************************************************/
static void broadcom_s2pi_transfer_callout(void *arg)
{
perf_begin(s2pi_transfer_perf);
px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 0);
SPI_EXCHANGE(s2pi_.spidev, s2pi_.spi_tx_data, s2pi_.spi_rx_data, s2pi_.spi_frame_size);
s2pi_.Status = STATUS_IDLE;
px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 1);
perf_end(s2pi_transfer_perf);
/* Invoke callback if there is one */
if (s2pi_.Callback != 0) {
perf_begin(s2pi_transfer_callback_perf);
s2pi_callback_t callback = s2pi_.Callback;
s2pi_.Callback = 0;
callback(STATUS_OK, s2pi_.CallbackData);
perf_end(s2pi_transfer_callback_perf);
}
}
status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8_t *rxData, size_t frameSize,
s2pi_callback_t callback, void *callbackData)
{
@@ -384,7 +428,8 @@ status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8
s2pi_.spi_tx_data = (uint8_t *)txData;
s2pi_.spi_rx_data = rxData;
s2pi_.spi_frame_size = frameSize;
work_queue(HPWORK, &broadcom_s2pi_transfer_work, broadcom_s2pi_transfer_callout, NULL, 0);
_spi_iface->schedule_now();
IRQ_UNLOCK();
@@ -410,7 +455,7 @@ status_t S2PI_Abort(s2pi_slave_t slave)
/* Abort SPI transfer. */
if (status == STATUS_BUSY) {
work_cancel(HPWORK, &broadcom_s2pi_transfer_work);
_spi_iface->schedule_clear();
}
return STATUS_OK;
src/drivers/distance_sensor/broadcom/afbrs50/CMakeLists.txt
+1 -1
View File
@@ -47,7 +47,7 @@ px4_add_module(
AFBRS50.cpp
AFBRS50.hpp
API/Src/irq.c
API/Src/s2pi.c
API/Src/s2pi.cpp
API/Src/timer.c
argus_hal_test.c
DEPENDS