AP_InertialSensor: added set_board_orientation() method

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -125,6 +125,11 @@ public:
     // class level parameters
     static const struct AP_Param::GroupInfo var_info[];
 
+    // set overall board orientation
+    void set_board_orientation(enum Rotation orientation) {
+        _board_orientation = orientation;
+    }
+
 protected:
 
     // sensor specific init to be overwritten by descendant classes
@@ -166,6 +171,9 @@ protected:
 
     // filtering frequency (0 means default)
     AP_Int8                 _mpu6000_filter;
+
+    // board orientation from AHRS
+    enum Rotation			_board_orientation;
 };
 
 #include "AP_InertialSensor_Oilpan.h"

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp

@@ -320,14 +320,21 @@ bool AP_InertialSensor_MPU6000::update( void )
 
     count_scale = 1.0 / _num_samples;
 
-    _gyro.x = _gyro_scale * _gyro_data_sign[0] * sum[_gyro_data_index[0]] * count_scale;
-    _gyro.y = _gyro_scale * _gyro_data_sign[1] * sum[_gyro_data_index[1]] * count_scale;
-    _gyro.z = _gyro_scale * _gyro_data_sign[2] * sum[_gyro_data_index[2]] * count_scale;
+    _gyro  = Vector3f(_gyro_data_sign[0] * sum[_gyro_data_index[0]],
+                      _gyro_data_sign[1] * sum[_gyro_data_index[1]],
+                      _gyro_data_sign[2] * sum[_gyro_data_index[2]]);
+    _gyro.rotate(_board_orientation);
+    _gyro *= _gyro_scale * count_scale;
     _gyro -= _gyro_offset;
 
-    _accel.x = accel_scale.x * _accel_data_sign[0] * sum[_accel_data_index[0]] * count_scale * MPU6000_ACCEL_SCALE_1G;
-    _accel.y = accel_scale.y * _accel_data_sign[1] * sum[_accel_data_index[1]] * count_scale * MPU6000_ACCEL_SCALE_1G;
-    _accel.z = accel_scale.z * _accel_data_sign[2] * sum[_accel_data_index[2]] * count_scale * MPU6000_ACCEL_SCALE_1G;
+    _accel   = Vector3f(_accel_data_sign[0] * sum[_accel_data_index[0]],
+                        _accel_data_sign[1] * sum[_accel_data_index[1]],
+                        _accel_data_sign[2] * sum[_accel_data_index[2]]);
+    _accel.rotate(_board_orientation);
+    _accel *= count_scale * MPU6000_ACCEL_SCALE_1G;
+    _accel.x *= accel_scale.x;
+    _accel.y *= accel_scale.y;
+    _accel.z *= accel_scale.z;
     _accel -= _accel_offset;
 
     _temp    = _temp_to_celsius(sum[_temp_data_index] * count_scale);

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

@@ -79,14 +79,23 @@ bool AP_InertialSensor_Oilpan::update()
     _delta_time_micros = _adc->Ch6(_sensors, adc_values);
     _temp = _adc->Ch(_gyro_temp_ch);
 
-    _gyro.x = _gyro_gain_x * _sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET );
-    _gyro.y = _gyro_gain_y * _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET );
-    _gyro.z = _gyro_gain_z * _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET );
+    _gyro   = Vector3f(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ),
+                       _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ),
+                       _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ));
+    _gyro.rotate(_board_orientation);
+    _gyro.x *= _gyro_gain_x;
+    _gyro.y *= _gyro_gain_y;
+    _gyro.z *= _gyro_gain_z;
     _gyro -= gyro_offset;
 
-    _accel.x = accel_scale.x * _sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET) * OILPAN_ACCEL_SCALE_1G;
-    _accel.y = accel_scale.y * _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET) * OILPAN_ACCEL_SCALE_1G;
-    _accel.z = accel_scale.z * _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET) * OILPAN_ACCEL_SCALE_1G;
+    _accel  = Vector3f(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
+                       _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
+                       _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
+    _accel.rotate(_board_orientation);
+    _accel.x *= accel_scale.x;
+    _accel.y *= accel_scale.y;
+    _accel.z *= accel_scale.z;
+    _accel   *= OILPAN_ACCEL_SCALE_1G;
     _accel -= accel_offset;
 
 /*

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -79,15 +79,22 @@ bool AP_InertialSensor_PX4::update(void)
 
     Vector3f accel_scale = _accel_scale.get();
 
-    _accel.x =   accel_scale.x * _raw_sensors.accelerometer_m_s2[0] / _raw_sensors.accelerometer_counter;
-    _accel.y = - accel_scale.y * _raw_sensors.accelerometer_m_s2[1] / _raw_sensors.accelerometer_counter;
-    _accel.z = - accel_scale.z * _raw_sensors.accelerometer_m_s2[2] / _raw_sensors.accelerometer_counter;
-    _accel -= _accel_offset;
-
-    _gyro.x =   _raw_sensors.gyro_rad_s[0] / _raw_sensors.gyro_counter;
-    _gyro.y = - _raw_sensors.gyro_rad_s[1] / _raw_sensors.gyro_counter;
-    _gyro.z = - _raw_sensors.gyro_rad_s[2] / _raw_sensors.gyro_counter;
-    _gyro -= _gyro_offset;
+    _accel   = Vector3f(_raw_sensors.accelerometer_m_s2[0],
+                        _raw_sensors.accelerometer_m_s2[1],
+                        _raw_sensors.accelerometer_m_s2[2]);
+    _accel.rotate(_board_orientation);
+    _accel.x *= accel_scale.x;
+    _accel.y *= accel_scale.y;
+    _accel.z *= accel_scale.z;
+    _accel   /= _raw_sensors.accelerometer_counter;
+    _accel   -= _accel_offset;
+
+    _gyro    = Vector3f(_raw_sensors.gyro_rad_s[0],
+                        _raw_sensors.gyro_rad_s[1],
+                        _raw_sensors.gyro_rad_s[2]);
+    _gyro.rotate(_board_orientation);
+    _gyro   /= _raw_sensors.gyro_counter;
+    _gyro   -= _gyro_offset;
 
     memset(&_raw_sensors, 0, sizeof(_raw_sensors));