diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index f717d23b81d69..5a3dfcfdf52bc 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(); } +#ifndef HAL_BUILD_AP_PERIPH 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 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"); + #ifndef HAL_BUILD_AP_PERIPH 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..b7efd42b00d9a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_config.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_config.h @@ -48,7 +48,7 @@ #endif #ifndef AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED -#define AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED AP_INERTIALSENSOR_ENABLED +#define AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED (AP_INERTIALSENSOR_ENABLED && !defined(HAL_BUILD_AP_PERIPH)) #endif #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED