zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: fixed gyro orientation on l3gd20H on pixhawk

master
Andrew Tridgell 8 years ago
parent
fbeafa0d0a
commit
f3f1f6b0b6
3 changed files with 26 additions and 19 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 11
      libraries/AP_InertialSensor/AP_InertialSensor.cpp
  2. 21
      libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
  3. 13
      libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h

11
libraries/AP_InertialSensor/AP_InertialSensor.cpp
Unescape View File

@ -692,7 +692,8 @@ AP_InertialSensor::detect_backends(void) @@ -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) @@ -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) @@ -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
Unescape View File

@ -382,20 +382,23 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu, @@ -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 &_ @@ -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) @@ -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() @@ -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
Unescape View File

@ -18,14 +18,16 @@ public: @@ -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: @@ -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;
};

Write Preview
Loading…
Cancel
Save