feat(afbrs50): static rate/DFM profiles, pipeline hardening, range offset calibration (#28454)

* feat(afbrs50): static rate/DFM profiles, pipeline hardening, range offset calibration

The distance-based short/long range switching only re-evaluates after a
valid measurement, so once the target leaves the range the driver latches
short-range mode and the sensor stays blind; frames that fail evaluation
were never published, leaving consumers on the stale last value. Flight
characterization of the LV85D and LX85D showed frame rate is the dominant
range knob and DFM 4X beats the mode-default 8X on validity, spread and
wrong-window returns at every rate, so the switching is replaced by
SENS_AFBR_RATE / SENS_AFBR_DFM / SENS_AFBR_PROF with per-module defaults,
SENS_AFBR_MODE gains Auto (module default) with fallback when the API
rejects a mode, and the rate is clamped to the API's 5 Hz frame-time floor
that previously put CONFIGURE in a silent retry loop.

Invalid or quality-gated (SENS_AFBR_QMIN) frames now publish
max_distance + 1 with quality 0 so uavcannode emits TOO_FAR, and the max
distance is bounded by the configured unambiguous range.

Pipeline fixes: measurementReadyCallback dereferenced g_dev after its null
check, stop() deleted the object while a callback could still arrive, the
DRDY interrupt stayed bound to the destroyed handle, S2PI_Abort left the
bus BUSY forever, S2PI_Init leaked on restart, setRateAndDfm spun
unbounded, and a lost completion callback or wedged device stalled the
state machine for good. Non-OK result codes are counted by name for
'afbrs50 status'.

'afbrs50 cal' runs the vendor absolute range offset calibration on a
low-priority task (the sequence busy-waits and would starve the IWDG
feeder on wq:uavcan) and persists the offsets to SENS_AFBR_OFS_LO/HI.

* fix(afbrs50): evaluate every completed frame and guard CONFIGURE against stale completions

Argus_EvaluateData is what releases the API's raw data buffer, and the
API refuses new measurements and rejects configuration writes once two
buffers are held. An error-status callback skipped it, so two bad frames
wedged the driver into the stall recovery with nothing published.

The abort completion of that recovery, arriving from the SPI thread,
could overwrite CONFIGURE with TRIGGER and skip the reconfigure.

Also note the removed parameters in the 1.18 release notes.

* refactor(afbrs50): run the driver in its own task and the SPI transfers on the bus work queue

The transfer work item ran on wq:SPI0 (which does not exist on STM32
targets) purely for its near-top priority: before API 1.6.6 a DRDY
firing within ~60 us of the last SPI clock was lost unless the transfer
callback had already run. Since 1.6.6 ADS_SPI_Callback re-checks the
IRQ pin and recovers a DRDY that arrives before the callback, so the
deadline is gone and the transfer item can live on the work queue of
the bus the sensor actually sits on, at its conventional priority. The
blocking exchange stays on a work queue because the API requests
transfers from hrt interrupt context.

The state machine cannot share that thread: the API's configuration
calls spin in ADS_AwaitIdle until the transfer they queued completes.
It previously borrowed hp_default, whose 2800 byte stack
Argus_EvaluateData overflows and whose priority puts the driver's
blocking configuration waits ahead of dshot and pwm_out. It now runs as
a SCHED_PRIORITY_SLOW_DRIVER task woken by the completion callback
through a semaphore, so the range offset calibration no longer needs
its own task either: the driver drops to SCHED_PRIORITY_DEFAULT for the
duration of the sequence, below the wq:uavcan IWDG feeder, and a stop
request aborts it.

* fix(afbrs50): publish only NO_OBJECT as too far, reinit on stuck CONFIGURE, protect calibration

Every evaluation failure and quality-gated frame went out as max_distance + 1, which collision prevention clamps to max_distance and enters as free space regardless of signal_quality, so a sensor fault on a horizontal mount cleared a real obstacle. Only the device's own STATUS_ARGUS_NO_OBJECT is published that way now; errors and gated frames are counted and left to the consumers' stream timeouts.

CONFIGURE drains a raw buffer an abort may leave behind, since the API rejects configuration writes until it is evaluated, and falls back to Argus_ReinitMode after ten consecutive failures because a sticky error status never returns to IDLE on its own.

The vendor calibration sequence blocks longer than ModuleBase's 5 s stop deadline while holding pointers into the task stack, so 'afbrs50 stop' is refused while it runs and a stop clears a not-yet-started request. The sequence also rewrites the per-pixel offset tables, which cannot be persisted; they are restored afterwards so the sensor runs in the state the stored global offsets re-create at boot. The destructor now also stops the API's periodic timer, the last path that could reach the completion callback after ModuleBase has deleted the instance.

* fix(afbrs50): publish invalid frames with signal quality 0 instead of dropping them

Dropping errored and quality-gated frames left a receiver unable to tell a sensor returning invalid readings from one that fell off the bus. Every frame is published again with signal_quality 0 marking the invalid ones. The distance sent with it is chosen for collision prevention, which ignores quality: NO_OBJECT stays beyond max_distance (free space, TOO_FAR on DroneCAN), errors carry min_distance (discarded, UNDEFINED on DroneCAN), and a gated frame keeps its measured distance with the quality floored to 0.
This commit is contained in:
Jacob Dahl
2026-09-08 20:12:16 -06:00
committed by GitHub
parent c1808fb460
commit bb75224d6d
9 changed files with 1414 additions and 348 deletions
@@ -7,10 +7,6 @@ param set-default IMU_GYRO_RATEMAX 1000
param set-default SENS_FLOW_RATE 150
param set-default SENS_IMU_CLPNOTI 0
param set-default SENS_AFBR_S_RATE 25
param set-default SENS_AFBR_L_RATE 5
param set-default SENS_AFBR_MODE 1
# Internal SPI
paa3905 -s start -Y 180
-6
View File
@@ -3,12 +3,6 @@
# board sensors init
#------------------------------------------------------------------------------
param set-default SENS_AFBR_S_RATE 25
param set-default SENS_AFBR_L_RATE 5
param set-default SENS_AFBR_MODE 1
param set-default SENS_AFBR_THRESH 2
param set-default SENS_AFBR_HYSTER 1
param set-default MAV_SYS_ID 158
param set-default MAV_COMP_ID 158
+2
View File
@@ -84,6 +84,8 @@ For users upgrading from v1.17, please take a moment to review the following bef
11. **Update AFBR-S50 startup scripts.**
The `-r` command-line rotation flag is removed; set the mounting orientation via [SENS_AFBR_ROT](../advanced_config/parameter_reference.md#SENS_AFBR_ROT) instead.
([PX4-Autopilot#27385](https://github.com/PX4/PX4-Autopilot/pull/27385))
`SENS_AFBR_S_RATE`, `SENS_AFBR_L_RATE`, `SENS_AFBR_THRESH` and `SENS_AFBR_HYSTER` are removed with the distance-based range switching; rate and dual frequency mode are now set per module by [SENS_AFBR_PROF](../advanced_config/parameter_reference.md#SENS_AFBR_PROF), or explicitly with [SENS_AFBR_RATE](../advanced_config/parameter_reference.md#SENS_AFBR_RATE) and [SENS_AFBR_DFM](../advanced_config/parameter_reference.md#SENS_AFBR_DFM).
([PX4-Autopilot#28454](https://github.com/PX4/PX4-Autopilot/pull/28454))
## Hardware Support
File diff suppressed because it is too large Load Diff
File diff suppressed because it is too large Load Diff
File diff suppressed because it is too large Load Diff
@@ -7,6 +7,7 @@
#include <board_config.h>
#include <lib/drivers/device/Device.hpp>
#include <nuttx/spi/spi.h>
#include <drivers/drv_hrt.h>
@@ -15,6 +16,7 @@
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
/*! A structure to hold all internal data required by the S2PI module. */
typedef struct {
@@ -41,6 +43,17 @@ typedef struct {
uint8_t *spi_rx_data;
size_t spi_frame_size;
/*! Incremented by S2PI_Abort to invalidate a transfer that is already
* in flight on the work queue. */
volatile uint8_t Generation;
/*! Set while the work queue thread is inside the SPI exchange. */
volatile bool InFlight;
/*! Callback of an aborted in-flight transfer, invoked with ERROR_ABORTED
* once the exchange physically completes. */
s2pi_callback_t AbortedCallback;
/*! The mapping of the GPIO blocks and pins for this device. */
const uint32_t GPIOs[ S2PI_IRQ + 1 ];
}
@@ -59,7 +72,7 @@ static perf_counter_t irq_perf = NULL;
class AFBRS50_SPI : public px4::ScheduledWorkItem
{
public:
AFBRS50_SPI();
explicit AFBRS50_SPI(const px4::wq_config_t &wq_config);
void schedule_now();
void schedule_clear();
@@ -69,42 +82,61 @@ private:
};
AFBRS50_SPI::AFBRS50_SPI():
// WARNING: SPI0 only exists for nxp and raspberry pi, so we hijack it here
// 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)
AFBRS50_SPI::AFBRS50_SPI(const px4::wq_config_t &wq_config):
// Transfers can be requested from interrupt context (the API's periodic
// timer runs on an hrt callout), so the blocking exchange is deferred to
// the work queue of the bus the sensor sits on. Since API v1.6.6 the
// library recovers a data-ready interrupt that fires before the transfer
// callback has run, so the callback is no longer deadline-critical and
// needs no elevated thread priority.
ScheduledWorkItem(MODULE_NAME, wq_config)
{
// Anything to do?
}
void AFBRS50_SPI::Run()
{
IRQ_LOCK();
if (s2pi_.Status != STATUS_BUSY) {
// Transfer was aborted before it started.
IRQ_UNLOCK();
return;
}
const uint8_t generation = s2pi_.Generation;
uint8_t *tx_data = s2pi_.spi_tx_data;
uint8_t *rx_data = s2pi_.spi_rx_data;
const size_t frame_size = s2pi_.spi_frame_size;
s2pi_.InFlight = true;
IRQ_UNLOCK();
px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 0);
SPI_EXCHANGE(s2pi_.spidev, s2pi_.spi_tx_data, s2pi_.spi_rx_data, s2pi_.spi_frame_size);
SPI_EXCHANGE(s2pi_.spidev, tx_data, rx_data, 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;
s2pi_.InFlight = false;
if (s2pi_.Callback != 0) {
s2pi_callback_t callback = s2pi_.Callback;
s2pi_.Callback = 0;
callback(STATUS_OK, s2pi_.CallbackData);
if (s2pi_.Generation == generation) {
s2pi_.Status = STATUS_IDLE;
if (s2pi_.Callback != 0) {
s2pi_callback_t callback = s2pi_.Callback;
s2pi_.Callback = 0;
callback(STATUS_OK, s2pi_.CallbackData);
}
} else {
// Aborted mid-exchange: the bus only now became free, so complete
// the abort here (see S2PI_Abort).
s2pi_.Status = STATUS_IDLE;
if (s2pi_.AbortedCallback != 0) {
s2pi_callback_t callback = s2pi_.AbortedCallback;
s2pi_.AbortedCallback = 0;
callback(ERROR_ABORTED, s2pi_.CallbackData);
}
}
IRQ_UNLOCK();
@@ -137,8 +169,6 @@ static AFBRS50_SPI *_spi_iface = nullptr;
*****************************************************************************/
status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps)
{
(void)defaultSlave;
px4_arch_configgpio(BROADCOM_AFBR_S50_S2PI_CS);
s2pi_.spidev = px4_spibus_initialize(BROADCOM_AFBR_S50_S2PI_SPI_BUS);
@@ -159,9 +189,17 @@ status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps)
// 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");
if (irq_perf == NULL) {
irq_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": irq callback");
}
_spi_iface = new AFBRS50_SPI();
if (_spi_iface == nullptr) {
device::Device::DeviceId device_id{};
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SPI;
device_id.devid_s.bus = defaultSlave;
_spi_iface = new AFBRS50_SPI(px4::device_bus_to_wq(device_id.devid));
}
return S2PI_SetBaudRate(baudRate_Bps);
}
@@ -448,18 +486,42 @@ status_t S2PI_Abort(s2pi_slave_t slave)
{
(void)slave;
status_t status = s2pi_.Status;
IRQ_LOCK();
/* Check if something is ongoing. */
if (status == STATUS_IDLE) {
if (s2pi_.Status != STATUS_BUSY) {
IRQ_UNLOCK();
return STATUS_OK;
}
/* Abort SPI transfer. */
if (status == STATUS_BUSY) {
s2pi_.Generation++;
if (s2pi_.InFlight) {
/* The exchange is physically on the wires and cannot be stopped
* (and must not be waited on: abort may run in interrupt context).
* Keep the status BUSY so nobody touches the bus and defer the
* ERROR_ABORTED completion to the work item's tail. A repeated
* abort leaves the already-stashed callback in place. */
if (s2pi_.Callback != 0) {
s2pi_.AbortedCallback = s2pi_.Callback;
s2pi_.Callback = 0;
}
} else {
/* Not started yet: cancel the pending work item and complete. */
_spi_iface->schedule_clear();
s2pi_.Status = STATUS_IDLE;
s2pi_callback_t callback = s2pi_.Callback;
s2pi_.Callback = 0;
if (callback != 0) {
callback(ERROR_ABORTED, s2pi_.CallbackData);
}
}
IRQ_UNLOCK();
return STATUS_OK;
}
@@ -45,6 +45,7 @@ px4_add_module(
Inc
SRCS
AFBRS50.cpp
AFBRS50_Calibration.cpp
AFBRS50.hpp
API/Src/irq.c
API/Src/s2pi.cpp
@@ -4,58 +4,73 @@ parameters:
definitions:
SENS_AFBR_MODE:
description:
short: AFBR Rangefinder Mode
long: This parameter defines the mode of the AFBR Rangefinder.
short: AFBR Rangefinder Measurement Mode
long: |-
Auto selects the module's default measurement mode as defined by the
AFBR-S50 API (LV85D/LX85D: Long Range). A mode the API rejects for
the detected module falls back to the module default.
type: enum
values:
-1: Auto (module default)
0: Short Range Mode
1: Long Range Mode
2: High Speed Short Range Mode
3: High Speed Long Range Mode
4: High Precision Short Range Mode
default: -1
reboot_required: true
min: -1
max: 4
SENS_AFBR_RATE:
description:
short: AFBR Rangefinder Measurement Rate
long: |-
0 selects the per-module default: on the LV85D and LX85D the rate of
the SENS_AFBR_PROF profile, on other modules the measurement mode's
default frame time. Lower rates increase the exposure budget per
frame and thus the radiometric range. The API limits the frame time
to 200 ms, so values below 5 Hz are clamped to 5 Hz.
type: int32
default: 0
unit: Hz
reboot_required: true
min: 0
max: 100
SENS_AFBR_DFM:
description:
short: AFBR Rangefinder Dual Frequency Mode
long: |-
Dual frequency mode multiplies the module's base unambiguous range
(LV85D 12.5 m, LX85D 25 m) by 4 or 8, at the cost of frame time and,
at low signal, of wrong-window returns when the subframes disagree.
Auto selects DFM 4X on the LV85D and LX85D, which measured better
than the mode-default 8X on validity, spread and wrong-window returns
at every rate flown, and the measurement mode's default elsewhere.
type: enum
values:
-1: Auto
0: DFM Off
1: DFM 4X
2: DFM 8X
default: -1
reboot_required: true
min: -1
max: 2
SENS_AFBR_SNM:
description:
short: AFBR Rangefinder Shot Noise Monitor Mode
long: This parameter defines the mode of the AFBR Rangefinder's shot noise
monitor.
type: enum
values:
0: Static Indoor Mode
1: Static Outdoor Mode
2: Dynamic Mode
3: Dynamic Plus Mode
default: 3
reboot_required: true
min: 0
max: 3
SENS_AFBR_S_RATE:
description:
short: AFBR Rangefinder Short Range Rate
long: This parameter defines measurement rate of the AFBR Rangefinder in short
range mode.
type: int32
default: 50
min: 1
max: 100
SENS_AFBR_L_RATE:
description:
short: AFBR Rangefinder Long Range Rate
long: This parameter defines measurement rate of the AFBR Rangefinder in long
range mode.
type: int32
default: 25
min: 1
max: 100
SENS_AFBR_THRESH:
description:
short: AFBR Rangefinder Short/Long Range Threshold
long: |-
This parameter defines the threshold for switching between short and long range mode.
The mode will switch from short to long range when the distance is greater than the threshold plus the hysteresis.
The mode will switch from long to short range when the distance is less than the threshold minus the hysteresis.
type: int32
default: 4
unit: m
min: 1
max: 50
SENS_AFBR_HYSTER:
description:
short: AFBR Rangefinder Short/Long Range Threshold Hysteresis
long: This parameter defines the hysteresis for switching between short and
long range mode.
type: int32
default: 1
unit: m
min: 1
max: 10
SENS_AFBR_ROT:
description:
short: AFBR Rangefinder Orientation
@@ -76,3 +91,59 @@ parameters:
reboot_required: true
min: 0
max: 25
SENS_AFBR_PROF:
description:
short: AFBR Rangefinder Performance Profile
long: |-
Trades detection range against update rate on the LV85D and LX85D;
other modules ignore it. Range: the highest rate that does not cost
reliable range (LV85D 20 Hz, LX85D 15 Hz). Fast: the module's native
rate (LV85D 50 Hz, LX85D 25 Hz) for optical flow and terrain
following near the ground; costs a few metres of reliable range in
bright light, none in low ambient light. Both profiles use DFM 4X.
An explicit SENS_AFBR_RATE or SENS_AFBR_DFM overrides the profile.
type: enum
values:
0: Range
1: Fast
default: 0
reboot_required: true
min: 0
max: 1
SENS_AFBR_QMIN:
description:
short: AFBR Rangefinder Minimum Signal Quality
long: |-
Measurements with a signal quality below this value are published
with signal quality 0 (invalid) so consumers drop them but still see
the sensor alive. Raising it rejects unreliable readings but also the
weak long-range returns, since those are the low quality ones.
0 publishes every measurement with its reported quality.
type: int32
default: 0
min: 0
max: 100
SENS_AFBR_OFS_LO:
description:
short: AFBR Rangefinder Range Offset (low power)
long: |-
Global range offset for the low laser power stage, applied on top of
the factory calibration at startup. Written by 'afbrs50 cal'.
0 leaves the factory offset unchanged.
type: float
default: 0.0
unit: m
decimal: 5
reboot_required: true
SENS_AFBR_OFS_HI:
description:
short: AFBR Rangefinder Range Offset (high power)
long: |-
Global range offset for the high laser power stage, applied on top of
the factory calibration at startup. Written by 'afbrs50 cal'.
0 leaves the factory offset unchanged.
type: float
default: 0.0
unit: m
decimal: 5
reboot_required: true