Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

drivers: broadcom AFBR fix close to ground false readings

Browse Source
This commit is contained in:
alexklimaj
2024-04-04 14:28:09 -06:00
committed by Daniel Agar
parent b544ea99d5
commit cc11e1fbbf
3 changed files with 132 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
+22 -20
View File
@@ -118,6 +118,8 @@ void AFBRS50::ProcessMeasurement(argus_hnd_t *hnd)
int AFBRS50::init() int AFBRS50::init()
{ {
// Retry initialization 3 times
for (int32_t i = 0; i < 3; i++) {
if (_hnd != nullptr) { if (_hnd != nullptr) {
// retry // retry
Argus_Deinit(_hnd); Argus_Deinit(_hnd);
@@ -142,7 +144,7 @@ int AFBRS50::init()
return PX4_ERROR; return PX4_ERROR;
} }
argus_mode_t mode = ARGUS_MODE_LONG_RANGE; argus_mode_t mode = ARGUS_MODE_SHORT_RANGE;
switch (mode_param) { switch (mode_param) {
case 0: case 0:
@@ -185,7 +187,7 @@ int AFBRS50::init()
// FALLTHROUGH // FALLTHROUGH
case AFBR_S50MV85G_V3: case AFBR_S50MV85G_V3:
_min_distance = 0.08f; _min_distance = 0.0f;
_max_distance = 10.f; _max_distance = 10.f;
_px4_rangefinder.set_min_distance(_min_distance); _px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_max_distance(_max_distance);
@@ -194,7 +196,7 @@ int AFBRS50::init()
break; break;
case AFBR_S50LV85D_V1: case AFBR_S50LV85D_V1:
_min_distance = 0.08f; _min_distance = 0.0f;
_max_distance = 30.f; _max_distance = 30.f;
_px4_rangefinder.set_min_distance(_min_distance); _px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_max_distance(_max_distance);
@@ -203,7 +205,7 @@ int AFBRS50::init()
break; break;
case AFBR_S50LX85D_V1: case AFBR_S50LX85D_V1:
_min_distance = 0.08f; _min_distance = 0.0f;
_max_distance = 50.f; _max_distance = 50.f;
_px4_rangefinder.set_min_distance(_min_distance); _px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_max_distance(_max_distance);
@@ -212,7 +214,7 @@ int AFBRS50::init()
break; break;
case AFBR_S50MV68B_V1: case AFBR_S50MV68B_V1:
_min_distance = 0.08f; _min_distance = 0.0f;
_max_distance = 10.f; _max_distance = 10.f;
_px4_rangefinder.set_min_distance(_min_distance); _px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_max_distance(_max_distance);
@@ -221,7 +223,7 @@ int AFBRS50::init()
break; break;
case AFBR_S50MV85I_V1: case AFBR_S50MV85I_V1:
_min_distance = 0.08f; _min_distance = 0.0f;
_max_distance = 5.f; _max_distance = 5.f;
_px4_rangefinder.set_min_distance(_min_distance); _px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_max_distance(_max_distance);
@@ -230,7 +232,7 @@ int AFBRS50::init()
break; break;
case AFBR_S50SV85K_V1: case AFBR_S50SV85K_V1:
_min_distance = 0.08f; _min_distance = 0.0f;
_max_distance = 10.f; _max_distance = 10.f;
_px4_rangefinder.set_min_distance(_min_distance); _px4_rangefinder.set_min_distance(_min_distance);
_px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_max_distance(_max_distance);
@@ -255,6 +257,7 @@ int AFBRS50::init()
} else { } else {
PX4_ERR("Argus_InitMode failed: %ld", status); PX4_ERR("Argus_InitMode failed: %ld", status);
} }
}
return PX4_ERROR; return PX4_ERROR;
} }
@@ -284,7 +287,7 @@ void AFBRS50::Run()
case STATE::CONFIGURE: { case STATE::CONFIGURE: {
_current_rate = (uint32_t)_p_sens_afbr_s_rate.get(); _current_rate = (uint32_t)_p_sens_afbr_s_rate.get();
status_t status = set_rate(_current_rate); status_t status = set_rate_and_dfm(_current_rate, DFM_MODE_OFF);
if (status != STATUS_OK) { if (status != STATUS_OK) {
PX4_ERR("CONFIGURE status not okay: %i", (int)status); PX4_ERR("CONFIGURE status not okay: %i", (int)status);
@@ -292,14 +295,6 @@ void AFBRS50::Run()
ScheduleNow(); ScheduleNow();
} }
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_SetConfigurationSmartPowerSaveEnabled(_hnd, false); status = Argus_SetConfigurationSmartPowerSaveEnabled(_hnd, false);
if (status != STATUS_OK) { if (status != STATUS_OK) {
@@ -352,7 +347,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())) {
_current_rate = (uint32_t)_p_sens_afbr_l_rate.get(); _current_rate = (uint32_t)_p_sens_afbr_l_rate.get();
status = set_rate(_current_rate); status = set_rate_and_dfm(_current_rate, DFM_MODE_8X);
if (status != STATUS_OK) { if (status != STATUS_OK) {
PX4_ERR("set_rate status not okay: %i", (int)status); PX4_ERR("set_rate status not okay: %i", (int)status);
@@ -366,7 +361,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())) {
_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); status = set_rate_and_dfm(_current_rate, DFM_MODE_OFF);
if (status != STATUS_OK) { if (status != STATUS_OK) {
PX4_ERR("set_rate status not okay: %i", (int)status); PX4_ERR("set_rate status not okay: %i", (int)status);
@@ -400,13 +395,20 @@ void AFBRS50::print_info()
get_info(); get_info();
} }
status_t AFBRS50::set_rate(uint32_t rate_hz) status_t AFBRS50::set_rate_and_dfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode)
{ {
while (Argus_GetStatus(_hnd) != STATUS_IDLE) { while (Argus_GetStatus(_hnd) != STATUS_IDLE) {
px4_usleep(1_ms); px4_usleep(1_ms);
} }
status_t status = Argus_SetConfigurationFrameTime(_hnd, (1000000 / rate_hz)); status_t status = Argus_SetConfigurationDFMMode(_hnd, dfm_mode);
if (status != STATUS_OK) {
PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status);
return status;
}
status = Argus_SetConfigurationFrameTime(_hnd, (1000000 / rate_hz));
if (status != STATUS_OK) { if (status != STATUS_OK) {
PX4_ERR("Argus_SetConfigurationFrameTime status not okay: %i", (int)status); PX4_ERR("Argus_SetConfigurationFrameTime status not okay: %i", (int)status);
src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp
+1 -1
View File
@@ -85,7 +85,7 @@ private:
static status_t measurement_ready_callback(status_t status, argus_hnd_t *hnd); static status_t measurement_ready_callback(status_t status, argus_hnd_t *hnd);
void get_info(); void get_info();
status_t set_rate(uint32_t rate_hz); status_t set_rate_and_dfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode);
argus_hnd_t *_hnd{nullptr}; argus_hnd_t *_hnd{nullptr};
src/drivers/distance_sensor/broadcom/afbrs50/parameters.c
+2 -2
View File
@@ -46,7 +46,7 @@
* @value 2 High Speed Short Range Mode * @value 2 High Speed Short Range Mode
* @value 3 High Speed Long Range Mode * @value 3 High Speed Long Range Mode
*/ */
PARAM_DEFINE_INT32(SENS_AFBR_MODE, 1); PARAM_DEFINE_INT32(SENS_AFBR_MODE, 0);
/** /**
* AFBR Rangefinder Short Range Rate * AFBR Rangefinder Short Range Rate
@@ -85,7 +85,7 @@ PARAM_DEFINE_INT32(SENS_AFBR_L_RATE, 25);
* @group Sensors * @group Sensors
* *
*/ */
PARAM_DEFINE_INT32(SENS_AFBR_THRESH, 5); PARAM_DEFINE_INT32(SENS_AFBR_THRESH, 4);
/** /**