Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_InertialSensor: count found backends correctly #12850

Closed
wants to merge 1 commit into from
Closed

AP_InertialSensor: count found backends correctly #12850

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
19 changes: 11 additions & 8 deletions libraries/AP_InertialSensor/AP_InertialSensor.cpp
View file
Open in desktop
Expand Up @@ -673,7 +673,7 @@ AP_InertialSensor::init(uint16_t sample_rate)
}
}

bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend, bool enabled)
{

if (!backend) {
Expand All @@ -682,7 +682,12 @@ bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
if (_backend_count == INS_MAX_BACKENDS) {
AP_HAL::panic("Too many INS backends");
}
_backends[_backend_count++] = backend;
if (enabled) {
_backends[_backend_count++] = backend;
}
else {
delete backend;
}
return true;
}

Expand Down Expand Up @@ -712,18 +717,16 @@ AP_InertialSensor::detect_backends(void)
}
#endif

uint8_t probe_count = 0;
uint8_t enable_mask = uint8_t(_enable_mask.get());
uint8_t found_mask = 0;
uint8_t found_count = 0;

/*
use ADD_BACKEND() macro to allow for INS_ENABLE_MASK for enabling/disabling INS backends
*/
#define ADD_BACKEND(x) do { \
if (((1U<<probe_count)&enable_mask) && _add_backend(x)) { \
found_mask |= (1U<<probe_count); \
if (_add_backend(x, ((1U<<found_count)&enable_mask))) { \
found_count++; \
} \
probe_count++; \
} while (0)

// macro for use by HAL_INS_PROBE_LIST
Expand Down Expand Up @@ -854,7 +857,7 @@ AP_InertialSensor::detect_backends(void)
break;

case AP_BoardConfig::PX4_BOARD_PCNC1:
_add_backend(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
break;

default:
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_InertialSensor/AP_InertialSensor.h
View file
Open in desktop
Expand Up @@ -374,7 +374,7 @@ class AP_InertialSensor : AP_AccelCal_Client

private:
// load backend drivers
bool _add_backend(AP_InertialSensor_Backend *backend);
bool _add_backend(AP_InertialSensor_Backend *backend, bool enabled);
void _start_backends();
AP_InertialSensor_Backend *_find_backend(int16_t backend_id, uint8_t instance);

Expand Down