From 2df6ed08c27209d6d735152d78785a1c9b4d4898 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Nov 2016 20:19:04 +1100 Subject: [PATCH] AP_InertialSensor: added optional per-instance orientation this is needed for multi-sensor boards to get orientation right for each sensor --- .../AP_InertialSensor/AP_InertialSensor.cpp | 2 +- .../AP_InertialSensor/AP_InertialSensor.h | 4 ++++ .../AP_InertialSensor_Backend.cpp | 9 ++++++++- .../AP_InertialSensor_Backend.h | 8 ++++++++ .../AP_InertialSensor_MPU6000.cpp | 18 +++++++++++++----- .../AP_InertialSensor_MPU6000.h | 11 ++++++++--- 6 files changed, 42 insertions(+), 10 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index df38b9505e..f2d1bc879e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -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))); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 7b7934c99f..d4adc5b987 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -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]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index b1b6d417e8..603229caa1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -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] ; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index d963f9e435..736c58a9a2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index 3bf6119b54..b21948c78e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -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 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 dev) + AP_HAL::OwnPtr 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 dev) + AP_HAL::OwnPtr 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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index 37825873e3..75007c9f0a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -34,10 +34,12 @@ public: } static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, - AP_HAL::OwnPtr dev); + AP_HAL::OwnPtr dev, + enum Rotation rotation = ROTATION_NONE); static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, - AP_HAL::OwnPtr dev); + AP_HAL::OwnPtr 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 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 _dev; AP_MPU6000_AuxiliaryBus *_auxiliary_bus;