fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 09:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update AFBR to 1.4.4. Remove changing measurement modes. Add parameters. Default to Long range mode"

Browse Source
This commit is contained in:
AlexKlimaj
2023-09-19 19:15:02 -04:00
committed by Daniel Agar
parent 6d71224cdb
commit 7ff00db9c5
39 changed files with 6383 additions and 2957 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
+110 -113
View File
@@ -33,6 +33,7 @@
/* Include Files */
#include "AFBRS50.hpp"
#include "argus_hal_test.h"
#include <lib/drivers/device/Device.hpp>
@@ -42,9 +43,6 @@
/*! Define the SPI baud rate (to be used in the SPI module). */
#define SPI_BAUD_RATE 5000000
#define LONG_RANGE_MODE_HZ 25
#define SHORT_RANGE_MODE_HZ 50
#include "s2pi.h"
#include "timer.h"
#include "argus_hal_test.h"
@@ -52,6 +50,7 @@
AFBRS50 *g_dev{nullptr};
AFBRS50::AFBRS50(uint8_t device_orientation):
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_px4_rangefinder(0, device_orientation)
{
@@ -61,6 +60,7 @@ AFBRS50::AFBRS50(uint8_t device_orientation):
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_AFBRS50;
_px4_rangefinder.set_device_id(device_id.devid);
_px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
}
AFBRS50::~AFBRS50()
@@ -70,12 +70,12 @@ AFBRS50::~AFBRS50()
perf_free(_sample_perf);
}
status_t AFBRS50::measurement_ready_callback(status_t status, void *data)
status_t AFBRS50::measurement_ready_callback(status_t status, argus_hnd_t *hnd)
{
if (!up_interrupt_context()) {
if (status == STATUS_OK) {
if (g_dev) {
g_dev->ProcessMeasurement(data);
g_dev->ProcessMeasurement(hnd);
}
} else {
@@ -86,35 +86,33 @@ status_t AFBRS50::measurement_ready_callback(status_t status, void *data)
return status;
}
void AFBRS50::ProcessMeasurement(void *data)
void AFBRS50::ProcessMeasurement(argus_hnd_t *hnd)
{
if (data != nullptr) {
perf_count(_sample_perf);
perf_count(_sample_perf);
argus_results_t res{};
status_t evaluate_status = Argus_EvaluateData(_hnd, &res, data);
argus_results_t 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;
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);
// 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);
}
}
@@ -137,7 +135,37 @@ int AFBRS50::init()
// Initialize the S2PI hardware required by the API.
S2PI_Init(BROADCOM_AFBR_S50_S2PI_SPI_BUS, SPI_BAUD_RATE);
status_t status = Argus_Init(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS);
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_LONG_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);
@@ -148,7 +176,6 @@ int AFBRS50::init()
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);
argus_laser_type_t lt = Argus_GetLaserType(_hnd);
switch (mv) {
case AFBR_S50MV85G_V1:
@@ -168,19 +195,20 @@ int AFBRS50::init()
case AFBR_S50LV85D_V1:
_min_distance = 0.08f;
if (lt == LASER_H_V2X) {
_max_distance = 50.f;
PX4_INFO_RAW("AFBR-S50LX85D (v2)\n");
} else {
_max_distance = 30.f;
PX4_INFO_RAW("AFBR-S50LV85D (v1)\n");
}
_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.08f;
_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:
@@ -223,6 +251,9 @@ int AFBRS50::init()
ScheduleDelayed(_measure_interval);
return PX4_OK;
} else {
PX4_ERR("Argus_InitMode failed: %ld", status);
}
return PX4_ERROR;
@@ -230,6 +261,15 @@ int AFBRS50::init()
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();
}
switch (_state) {
case STATE::TEST: {
if (_testing) {
@@ -243,7 +283,8 @@ void AFBRS50::Run()
break;
case STATE::CONFIGURE: {
status_t status = set_rate(SHORT_RANGE_MODE_HZ);
_current_rate = (uint32_t)_p_sens_afbr_s_rate.get();
status_t status = set_rate(_current_rate);
if (status != STATUS_OK) {
PX4_ERR("CONFIGURE status not okay: %i", (int)status);
@@ -251,24 +292,18 @@ void AFBRS50::Run()
ScheduleNow();
}
status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_B, DFM_MODE_8X);
status = Argus_SetConfigurationDFMMode(_hnd, DFM_MODE_8X);
if (status != STATUS_OK) {
PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status);
_state = STATE::STOP;
ScheduleNow();
}
status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_A, DFM_MODE_8X);
status = Argus_SetConfigurationSmartPowerSaveEnabled(_hnd, false);
if (status != STATUS_OK) {
PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status);
}
// start in short range mode
_mode = ARGUS_MODE_B;
set_mode(_mode);
if (status != STATUS_OK) {
PX4_ERR("CONFIGURE status not okay: %i", (int)status);
PX4_ERR("Argus_SetConfigurationSmartPowerSaveEnabled status not okay: %i", (int)status);
ScheduleNow();
} else {
@@ -288,7 +323,7 @@ void AFBRS50::Run()
}
}
UpdateMode();
Evaluate_rate();
}
break;
@@ -306,49 +341,41 @@ void AFBRS50::Run()
ScheduleDelayed(_measure_interval);
}
void AFBRS50::UpdateMode()
void AFBRS50::Evaluate_rate()
{
// only update mode if _current_distance is a valid measurement
if ((_current_distance > 0) && (_current_quality > 0)) {
// 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)) {
if ((_current_distance >= _long_range_threshold) && (_mode != ARGUS_MODE_A)) {
// change to long range mode
argus_mode_t mode = ARGUS_MODE_A;
status_t status = set_mode(mode);
status_t status = STATUS_OK;
if (status != STATUS_OK) {
PX4_ERR("set_mode status not okay: %i", (int)status);
}
if ((_current_distance >= (_p_sens_afbr_thresh.get() + _p_sens_afbr_hyster.get()))
&& (_current_rate != (uint32_t)_p_sens_afbr_l_rate.get())) {
status = set_rate(LONG_RANGE_MODE_HZ);
_current_rate = (uint32_t)_p_sens_afbr_l_rate.get();
status = set_rate(_current_rate);
if (status != STATUS_OK) {
PX4_ERR("set_rate status not okay: %i", (int)status);
} else {
PX4_INFO("switched to long range rate: %i", (int)_current_rate);
_last_rate_switch = hrt_absolute_time();
}
status = set_rate(LONG_RANGE_MODE_HZ);
} else if ((_current_distance <= (_p_sens_afbr_thresh.get() - _p_sens_afbr_hyster.get()))
&& (_current_rate != (uint32_t)_p_sens_afbr_s_rate.get())) {
_current_rate = (uint32_t)_p_sens_afbr_s_rate.get();
status = set_rate(_current_rate);
if (status != STATUS_OK) {
PX4_ERR("set_rate status not okay: %i", (int)status);
}
} else if ((_current_distance <= _short_range_threshold) && (_mode != ARGUS_MODE_B)) {
// change to short range mode
argus_mode_t mode = ARGUS_MODE_B;
status_t status = set_mode(mode);
if (status != STATUS_OK) {
PX4_ERR("set_mode status not okay: %i", (int)status);
}
status = set_rate(SHORT_RANGE_MODE_HZ);
if (status != STATUS_OK) {
PX4_ERR("set_rate status not okay: %i", (int)status);
} else {
PX4_INFO("switched to short range rate: %i", (int)_current_rate);
_last_rate_switch = hrt_absolute_time();
}
}
ScheduleDelayed(1000_ms); // don't switch again for at least 1 second
}
}
@@ -373,33 +400,6 @@ void AFBRS50::print_info()
get_info();
}
status_t AFBRS50::set_mode(argus_mode_t mode)
{
while (Argus_GetStatus(_hnd) != STATUS_IDLE) {
px4_usleep(1_ms);
}
status_t status = Argus_SetConfigurationMeasurementMode(_hnd, mode);
if (status != STATUS_OK) {
PX4_ERR("Argus_SetConfigurationMeasurementMode status not okay: %i", (int)status);
return status;
}
argus_mode_t current_mode;
status = Argus_GetConfigurationMeasurementMode(_hnd, &current_mode);
if (status != STATUS_OK) {
PX4_ERR("Argus_GetConfigurationMeasurementMode status not okay: %i", (int)status);
return status;
} else {
_mode = current_mode;
}
return status;
}
status_t AFBRS50::set_rate(uint32_t rate_hz)
{
while (Argus_GetStatus(_hnd) != STATUS_IDLE) {
@@ -429,13 +429,10 @@ status_t AFBRS50::set_rate(uint32_t rate_hz)
void AFBRS50::get_info()
{
argus_mode_t current_mode;
argus_dfm_mode_t dfm_mode;
Argus_GetConfigurationMeasurementMode(_hnd, &current_mode);
Argus_GetConfigurationDFMMode(_hnd, current_mode, &dfm_mode);
Argus_GetConfigurationDFMMode(_hnd, &dfm_mode);
PX4_INFO_RAW("distance: %.3fm\n", (double)_current_distance);
PX4_INFO_RAW("mode: %d\n", current_mode);
PX4_INFO_RAW("dfm mode: %d\n", dfm_mode);
PX4_INFO_RAW("rate: %u Hz\n", (uint)(1000000 / _measure_interval));
}