aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: fixed gyro orientation on l3gd20H on pixhawk

Browse Source
This commit is contained in:
Andrew Tridgell 2016-11-16 15:16:15 +11:00
parent fbeafa0d0a
commit f3f1f6b0b6
3 changed files with 26 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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

21
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

@ -382,20 +382,23 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> 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<AP_HAL::SPIDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> 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);

13
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h
View File

@ -18,14 +18,16 @@ public:
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> 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<AP_HAL::SPIDevice> dev_gyro,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> 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;
};