Skip to content

Commit

Permalink
Update AFBR to 1.4.4. Remove changing measurement modes. Add paramete…
Browse files Browse the repository at this point in the history 
…rs. Default to Long range mode"
  • Loading branch information
@AlexKlimaj @dagar
AlexKlimaj authored and dagar committed Sep 19, 2023
1 parent 6d71224 commit 7ff00db
Show file tree
Hide file tree
Showing 39 changed files with 6,497 additions and 3,071 deletions.
217 changes: 107 additions & 110 deletions src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@

/* Include Files */
#include "AFBRS50.hpp"
#include "argus_hal_test.h"

#include <lib/drivers/device/Device.hpp>

Expand All @@ -42,16 +43,14 @@
/*! 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"

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)
{
Expand All @@ -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()
Expand All @@ -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 {
Expand All @@ -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;
}
// 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;
}

_current_distance = result_m;
_current_quality = quality;
_px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality);
// 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);
}
}

Expand All @@ -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);
Expand All @@ -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:
Expand All @@ -168,19 +195,20 @@ int AFBRS50::init()

case AFBR_S50LV85D_V1:
_min_distance = 0.08f;
_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;

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");
}

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:
Expand Down Expand Up @@ -223,13 +251,25 @@ int AFBRS50::init()

ScheduleDelayed(_measure_interval);
return PX4_OK;

} else {
PX4_ERR("Argus_InitMode failed: %ld", status);
}

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();
}

switch (_state) {
case STATE::TEST: {
if (_testing) {
Expand All @@ -243,32 +283,27 @@ 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);
_state = STATE::STOP;
ScheduleNow();
}

status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_B, DFM_MODE_8X);

if (status != STATUS_OK) {
PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status);
}

status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_A, 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();
}

// start in short range mode
_mode = ARGUS_MODE_B;
set_mode(_mode);
status = Argus_SetConfigurationSmartPowerSaveEnabled(_hnd, false);

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 {
Expand All @@ -288,7 +323,7 @@ void AFBRS50::Run()
}
}

UpdateMode();
Evaluate_rate();
}
break;

Expand All @@ -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);
}

status = set_rate(LONG_RANGE_MODE_HZ);

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();
}

} 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);
}
} else if ((_current_distance <= (_p_sens_afbr_thresh.get() - _p_sens_afbr_hyster.get()))
&& (_current_rate != (uint32_t)_p_sens_afbr_s_rate.get())) {

status = set_rate(SHORT_RANGE_MODE_HZ);
_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 {
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
}
}

Expand All @@ -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) {
Expand Down Expand Up @@ -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));
}
Expand Down
Loading

0 comments on commit 7ff00db

Please sign in to comment.