Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added set_board_orientation() method

Browse Source
This commit is contained in:
Andrew Tridgell 2013-01-13 16:03:13 +11:00
parent 60a4447a86
commit d7996acdf7
4 changed files with 51 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

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

19
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

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

21
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

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

23
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -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;
_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.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;
_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));