AP_InertialSensor: added optional per-instance orientation

this is needed for multi-sensor boards to get orientation right for
each sensor

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)));

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];
 

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] ;
 

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

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

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

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