From 9381404a9ff5b74ec35820b1d640f05122210d9d Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Tue, 15 Oct 2024 12:13:17 +0800
Subject: [PATCH] AP_InertialSensor: fix continuing after ins init fail in
 AP_Periph

---
 libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h | 2 ++
 libraries/AP_InertialSensor/AP_InertialSensor.cpp        | 6 +++++-
 libraries/AP_InertialSensor/AP_InertialSensor_config.h   | 4 ++++
 3 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h
index 9240bf000a226..4954c9be656eb 100644
--- 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
index f717d23b81d69..8c6f05fd1e678 100644
--- 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
index d4775b7219133..5ad27d0300334 100644
--- 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