fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:38:52 +08:00
Code Issues 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 16:59:02 -06:00
committed by GitHub
co-authored by Alex Klimaj
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
@@ -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,27 +31,21 @@
*
****************************************************************************/
/* Include Files */
#include "AFBRS50.hpp"
#include "argus_hal_test.h"
#include "s2pi.h"
#include <lib/drivers/device/Device.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
/*! Define the SPI baud rate (to be used in the SPI module). */
#define SPI_BAUD_RATE 5000000
#include "s2pi.h"
#include "timer.h"
#include "argus_hal_test.h"
using namespace time_literals;
AFBRS50 *g_dev{nullptr};
AFBRS50::AFBRS50(uint8_t device_orientation):
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
// ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::SPI6),
_px4_rangefinder(0, device_orientation)
{
device::Device::DeviceId device_id{};
@@ -65,278 +59,263 @@ AFBRS50::AFBRS50(uint8_t device_orientation):
AFBRS50::~AFBRS50()
{
stop();
ScheduleClear();
Argus_StopMeasurementTimer(_hnd);
Argus_Deinit(_hnd);
Argus_DestroyHandle(_hnd);
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_not_ready_perf);
}
status_t AFBRS50::measurement_ready_callback(status_t status, argus_hnd_t *hnd)
status_t AFBRS50::measurementReadyCallback(status_t status, argus_hnd_t *hnd)
{
if (!up_interrupt_context()) {
if (status == STATUS_OK) {
if (g_dev) {
g_dev->ProcessMeasurement(hnd);
}
// Called from the SPI comms thread context
} else {
PX4_ERR("Measurement Ready Callback received error!: %i", (int)status);
}
if (up_interrupt_context()) {
// We cannot be in interrupt context
g_dev->recordCommsError();
return ERROR_FAIL;
}
if ((g_dev == nullptr) || (status != STATUS_OK)) {
g_dev->recordCommsError();
return ERROR_FAIL;
}
g_dev->scheduleCollect();
return status;
}
void AFBRS50::ProcessMeasurement(argus_hnd_t *hnd)
void AFBRS50::scheduleCollect()
{
_state = STATE::COLLECT;
ScheduleNow();
}
void AFBRS50::recordCommsError()
{
perf_count(_comms_errors);
}
void AFBRS50::processMeasurement()
{
perf_count(_sample_perf);
argus_results_t res{};
status_t evaluate_status = Argus_EvaluateData(hnd, &res);
status_t evaluate_status = Argus_EvaluateData(_hnd, &res);
if ((evaluate_status == STATUS_OK) && (res.Status == STATUS_OK)) {
uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000);
float result_m = static_cast<float>(result_mm) / 1000.f;
int8_t quality = res.Bin.SignalQuality;
// Signal quality indicates 100% for good signals, 50% and lower for weak signals.
// 1% is an errored signal (not reliable). Signal Quality of 0% is unknown.
if (quality == 1) {
quality = 0;
}
// distance quality check
if (result_m > _max_distance) {
result_m = 0.0;
quality = 0;
}
_current_distance = result_m;
_current_quality = quality;
_px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality);
if ((evaluate_status != STATUS_OK) || (res.Status != STATUS_OK)) {
perf_count(_comms_errors);
return;
}
uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000);
float distance = static_cast<float>(result_mm) / 1000.f;
int8_t quality = res.Bin.SignalQuality;
// Signal quality indicates 100% for good signals, 50% and lower for weak signals.
// 1% is an errored signal (not reliable). Signal Quality of 0% is unknown.
if (quality == 1) {
quality = 0;
}
// TODO: I don't think we need this..
if (distance > _max_distance) {
distance = 0.0;
quality = 0;
}
_current_distance = distance;
_current_quality = quality;
_px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), distance, quality);
}
int AFBRS50::init()
{
// Retry initialization 3 times
for (int32_t i = 0; i < 3; i++) {
if (_hnd != nullptr) {
// retry
Argus_Deinit(_hnd);
Argus_DestroyHandle(_hnd);
_hnd = nullptr;
}
_hnd = Argus_CreateHandle();
if (_hnd == nullptr) {
PX4_ERR("Handle not initialized");
return PX4_ERROR;
}
// Initialize the S2PI hardware required by the API.
S2PI_Init(BROADCOM_AFBR_S50_S2PI_SPI_BUS, SPI_BAUD_RATE);
int32_t mode_param = _p_sens_afbr_mode.get();
if (mode_param < 0 || mode_param > 3) {
PX4_ERR("Invalid mode parameter: %li", mode_param);
return PX4_ERROR;
}
argus_mode_t mode = ARGUS_MODE_SHORT_RANGE;
switch (mode_param) {
case 0:
mode = ARGUS_MODE_SHORT_RANGE;
break;
case 1:
mode = ARGUS_MODE_LONG_RANGE;
break;
case 2:
mode = ARGUS_MODE_HIGH_SPEED_SHORT_RANGE;
break;
case 3:
mode = ARGUS_MODE_HIGH_SPEED_LONG_RANGE;
break;
default:
break;
}
status_t status = Argus_InitMode(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS, mode);
if (status == STATUS_OK) {
uint32_t id = Argus_GetChipID(_hnd);
uint32_t value = Argus_GetAPIVersion();
uint8_t a = (value >> 24) & 0xFFU;
uint8_t b = (value >> 16) & 0xFFU;
uint8_t c = value & 0xFFFFU;
PX4_INFO_RAW("AFBR-S50 Chip ID: %u, API Version: %u v%d.%d.%d\n", (uint)id, (uint)value, a, b, c);
argus_module_version_t mv = Argus_GetModuleVersion(_hnd);
switch (mv) {
case AFBR_S50MV85G_V1:
// FALLTHROUGH
case AFBR_S50MV85G_V2:
// FALLTHROUGH
case AFBR_S50MV85G_V3:
_min_distance = 0.0f;
_max_distance = 10.f;
_px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance);
_px4_rangefinder.set_fov(math::radians(6.f));
PX4_INFO_RAW("AFBR-S50MV85G\n");
break;
case AFBR_S50LV85D_V1:
_min_distance = 0.0f;
_max_distance = 30.f;
_px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance);
_px4_rangefinder.set_fov(math::radians(6.f));
PX4_INFO_RAW("AFBR-S50LV85D\n");
break;
case AFBR_S50LX85D_V1:
_min_distance = 0.0f;
_max_distance = 50.f;
_px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance);
_px4_rangefinder.set_fov(math::radians(6.f));
PX4_INFO_RAW("AFBR-S50LX85D\n");
break;
case AFBR_S50MV68B_V1:
_min_distance = 0.0f;
_max_distance = 10.f;
_px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance);
_px4_rangefinder.set_fov(math::radians(1.f));
PX4_INFO_RAW("AFBR-S50MV68B (v1)\n");
break;
case AFBR_S50MV85I_V1:
_min_distance = 0.0f;
_max_distance = 5.f;
_px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance);
_px4_rangefinder.set_fov(math::radians(6.f));
PX4_INFO_RAW("AFBR-S50MV85I (v1)\n");
break;
case AFBR_S50SV85K_V1:
_min_distance = 0.0f;
_max_distance = 10.f;
_px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance);
_px4_rangefinder.set_fov(math::radians(4.f));
PX4_INFO_RAW("AFBR-S50SV85K (v1)\n");
break;
default:
break;
}
if (_testing) {
_state = STATE::TEST;
} else {
_state = STATE::CONFIGURE;
}
ScheduleDelayed(_measure_interval);
return PX4_OK;
} else {
PX4_ERR("Argus_InitMode failed: %ld", status);
}
if (hrt_absolute_time() < 1_ms) {
PX4_WARN("Power-up time requires at least 1ms!");
}
return PX4_ERROR;
}
void AFBRS50::Run()
{
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
// update parameters from storage
ModuleParams::updateParams();
if (_hnd != nullptr) {
Argus_Deinit(_hnd);
Argus_DestroyHandle(_hnd);
_hnd = nullptr;
}
switch (_state) {
case STATE::TEST: {
if (_testing) {
Argus_VerifyHALImplementation(Argus_GetSPISlave(_hnd));
_testing = false;
_hnd = Argus_CreateHandle();
} else {
_state = STATE::CONFIGURE;
}
}
if (_hnd == nullptr) {
PX4_ERR("Handle not initialized");
return PX4_ERROR;
}
// Initialize the S2PI hardware required by the API.
static constexpr uint32_t SPI_BAUD_RATE = 5000000;
S2PI_Init(BROADCOM_AFBR_S50_S2PI_SPI_BUS, SPI_BAUD_RATE);
// Initialize device with initial mode
status_t status = Argus_InitMode(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS, argusModeFromParameter());
if (status != STATUS_OK) {
PX4_ERR("Argus_InitMode failed: %ld", status);
return PX4_ERROR;
}
uint32_t id = Argus_GetChipID(_hnd);
uint32_t value = Argus_GetAPIVersion();
uint8_t a = (value >> 24) & 0xFFU;
uint8_t b = (value >> 16) & 0xFFU;
uint8_t c = value & 0xFFFFU;
PX4_INFO("AFBR-S50 Chip ID: %u, API Version: %u v%d.%d.%d", (uint)id, (uint)value, a, b, c);
char module_string[20] = {};
argus_module_version_t mv = Argus_GetModuleVersion(_hnd);
float min_distance = 0.f;
float max_distance = 30.f;
float fov_degrees = 6.f;
switch (mv) {
case AFBR_S50MV85G_V1:
case AFBR_S50MV85G_V2:
case AFBR_S50MV85G_V3:
max_distance = 10.f;
fov_degrees = 6.f;
snprintf(module_string, sizeof(module_string), "AFBR-S50MV85G");
break;
case STATE::CONFIGURE: {
_current_rate = (uint32_t)_p_sens_afbr_s_rate.get();
status_t status = set_rate_and_dfm(_current_rate, DFM_MODE_OFF);
if (status != STATUS_OK) {
PX4_ERR("CONFIGURE status not okay: %i", (int)status);
_state = STATE::STOP;
ScheduleNow();
}
status = Argus_SetConfigurationSmartPowerSaveEnabled(_hnd, false);
if (status != STATUS_OK) {
PX4_ERR("Argus_SetConfigurationSmartPowerSaveEnabled status not okay: %i", (int)status);
ScheduleNow();
} else {
_state = STATE::COLLECT;
ScheduleDelayed(_measure_interval);
}
}
case AFBR_S50LV85D_V1:
max_distance = 30.f;
fov_degrees = 6.f;
snprintf(module_string, sizeof(module_string), "AFBR-S50LV85D");
break;
case STATE::COLLECT: {
// 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);
}
}
Evaluate_rate();
}
case AFBR_S50LX85D_V1:
max_distance = 50.f;
fov_degrees = 6.f;
snprintf(module_string, sizeof(module_string), "AFBR-S50LX85D");
break;
case STATE::STOP: {
Argus_StopMeasurementTimer(_hnd);
Argus_Deinit(_hnd);
Argus_DestroyHandle(_hnd);
}
case AFBR_S50MV68B_V1:
max_distance = 10.f;
fov_degrees = 1.f;
snprintf(module_string, sizeof(module_string), "AFBR-S50MV68B");
break;
case AFBR_S50MV85I_V1:
max_distance = 5.f;
fov_degrees = 6.f;
snprintf(module_string, sizeof(module_string), "AFBR-S50MV85I");
break;
case AFBR_S50SV85K_V1:
max_distance = 10.f;
fov_degrees = 4.f;
snprintf(module_string, sizeof(module_string), "AFBR-S50SV85K");
break;
default:
break;
}
ScheduleDelayed(_measure_interval);
PX4_INFO("Module: %s", module_string);
_max_distance = max_distance;
_px4_rangefinder.set_min_distance(min_distance);
_px4_rangefinder.set_max_distance(max_distance);
_px4_rangefinder.set_fov(math::radians(fov_degrees));
_state = STATE::CONFIGURE;
// Initialization Time is 300ms
ScheduleDelayed(350_ms);
return PX4_OK;
}
void AFBRS50::Evaluate_rate()
void AFBRS50::Run()
{
if (_parameter_update_sub.updated()) {
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
ModuleParams::updateParams();
}
switch (_state) {
case STATE::CONFIGURE: {
_current_rate = (uint32_t)_p_sens_afbr_s_rate.get();
status_t status = setRateAndDfm(_current_rate, DFM_MODE_OFF);
if (status != STATUS_OK) {
PX4_ERR("CONFIGURE status not okay: %i", (int)status);
ScheduleDelayed(350_ms);
return;
}
status = Argus_SetConfigurationSmartPowerSaveEnabled(_hnd, false);
if (status != STATUS_OK) {
PX4_ERR("Argus_SetConfigurationSmartPowerSaveEnabled status not okay: %i", (int)status);
// TODO: delay?
ScheduleNow();
return;
}
// Enable interrupt on falling edge
px4_arch_configgpio(BROADCOM_AFBR_S50_S2PI_IRQ);
_state = STATE::TRIGGER;
// TODO: delay after configure?
ScheduleNow();
// ScheduleDelayed(50_ms);
}
break;
case STATE::TRIGGER: {
if (Argus_GetStatus(_hnd) != STATUS_IDLE) {
perf_count(_not_ready_perf);
ScheduleDelayed(10_ms);
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);
if (status != STATUS_OK) {
PX4_ERR("Argus_TriggerMeasurement status not okay: %i", (int)status);
perf_count(_not_ready_perf);
ScheduleDelayed(50_ms);
}
}
break;
case STATE::COLLECT: {
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);
}
break;
default:
break;
}
}
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
if ((_current_distance > 0) && (_current_quality > 0) && ((hrt_absolute_time() - _last_rate_switch) > 1_s)) {
@@ -347,7 +326,7 @@ void AFBRS50::Evaluate_rate()
&& (_current_rate != (uint32_t)_p_sens_afbr_l_rate.get())) {
_current_rate = (uint32_t)_p_sens_afbr_l_rate.get();
status = set_rate_and_dfm(_current_rate, DFM_MODE_8X);
status = setRateAndDfm(_current_rate, DFM_MODE_8X);
if (status != STATUS_OK) {
PX4_ERR("set_rate status not okay: %i", (int)status);
@@ -361,7 +340,7 @@ void AFBRS50::Evaluate_rate()
&& (_current_rate != (uint32_t)_p_sens_afbr_s_rate.get())) {
_current_rate = (uint32_t)_p_sens_afbr_s_rate.get();
status = set_rate_and_dfm(_current_rate, DFM_MODE_OFF);
status = setRateAndDfm(_current_rate, DFM_MODE_OFF);
if (status != STATUS_OK) {
PX4_ERR("set_rate status not okay: %i", (int)status);
@@ -374,28 +353,7 @@ void AFBRS50::Evaluate_rate()
}
}
void AFBRS50::stop()
{
_state = STATE::STOP;
ScheduleNow();
}
int AFBRS50::test()
{
_testing = true;
init();
return PX4_OK;
}
void AFBRS50::print_info()
{
perf_print_counter(_sample_perf);
get_info();
}
status_t AFBRS50::set_rate_and_dfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode)
status_t AFBRS50::setRateAndDfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode)
{
while (Argus_GetStatus(_hnd) != STATUS_IDLE) {
px4_usleep(1_ms);
@@ -423,20 +381,53 @@ status_t AFBRS50::set_rate_and_dfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode)
return status;
} else {
_measure_interval = current_rate;
_measurement_inverval = current_rate;
}
return status;
}
void AFBRS50::get_info()
argus_mode_t AFBRS50::argusModeFromParameter()
{
argus_dfm_mode_t dfm_mode;
Argus_GetConfigurationDFMMode(_hnd, &dfm_mode);
int32_t mode_param = _p_sens_afbr_mode.get();
argus_mode_t mode = ARGUS_MODE_SHORT_RANGE;
if (mode_param < 0 || mode_param > 3) {
PX4_ERR("Invalid mode parameter: %li", mode_param);
return mode;
}
switch (mode_param) {
case 0:
mode = ARGUS_MODE_SHORT_RANGE;
break;
case 1:
mode = ARGUS_MODE_LONG_RANGE;
break;
case 2:
mode = ARGUS_MODE_HIGH_SPEED_SHORT_RANGE;
break;
case 3:
mode = ARGUS_MODE_HIGH_SPEED_LONG_RANGE;
break;
default:
break;
}
return mode;
}
void AFBRS50::printInfo()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_not_ready_perf);
PX4_INFO_RAW("distance: %.3fm\n", (double)_current_distance);
PX4_INFO_RAW("dfm mode: %d\n", dfm_mode);
PX4_INFO_RAW("rate: %u Hz\n", (uint)(1000000 / _measure_interval));
PX4_INFO_RAW("rate: %u Hz\n", (uint)(1000000 / _measurement_inverval));
}
namespace afbrs50
@@ -456,7 +447,6 @@ static int start(const uint8_t rotation)
return PX4_ERROR;
}
// Initialize the sensor.
if (g_dev->init() != PX4_OK) {
PX4_ERR("driver start failed");
delete g_dev;
@@ -474,7 +464,7 @@ static int status()
return PX4_ERROR;
}
g_dev->print_info();
g_dev->printInfo();
return PX4_OK;
}
@@ -495,30 +485,6 @@ static int stop()
return PX4_OK;
}
static int test(const uint8_t rotation)
{
if (g_dev != nullptr) {
PX4_ERR("already started");
return PX4_ERROR;
}
g_dev = new AFBRS50(rotation);
if (g_dev == nullptr) {
PX4_ERR("object instantiate failed");
return PX4_ERROR;
}
if (g_dev->test() != PX4_OK) {
PX4_ERR("driver test failed");
delete g_dev;
g_dev = nullptr;
return PX4_ERROR;
}
return PX4_OK;
}
static int usage()
{
PRINT_MODULE_DESCRIPTION(
@@ -540,7 +506,6 @@ $ afbrs50 stop
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
PRINT_MODULE_USAGE_PARAM_INT('r', 25, 0, 25, "Sensor rotation - downward facing by default", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
return PX4_OK;
}
@@ -581,9 +546,6 @@ extern "C" __EXPORT int afbrs50_main(int argc, char *argv[])
} else if (!strcmp(argv[myoptind], "stop")) {
return afbrs50::stop();
} else if (!strcmp(argv[myoptind], "test")) {
return afbrs50::test(rotation);
}
return afbrs50::usage();
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