Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: fix continuing after ins init fail in AP_Periph

Browse Source
This commit is contained in:
bugobliterator 2024-10-15 12:13:17 +08:00 committed by Andrew Tridgell
parent 76d6a88b7b
commit 9381404a9f
3 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h
View File

@ -351,6 +351,8 @@
#define AP_RTC_ENABLED defined(HAL_PERIPH_ENABLE_RTC) #define AP_RTC_ENABLED defined(HAL_PERIPH_ENABLE_RTC)
#define HAL_VISUALODOM_ENABLED defined(HAL_PERIPH_ENABLE_VISUALODOM) #define HAL_VISUALODOM_ENABLED defined(HAL_PERIPH_ENABLE_VISUALODOM)
#define AP_INERTIALSENSOR_ENABLED defined(HAL_PERIPH_ENABLE_IMU) #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 #ifndef AP_BOOTLOADER_ALWAYS_ERASE
#define AP_BOOTLOADER_ALWAYS_ERASE 1 #define AP_BOOTLOADER_ALWAYS_ERASE 1

6
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -854,9 +854,11 @@ void AP_InertialSensor::_start_backends()
_backends[i]->start(); _backends[i]->start();
} }
#if AP_INERTIALSENSOR_ALLOW_NO_SENSORS
if (_gyro_count == 0 || _accel_count == 0) { if (_gyro_count == 0 || _accel_count == 0) {
AP_HAL::panic("INS needs at least 1 gyro and 1 accel"); AP_HAL::panic("INS needs at least 1 gyro and 1 accel");
} }
#endif
// clear IDs for unused sensor instances // clear IDs for unused sensor instances
for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) { 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 // 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(); init_gyro();
} }
@ -1324,8 +1326,10 @@ AP_InertialSensor::detect_backends(void)
#else #else
DEV_PRINTF("INS: unable to initialise driver\n"); DEV_PRINTF("INS: unable to initialise driver\n");
GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "INS: unable to initialise driver"); 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"); AP_BoardConfig::config_error("INS: unable to initialise driver");
#endif #endif
#endif
} }
} }

4
libraries/AP_InertialSensor/AP_InertialSensor_config.h
View File

@ -51,6 +51,10 @@
#define AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED AP_INERTIALSENSOR_ENABLED #define AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED AP_INERTIALSENSOR_ENABLED
#endif #endif
#ifndef AP_INERTIALSENSOR_ALLOW_NO_SENSORS
#define AP_INERTIALSENSOR_ALLOW_NO_SENSORS 0
#endif
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
#ifndef HAL_INS_NUM_HARMONIC_NOTCH_FILTERS #ifndef HAL_INS_NUM_HARMONIC_NOTCH_FILTERS
#define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2 #define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2