diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 296bff0c79..22de590d79 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -692,7 +692,8 @@ AP_InertialSensor::detect_backends(void) _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180)); _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), ROTATION_ROLL_180)); + hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), + ROTATION_ROLL_180, ROTATION_ROLL_180_YAW_270)); } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) { // older Pixhawk2 boards have the MPU6000 instead of MPU9250 @@ -701,7 +702,8 @@ AP_InertialSensor::detect_backends(void) } _add_backend(AP_InertialSensor_LSM9DS0::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME), - hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_ROLL_180_YAW_270)); + hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), + ROTATION_ROLL_180_YAW_270, ROTATION_ROLL_180_YAW_90)); if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270))) { _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_270)); } @@ -733,8 +735,9 @@ AP_InertialSensor::detect_backends(void) #elif HAL_INS_DEFAULT == HAL_INS_RASPILOT _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))); + hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME), + hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), + ROTATION_NONE, ROTATION_YAW_90)); #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))); #elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index b66504576e..f201017270 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -382,20 +382,23 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu, AP_HAL::OwnPtr dev_accel, int drdy_pin_num_a, int drdy_pin_num_g, - enum Rotation rotation) + enum Rotation rotation_a, + enum Rotation rotation_g) : AP_InertialSensor_Backend(imu) , _dev_gyro(std::move(dev_gyro)) , _dev_accel(std::move(dev_accel)) , _drdy_pin_num_a(drdy_pin_num_a) , _drdy_pin_num_g(drdy_pin_num_g) - , _rotation(rotation) + , _rotation_a(rotation_a) + , _rotation_g(rotation_g) { } AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::probe(AP_InertialSensor &_imu, AP_HAL::OwnPtr dev_gyro, AP_HAL::OwnPtr dev_accel, - enum Rotation rotation) + enum Rotation rotation_a, + enum Rotation rotation_g) { if (!dev_gyro || !dev_accel) { return nullptr; @@ -403,7 +406,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::probe(AP_InertialSensor &_ AP_InertialSensor_LSM9DS0 *sensor = new AP_InertialSensor_LSM9DS0(_imu, std::move(dev_gyro), std::move(dev_accel), LSM9DS0_DRY_X_PIN, LSM9DS0_DRY_G_PIN, - rotation); + rotation_a, rotation_g); if (!sensor || !sensor->_init_sensor()) { delete sensor; return nullptr; @@ -519,8 +522,8 @@ void AP_InertialSensor_LSM9DS0::start(void) _gyro_instance = _imu.register_gyro(760, _dev_gyro->get_bus_id_devtype(DEVTYPE_GYR_L3GD20)); _accel_instance = _imu.register_accel(800, _dev_accel->get_bus_id_devtype(DEVTYPE_ACC_LSM303D)); - set_gyro_orientation(_gyro_instance, _rotation); - set_accel_orientation(_accel_instance, _rotation); + set_gyro_orientation(_gyro_instance, _rotation_g); + set_accel_orientation(_accel_instance, _rotation_a); _set_accel_max_abs_offset(_accel_instance, 5.0f); @@ -757,12 +760,6 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g() 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; _rotate_and_correct_gyro(_gyro_instance, gyro_data); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h index 891825b4f9..038abc39d5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h @@ -18,14 +18,16 @@ public: static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, AP_HAL::OwnPtr dev_gyro, AP_HAL::OwnPtr dev_accel, - enum Rotation rotation = ROTATION_NONE); + enum Rotation rotation_a = ROTATION_NONE, + enum Rotation rotation_g = ROTATION_NONE); private: AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu, AP_HAL::OwnPtr dev_gyro, AP_HAL::OwnPtr dev_accel, int drdy_pin_num_a, int drdy_pin_num_b, - enum Rotation rotation); + enum Rotation rotation_a, + enum Rotation rotation_g); struct PACKED sensor_raw_data { int16_t x; @@ -95,7 +97,12 @@ private: uint8_t _gyro_instance; uint8_t _accel_instance; - enum Rotation _rotation; + /* + for boards that have a separate LSM303D and L3GD20 there can be + different rotations for each + */ + enum Rotation _rotation_a; + enum Rotation _rotation_g; uint8_t _reg_check_counter; };