AP_InertialSensor: fix continuing after ins init fail in AP_Periph

ArduPilot · Oct 22, 2024 · 9381404 · 9381404
1 parent 76d6a88
commit 9381404
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 1 deletion.
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h
@@ -351,6 +351,8 @@
 #define AP_RTC_ENABLED defined(HAL_PERIPH_ENABLE_RTC)
 #define HAL_VISUALODOM_ENABLED defined(HAL_PERIPH_ENABLE_VISUALODOM)
 #define AP_INERTIALSENSOR_ENABLED defined(HAL_PERIPH_ENABLE_IMU)
+#define AP_INERTIALSENSOR_ALLOW_NO_SENSORS defined(HAL_PERIPH_ENABLE_IMU)
+#define AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED 0
 
 #ifndef AP_BOOTLOADER_ALWAYS_ERASE
 #define AP_BOOTLOADER_ALWAYS_ERASE 1

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -854,9 +854,11 @@ void AP_InertialSensor::_start_backends()
         _backends[i]->start();
     }
 
+#if AP_INERTIALSENSOR_ALLOW_NO_SENSORS
     if (_gyro_count == 0 || _accel_count == 0) {
         AP_HAL::panic("INS needs at least 1 gyro and 1 accel");
     }
+#endif
 
     // clear IDs for unused sensor instances
     for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
@@ -949,7 +951,7 @@ AP_InertialSensor::init(uint16_t loop_rate)
     }
 
     // calibrate gyros unless gyro calibration has been disabled
-    if (gyro_calibration_timing() != GYRO_CAL_NEVER) {
+    if (gyro_calibration_timing() != GYRO_CAL_NEVER && _gyro_count > 0) {
         init_gyro();
     }
 
@@ -1324,8 +1326,10 @@ AP_InertialSensor::detect_backends(void)
         #else
         DEV_PRINTF("INS: unable to initialise driver\n");
         GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "INS: unable to initialise driver");
+        #if !AP_INERTIALSENSOR_ALLOW_NO_SENSORS
         AP_BoardConfig::config_error("INS: unable to initialise driver");
         #endif
+        #endif
     }
 }
 

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_config.h b/libraries/AP_InertialSensor/AP_InertialSensor_config.h
@@ -51,6 +51,10 @@
 #define AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED AP_INERTIALSENSOR_ENABLED
 #endif
 
+#ifndef AP_INERTIALSENSOR_ALLOW_NO_SENSORS
+#define AP_INERTIALSENSOR_ALLOW_NO_SENSORS 0
+#endif
+
 #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
 #ifndef HAL_INS_NUM_HARMONIC_NOTCH_FILTERS
 #define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2