Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added optional per-instance orientation 

this is needed for multi-sensor boards to get orientation right for
each sensor
This commit is contained in:
Andrew Tridgell 2016-11-03 20:19:04 +11:00
parent ac2572384d
commit 2df6ed08c2
6 changed files with 42 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -591,7 +591,7 @@ AP_InertialSensor::detect_backends(void)
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_TEST_V1) {
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME)));
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_TEST_V2) {
_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), 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)));

4
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -350,6 +350,10 @@ private:
// board orientation from AHRS
enum Rotation _board_orientation;
// per-sensor orientation to allow for board type defaults at runtime
enum Rotation _gyro_orientation[INS_MAX_INSTANCES];
enum Rotation _accel_orientation[INS_MAX_INSTANCES];
// calibrated_ok flags
bool _gyro_cal_ok[INS_MAX_INSTANCES];

9
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -19,6 +19,9 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect
offsets and scaling.
*/
// rotate for sensor orientation
accel.rotate(_imu._accel_orientation[instance]);
// apply offsets
accel -= _imu._accel_offset[instance];
@ -34,8 +37,12 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect
void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro)
{
// rotate for sensor orientation
gyro.rotate(_imu._gyro_orientation[instance]);
// gyro calibration is always assumed to have been done in sensor frame
gyro -= _imu._gyro_offset[instance];
gyro.rotate(_imu._board_orientation);
}
@ -147,7 +154,7 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f
cal_sample.x /= accel_scale.x;
cal_sample.y /= accel_scale.y;
cal_sample.z /= accel_scale.z;
//remove offsets
cal_sample += _imu._accel_offset[instance].get() * _imu._delta_velocity_dt[instance] ;

8
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -148,6 +148,14 @@ protected:
int8_t _last_accel_filter_hz[INS_MAX_INSTANCES];
int8_t _last_gyro_filter_hz[INS_MAX_INSTANCES];
void set_gyro_orientation(uint8_t instance, enum Rotation rotation) {
_imu._gyro_orientation[instance] = rotation;
}
void set_accel_orientation(uint8_t instance, enum Rotation rotation) {
_imu._accel_orientation[instance] = rotation;
}
// note that each backend is also expected to have a static detect()
// function which instantiates an instance of the backend sensor
// driver if the sensor is available

18
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -232,11 +232,13 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f);
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool use_fifo)
bool use_fifo,
enum Rotation rotation)
: AP_InertialSensor_Backend(imu)
, _use_fifo(use_fifo)
, _temp_filter(1000, 1)
, _dev(std::move(dev))
, _rotation(rotation)
{
}
@ -246,10 +248,11 @@ AP_InertialSensor_MPU6000::~AP_InertialSensor_MPU6000()
}
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation)
{
AP_InertialSensor_MPU6000 *sensor =
new AP_InertialSensor_MPU6000(imu, std::move(dev), true);
new AP_InertialSensor_MPU6000(imu, std::move(dev), true, rotation);
if (!sensor || !sensor->_init()) {
delete sensor;
return nullptr;
@ -261,13 +264,14 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::probe(AP_InertialSensor &i
AP_InertialSensor_Backend *AP_InertialSensor_MPU6000::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev)
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
enum Rotation rotation)
{
AP_InertialSensor_MPU6000 *sensor;
dev->set_read_flag(0x80);
sensor = new AP_InertialSensor_MPU6000(imu, std::move(dev), false);
sensor = new AP_InertialSensor_MPU6000(imu, std::move(dev), false, rotation);
if (!sensor || !sensor->_init()) {
delete sensor;
return nullptr;
@ -385,6 +389,10 @@ void AP_InertialSensor_MPU6000::start()
_gyro_instance = _imu.register_gyro(1000);
_accel_instance = _imu.register_accel(1000);
// setup sensor rotations from probe()
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation);
hal.scheduler->resume_timer_procs();
// start the timer process to read samples

11
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -34,10 +34,12 @@ public:
}
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
enum Rotation rotation = ROTATION_NONE);
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev);
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
enum Rotation rotation = ROTATION_NONE);
/* update accel and gyro state */
bool update();
@ -52,7 +54,8 @@ public:
private:
AP_InertialSensor_MPU6000(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool use_fifo);
bool use_fifo,
enum Rotation rotation);
#if MPU6000_DEBUG
void _dump_registers();
@ -99,6 +102,8 @@ private:
float _temp_filtered;
LowPassFilter2pFloat _temp_filter;
enum Rotation _rotation;
AP_HAL::DigitalSource *_drdy_pin;
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
AP_MPU6000_AuxiliaryBus *_auxiliary_bus;