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

AP_InertialSensor: Enable dual IMU for raspilot

Browse Source
This commit is contained in:
raspilot 2015-09-22 01:27:24 +08:00 committed by Lucas De Marchi
parent 9d787e44cc
commit 1d1d224c18
2 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -545,9 +545,10 @@ AP_InertialSensor::detect_backends(void)
#elif HAL_INS_DEFAULT == HAL_INS_L3G4200D #elif HAL_INS_DEFAULT == HAL_INS_L3G4200D
_add_backend(AP_InertialSensor_L3G4200D::probe(*this, hal.i2c_mgr->get_device(HAL_INS_L3G4200D_I2C_BUS, HAL_INS_L3G4200D_I2C_ADDR)); _add_backend(AP_InertialSensor_L3G4200D::probe(*this, hal.i2c_mgr->get_device(HAL_INS_L3G4200D_I2C_BUS, HAL_INS_L3G4200D_I2C_ADDR));
#elif HAL_INS_DEFAULT == HAL_INS_RASPILOT #elif HAL_INS_DEFAULT == HAL_INS_RASPILOT
//_add_backend(AP_InertialSensor_L3GD20::detect);
//_add_backend(AP_InertialSensor_LSM303D::detect);
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME))); _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME)));
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C #elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR)); _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR));
#elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT #elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT

19
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -24,6 +24,14 @@
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
#define LSM9DS0_DRY_X_PIN RPI_GPIO_17
#define LSM9DS0_DRY_G_PIN RPI_GPIO_6
#else
#define LSM9DS0_DRY_X_PIN -1
#define LSM9DS0_DRY_G_PIN -1
#endif
#define LSM9DS0_G_WHOAMI 0xD4 #define LSM9DS0_G_WHOAMI 0xD4
#define LSM9DS0_XM_WHOAMI 0x49 #define LSM9DS0_XM_WHOAMI 0x49
@ -388,11 +396,9 @@ AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::probe(AP_InertialSensor &_
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro, AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel) AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel)
{ {
int drdy_pin_num_a = -1, drdy_pin_num_g = -1;
AP_InertialSensor_LSM9DS0 *sensor = AP_InertialSensor_LSM9DS0 *sensor =
new AP_InertialSensor_LSM9DS0(_imu, std::move(dev_gyro), std::move(dev_accel), new AP_InertialSensor_LSM9DS0(_imu, std::move(dev_gyro), std::move(dev_accel),
drdy_pin_num_a, drdy_pin_num_g); LSM9DS0_DRY_X_PIN, LSM9DS0_DRY_G_PIN);
if (!sensor || !sensor->_init_sensor()) { if (!sensor || !sensor->_init_sensor()) {
delete sensor; delete sensor;
return nullptr; return nullptr;
@ -740,6 +746,13 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g()
} }
Vector3f gyro_data(raw_data.x, -raw_data.y, -raw_data.z); Vector3f gyro_data(raw_data.x, -raw_data.y, -raw_data.z);
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
// LSM303D on RasPilot
// FIXME: wrong way to provide rotation per-board
gyro_data.rotate(ROTATION_YAW_90);
#endif
gyro_data *= _gyro_scale; gyro_data *= _gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, gyro_data); _rotate_and_correct_gyro(_gyro_instance, gyro_data);