mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: use BMI160 for Intel Aero
This commit is contained in:
parent
025082b280
commit
68fe536da3
|
@ -567,6 +567,14 @@ AP_InertialSensor::detect_backends(void)
|
|||
} else {
|
||||
hal.console->printf("MPU9250: External IMU not detected\n");
|
||||
}
|
||||
#elif HAL_INS_DEFAULT == HAL_INS_AERO
|
||||
auto *backend = AP_InertialSensor_BMI160::probe(*this,
|
||||
hal.spi->get_device("bmi160"));
|
||||
if (backend) {
|
||||
_add_backend(backend);
|
||||
} else {
|
||||
hal.console->printf("aero: onboard IMU not detected\n");
|
||||
}
|
||||
#else
|
||||
#error Unrecognised HAL_INS_TYPE setting
|
||||
#endif
|
||||
|
|
|
@ -99,7 +99,11 @@
|
|||
#define BMI160_READ_FLAG 0x80
|
||||
#define BMI160_HARDWARE_INIT_MAX_TRIES 5
|
||||
|
||||
#define BMI160_INT1_GPIO -1
|
||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO
|
||||
# define BMI160_INT1_GPIO AERO_GPIO_BMI160_INT1
|
||||
#else
|
||||
# define BMI160_INT1_GPIO -1
|
||||
#endif
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -384,6 +388,11 @@ read_fifo_read_data:
|
|||
(float)(int16_t)le16toh(raw_data[i].gyro.y),
|
||||
(float)(int16_t)le16toh(raw_data[i].gyro.z)};
|
||||
|
||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO
|
||||
accel.rotate(ROTATION_ROLL_180);
|
||||
gyro.rotate(ROTATION_ROLL_180);
|
||||
#endif
|
||||
|
||||
accel *= _accel_scale;
|
||||
gyro *= _gyro_scale;
|
||||
|
||||
|
|
Loading…
Reference in New Issue