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

allows for first 3 to work without a panic

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -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;
 }
 
 /*

libraries/AP_InertialSensor/AP_InertialSensor.h

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

libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp

@@ -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);

libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp

@@ -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);

libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp

@@ -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);

libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp

@@ -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,

libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.cpp

@@ -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)
 {
+    if (started) {
         update_accel(accel_instance);
         update_gyro(gyro_instance);
-    return true;
+    }
+    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()

libraries/AP_InertialSensor/AP_InertialSensor_ExternalAHRS.h

@@ -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
 

libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

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

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp

@@ -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,7 +1006,6 @@ 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,

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp

@@ -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,14 +725,12 @@ 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,

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

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

libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp

@@ -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));

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp

@@ -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);

libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp

@@ -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);

libraries/AP_InertialSensor/AP_InertialSensor_RST.cpp

@@ -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);

libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp

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