mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: set orientation ROLL_180 for LSM9DS1 on Navio2
This commit is contained in:
parent
163fa07ac0
commit
96db31635d
|
@ -599,7 +599,7 @@ void Compass::_detect_backends(void)
|
||||||
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO2
|
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO2
|
||||||
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
|
ADD_BACKEND(AP_Compass_AK8963::probe_mpu9250(*this, 0),
|
||||||
AP_Compass_AK8963::name, false);
|
AP_Compass_AK8963::name, false);
|
||||||
ADD_BACKEND(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")),
|
ADD_BACKEND(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m"), ROTATION_ROLL_180),
|
||||||
AP_Compass_LSM9DS1::name, false);
|
AP_Compass_LSM9DS1::name, false);
|
||||||
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
|
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
|
||||||
AP_Compass_HMC5843::name, true);
|
AP_Compass_HMC5843::name, true);
|
||||||
|
|
|
@ -203,10 +203,6 @@ void AP_Compass_LSM9DS1::read()
|
||||||
_accum_count = 0;
|
_accum_count = 0;
|
||||||
|
|
||||||
_sem->give();
|
_sem->give();
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
|
|
||||||
field.rotate(ROTATION_ROLL_180);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
publish_filtered_field(field, _compass_instance);
|
publish_filtered_field(field, _compass_instance);
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue