Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: fixed handling of more than 3 accel/gyro instances

Browse Source 
allows for first 3 to work without a panic
This commit is contained in:
Andrew Tridgell 2021-03-18 12:36:41 +11:00
parent fd3bf0f639
commit b3ed4f4b12
17 changed files with 91 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -676,11 +676,11 @@ AP_InertialSensor *AP_InertialSensor::get_singleton()
/*
register a new gyro instance
*/
uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz,
uint32_t id)
bool AP_InertialSensor::register_gyro(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id)
{
if (_gyro_count == INS_MAX_INSTANCES) {
AP_HAL::panic("Too many gyros");
gcs().send_text(MAV_SEVERITY_WARNING, "Failed to register gyro id %u", unsigned(id));
return false;
}
_gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
@ -704,17 +704,19 @@ uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz,
}
#endif
return _gyro_count++;
instance = _gyro_count++;
return true;
}
/*
register a new accel instance
*/
uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz,
uint32_t id)
bool AP_InertialSensor::register_accel(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id)
{
if (_accel_count == INS_MAX_INSTANCES) {
AP_HAL::panic("Too many accels");
gcs().send_text(MAV_SEVERITY_WARNING, "Failed to register accel id %u", unsigned(id));
return false;
}
_accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
@ -742,7 +744,8 @@ uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz,
_accel_id[_accel_count].save();
#endif
return _accel_count++;
instance = _accel_count++;
return true;
}
/*

4
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -96,8 +96,8 @@ public:
/// Register a new gyro/accel driver, allocating an instance
/// number
uint8_t register_gyro(uint16_t raw_sample_rate_hz, uint32_t id);
uint8_t register_accel(uint16_t raw_sample_rate_hz, uint32_t id);
bool register_gyro(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id);
bool register_accel(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id);
// a function called by the main thread at the main loop rate:
void periodic();

6
libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
View File

@ -89,8 +89,10 @@ AP_InertialSensor_ADIS1647x::probe(AP_InertialSensor &imu,
void AP_InertialSensor_ADIS1647x::start()
{
accel_instance = _imu.register_accel(BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X));
gyro_instance = _imu.register_gyro(BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X));
if (!_imu.register_accel(accel_instance, BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X)) ||
!_imu.register_gyro(gyro_instance, BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X))) {
return;
}
// setup sensor rotations from probe()
set_gyro_orientation(gyro_instance, rotation);

6
libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp
View File

@ -101,8 +101,10 @@ AP_InertialSensor_BMI055::probe(AP_InertialSensor &imu,
void AP_InertialSensor_BMI055::start()
{
accel_instance = _imu.register_accel(ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI055));
gyro_instance = _imu.register_gyro(GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI055));
if (!_imu.register_accel(accel_instance, ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI055)) ||
!_imu.register_gyro(gyro_instance, GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI055))) {
return;
}
// setup sensor rotations from probe()
set_gyro_orientation(gyro_instance, rotation);

6
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
View File

@ -97,8 +97,10 @@ AP_InertialSensor_BMI088::probe(AP_InertialSensor &imu,
void AP_InertialSensor_BMI088::start()
{
accel_instance = _imu.register_accel(ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI088));
gyro_instance = _imu.register_gyro(GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI088));
if (!_imu.register_accel(accel_instance, ACCEL_BACKEND_SAMPLE_RATE, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI088)) ||
!_imu.register_gyro(gyro_instance, GYRO_BACKEND_SAMPLE_RATE, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI088))) {
return;
}
// setup sensor rotations from probe()
set_gyro_orientation(gyro_instance, rotation);

6
libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
View File

@ -178,8 +178,10 @@ void AP_InertialSensor_BMI160::start()
_dev->get_semaphore()->give();
_accel_instance = _imu.register_accel(BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160));
_gyro_instance = _imu.register_gyro(BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160));
if (!_imu.register_accel(_accel_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160)) ||
!_imu.register_gyro(_gyro_instance, BMI160_ODR_TO_HZ(BMI160_ODR), _dev->get_bus_id_devtype(DEVTYPE_BMI160))) {
return;
}
/* Call _poll_data() at 1kHz */
_dev->register_periodic_callback(1000,

21
libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.cpp
View File

@ -14,6 +14,9 @@ AP_InertialSensor_ExternalAHRS::AP_InertialSensor_ExternalAHRS(AP_InertialSensor
void AP_InertialSensor_ExternalAHRS::handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt)
{
if (!started) {
return;
}
Vector3f accel = pkt.accel;
Vector3f gyro = pkt.gyro;
@ -29,18 +32,22 @@ void AP_InertialSensor_ExternalAHRS::handle_external(const AP_ExternalAHRS::ins_
bool AP_InertialSensor_ExternalAHRS::update(void)
{
update_accel(accel_instance);
update_gyro(gyro_instance);
return true;
if (started) {
update_accel(accel_instance);
update_gyro(gyro_instance);
}
return started;
}
void AP_InertialSensor_ExternalAHRS::start()
{
const float rate = AP::externalAHRS().get_IMU_rate();
gyro_instance = _imu.register_gyro(rate,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 1, DEVTYPE_SERIAL));
accel_instance = _imu.register_accel(rate,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 2, DEVTYPE_SERIAL));
if (_imu.register_gyro(gyro_instance, rate,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 1, DEVTYPE_SERIAL)) &&
_imu.register_accel(accel_instance, rate,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL, serial_port, 2, DEVTYPE_SERIAL))) {
started = true;
}
}
void AP_InertialSensor_ExternalAHRS::accumulate()

1
libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h
View File

@ -23,6 +23,7 @@ private:
uint8_t gyro_instance;
uint8_t accel_instance;
const uint8_t serial_port;
bool started;
};
#endif // HAL_EXTERNAL_AHRS_ENABLED

5
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

@ -27,8 +27,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu
bool AP_InertialSensor_HIL::_init_sensor(void)
{
// grab the used instances
_imu.register_gyro(1200, 1);
_imu.register_accel(1200, 1);
uint8_t instance;
_imu.register_gyro(instance, 1200, 1);
_imu.register_accel(instance, 1200, 1);
_imu.set_hil_mode();

21
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -196,7 +196,7 @@ bool AP_InertialSensor_Invensense::_has_auxiliary_bus()
void AP_InertialSensor_Invensense::start()
{
_dev->get_semaphore()->take_blocking();
WITH_SEMAPHORE(_dev->get_semaphore());
// initially run the bus at low speed
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
@ -284,8 +284,10 @@ void AP_InertialSensor_Invensense::start()
break;
}
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(gdev));
_accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(adev));
if (!_imu.register_gyro(_gyro_instance, 1000, _dev->get_bus_id_devtype(gdev)) ||
!_imu.register_accel(_accel_instance, 1000, _dev->get_bus_id_devtype(adev))) {
return;
}
// setup ODR and on-sensor filtering
_set_filter_register();
@ -387,8 +389,6 @@ void AP_InertialSensor_Invensense::start()
// now that we have initialised, we set the bus speed to high
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
_dev->get_semaphore()->give();
// setup sensor rotations from probe()
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation);
@ -930,7 +930,7 @@ bool AP_InertialSensor_Invensense::_check_whoami(void)
bool AP_InertialSensor_Invensense::_hardware_init(void)
{
_dev->get_semaphore()->take_blocking();
WITH_SEMAPHORE(_dev->get_semaphore());
// setup for register checking. We check much less often on I2C
// where the cost of the checks is higher
@ -940,7 +940,6 @@ bool AP_InertialSensor_Invensense::_hardware_init(void)
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
if (!_check_whoami()) {
_dev->get_semaphore()->give();
return false;
}
@ -998,7 +997,6 @@ bool AP_InertialSensor_Invensense::_hardware_init(void)
if (tries == 5) {
hal.console->printf("Failed to boot Invensense 5 times\n");
_dev->get_semaphore()->give();
return false;
}
@ -1008,8 +1006,7 @@ bool AP_InertialSensor_Invensense::_hardware_init(void)
// this avoids a sensor bug, see description above
_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
}
_dev->get_semaphore()->give();
return true;
}
@ -1142,7 +1139,7 @@ void AP_Invensense_AuxiliaryBus::_configure_slaves()
return;
}
backend._dev->get_semaphore()->take_blocking();
WITH_SEMAPHORE(backend._dev->get_semaphore());
/* Enable the I2C master to slaves on the auxiliary I2C bus*/
if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) {
@ -1162,8 +1159,6 @@ void AP_Invensense_AuxiliaryBus::_configure_slaves()
backend._register_write(MPUREG_I2C_MST_DELAY_CTRL,
BIT_I2C_SLV0_DLY_EN | BIT_I2C_SLV1_DLY_EN |
BIT_I2C_SLV2_DLY_EN | BIT_I2C_SLV3_DLY_EN);
backend._dev->get_semaphore()->give();
}
int AP_Invensense_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave,

21
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
View File

@ -156,7 +156,7 @@ bool AP_InertialSensor_Invensensev2::_has_auxiliary_bus()
void AP_InertialSensor_Invensensev2::start()
{
_dev->get_semaphore()->take_blocking();
WITH_SEMAPHORE(_dev->get_semaphore());
// initially run the bus at low speed
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
@ -192,8 +192,11 @@ void AP_InertialSensor_Invensensev2::start()
break;
}
_gyro_instance = _imu.register_gyro(1125, _dev->get_bus_id_devtype(gdev));
_accel_instance = _imu.register_accel(1125, _dev->get_bus_id_devtype(adev));
if (!_imu.register_gyro(_gyro_instance, 1125, _dev->get_bus_id_devtype(gdev)) ||
!_imu.register_accel(_accel_instance, 1125, _dev->get_bus_id_devtype(adev))) {
return;
}
// setup on-sensor filtering and scaling
_set_filter_and_scaling();
@ -219,8 +222,6 @@ void AP_InertialSensor_Invensensev2::start()
// now that we have initialised, we set the bus speed to high
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
_dev->get_semaphore()->give();
// setup sensor rotations from probe()
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation);
@ -664,7 +665,7 @@ bool AP_InertialSensor_Invensensev2::_check_whoami(void)
bool AP_InertialSensor_Invensensev2::_hardware_init(void)
{
_dev->get_semaphore()->take_blocking();
WITH_SEMAPHORE(_dev->get_semaphore());
// disabled setup of checked registers as it can't cope with bank switching
_dev->setup_checked_registers(7, _dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20);
@ -674,7 +675,6 @@ bool AP_InertialSensor_Invensensev2::_hardware_init(void)
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
if (!_check_whoami()) {
_dev->get_semaphore()->give();
return false;
}
@ -725,15 +725,13 @@ bool AP_InertialSensor_Invensensev2::_hardware_init(void)
if (tries == 5) {
hal.console->printf("Failed to boot Invensense 5 times\n");
_dev->get_semaphore()->give();
return false;
}
if (_inv2_type == Invensensev2_ICM20649) {
_clip_limit = 29.5f * GRAVITY_MSS;
}
_dev->get_semaphore()->give();
return true;
}
@ -860,7 +858,7 @@ void AP_Invensensev2_AuxiliaryBus::_configure_slaves()
{
auto &backend = AP_InertialSensor_Invensensev2::from(_ins_backend);
backend._dev->get_semaphore()->take_blocking();
WITH_SEMAPHORE(backend._dev->get_semaphore());
/* Enable the I2C master to slaves on the auxiliary I2C bus*/
if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) {
@ -881,7 +879,6 @@ void AP_Invensensev2_AuxiliaryBus::_configure_slaves()
BIT_I2C_SLV0_DLY_EN | BIT_I2C_SLV1_DLY_EN |
BIT_I2C_SLV2_DLY_EN | BIT_I2C_SLV3_DLY_EN);
backend._dev->get_semaphore()->give();
}
int AP_Invensensev2_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave,

6
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -161,8 +161,10 @@ void AP_InertialSensor_Invensensev3::start()
break;
}
gyro_instance = _imu.register_gyro(INV3_ODR, dev->get_bus_id_devtype(devtype));
accel_instance = _imu.register_accel(INV3_ODR, dev->get_bus_id_devtype(devtype));
if (!_imu.register_gyro(gyro_instance, INV3_ODR, dev->get_bus_id_devtype(devtype)) ||
!_imu.register_accel(accel_instance, INV3_ODR, dev->get_bus_id_devtype(devtype))) {
return;
}
// setup on-sensor filtering and scaling
set_filter_and_scaling();

8
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -250,8 +250,10 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void)
*/
void AP_InertialSensor_L3G4200D::start(void)
{
_gyro_instance = _imu.register_gyro(800, _dev_gyro->get_bus_id_devtype(DEVTYPE_L3G4200D));
_accel_instance = _imu.register_accel(800, _dev_accel->get_bus_id_devtype(DEVTYPE_L3G4200D));
if (!_imu.register_gyro(_gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_L3G4200D)) ||
!_imu.register_accel(_accel_instance, 800, _dev_accel->get_bus_id_devtype(DEVTYPE_L3G4200D))) {
return;
}
// start the timer process to read samples
_dev_accel->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate_accel, void));
@ -346,4 +348,4 @@ void AP_InertialSensor_L3G4200D::_accumulate_accel (void)
}
}
#endif
#endif

6
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -512,8 +512,10 @@ fail_whoami:
*/
void AP_InertialSensor_LSM9DS0::start(void)
{
_gyro_instance = _imu.register_gyro(760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20));
_accel_instance = _imu.register_accel(1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D));
if (!_imu.register_gyro(_gyro_instance, 760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20)) ||
!_imu.register_accel(_accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D))) {
return;
}
if (whoami_g == LSM9DS0_G_WHOAMI_H) {
set_gyro_orientation(_gyro_instance, _rotation_gH);

6
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp
View File

@ -330,8 +330,10 @@ void AP_InertialSensor_LSM9DS1::_fifo_reset()
*/
void AP_InertialSensor_LSM9DS1::start(void)
{
_gyro_instance = _imu.register_gyro(952, _dev->get_bus_id_devtype(DEVTYPE_GYR_LSM9DS1));
_accel_instance = _imu.register_accel(952, _dev->get_bus_id_devtype(DEVTYPE_ACC_LSM9DS1));
if (!_imu.register_gyro(_gyro_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_GYR_LSM9DS1)) ||
!_imu.register_accel(_accel_instance, 952, _dev->get_bus_id_devtype(DEVTYPE_ACC_LSM9DS1))) {
return;
}
set_accel_orientation(_accel_instance, _rotation);
set_gyro_orientation(_gyro_instance, _rotation);

6
libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp
View File

@ -336,8 +336,10 @@ bool AP_InertialSensor_RST::_init_sensor(void)
*/
void AP_InertialSensor_RST::start(void)
{
_gyro_instance = _imu.register_gyro(800, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_I3G4250D));
_accel_instance = _imu.register_accel(1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_IIS328DQ));
if (!_imu.register_gyro(_gyro_instance, 800, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_I3G4250D)) ||
!_imu.register_accel(_accel_instance, 1000, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_IIS328DQ))) {
return;
}
set_gyro_orientation(_gyro_instance, _rotation_g);
set_accel_orientation(_accel_instance, _rotation_a);

10
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -337,10 +337,12 @@ uint8_t AP_InertialSensor_SITL::bus_id = 0;
void AP_InertialSensor_SITL::start()
{
gyro_instance = _imu.register_gyro(gyro_sample_hz,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 1, DEVTYPE_SITL));
accel_instance = _imu.register_accel(accel_sample_hz,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 2, DEVTYPE_SITL));
if (!_imu.register_gyro(gyro_instance, gyro_sample_hz,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 1, DEVTYPE_SITL)) ||
!_imu.register_accel(accel_instance, accel_sample_hz,
AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 2, DEVTYPE_SITL))) {
return;
}
bus_id++;
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void));
}