|
|
|
@ -33,6 +33,10 @@
@@ -33,6 +33,10 @@
|
|
|
|
|
#define timing_printf(fmt, args...) |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
#ifndef HAL_DEFAULT_INS_FAST_SAMPLE |
|
|
|
|
#define HAL_DEFAULT_INS_FAST_SAMPLE 0 |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
|
|
|
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) |
|
|
|
@ -431,7 +435,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
@@ -431,7 +435,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
|
|
|
|
|
// @User: Advanced
|
|
|
|
|
// @Values: 1:FirstIMUOnly,3:FirstAndSecondIMU
|
|
|
|
|
// @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU
|
|
|
|
|
AP_GROUPINFO("FAST_SAMPLE", 36, AP_InertialSensor, _fast_sampling_mask, 0), |
|
|
|
|
AP_GROUPINFO("FAST_SAMPLE", 36, AP_InertialSensor, _fast_sampling_mask, HAL_DEFAULT_INS_FAST_SAMPLE), |
|
|
|
|
|
|
|
|
|
// @Group: NOTCH_
|
|
|
|
|
// @Path: ../Filter/NotchFilter.cpp
|
|
|
|
@ -697,7 +701,10 @@ AP_InertialSensor::detect_backends(void)
@@ -697,7 +701,10 @@ AP_InertialSensor::detect_backends(void)
|
|
|
|
|
ADD_BACKEND(AP_InertialSensor_HIL::detect(*this)); |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
|
|
|
#if defined(HAL_INS_PROBE_LIST) |
|
|
|
|
// IMUs defined by IMU lines in hwdef.dat
|
|
|
|
|
HAL_INS_PROBE_LIST; |
|
|
|
|
#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
|
|
|
ADD_BACKEND(AP_InertialSensor_SITL::detect(*this)); |
|
|
|
|
#elif HAL_INS_DEFAULT == HAL_INS_HIL |
|
|
|
|
ADD_BACKEND(AP_InertialSensor_HIL::detect(*this)); |
|
|
|
@ -769,13 +776,6 @@ AP_InertialSensor::detect_backends(void)
@@ -769,13 +776,6 @@ AP_InertialSensor::detect_backends(void)
|
|
|
|
|
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_NONE)); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AP_BoardConfig::PX4_BOARD_PIXRACER: |
|
|
|
|
// only do fast samplng on ICM-20608. The MPU9250 doesn't handle high rate well when it has a mag enabled
|
|
|
|
|
_fast_sampling_mask.set_default(1); |
|
|
|
|
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90)); |
|
|
|
|
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90)); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO: |
|
|
|
|
_fast_sampling_mask.set_default(3); |
|
|
|
|
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90)); |
|
|
|
|