From add298c0b5af57286ac4c6d57439a919d6102b96 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 20 Jan 2017 21:42:33 +1100 Subject: [PATCH] mc_att_control: use legacy offset and scale definition (+2 squashed commits) Squashed commits: [f81a8b0] mc_att_control: remove unnecessary initialisers [f3d3f48] mc_att_control: accommodate changes to sensor_correction topic --- .../mc_att_control/mc_att_control_main.cpp | 33 ++++++++++++++----- 1 file changed, 24 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 138cfff0c8..ae67a6045c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -536,15 +536,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _ts_opt_recovery = new TailsitterRecovery(); } - /* initialize _corrections to identity as we might not immediately get a topic update */ + /* initialize thermal corrections as we might not immediately get a topic update */ + memset(&_sensor_correction, 0, sizeof(_sensor_correction)); for (unsigned i = 0; i < 3; i++) { - _sensor_correction.gyro_scale[i] = 1.0f; - _sensor_correction.accel_scale[i] = 1.0f; + // used scale factors to unity + _sensor_correction.gyro_scale_0[i] = 1.0f; + _sensor_correction.gyro_scale_1[i] = 1.0f; + _sensor_correction.gyro_scale_2[i] = 1.0f; } - _sensor_correction.baro_scale = 1.0f; - - } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -994,9 +994,24 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _board_rotation = board_rotation_offset * _board_rotation; // get the raw gyro data and correct for thermal errors - math::Vector<3> rates(_sensor_gyro.x * _sensor_correction.gyro_scale[0] + _sensor_correction.gyro_offset[0], - _sensor_gyro.y * _sensor_correction.gyro_scale[1] + _sensor_correction.gyro_offset[1], - _sensor_gyro.z * _sensor_correction.gyro_scale[2] + _sensor_correction.gyro_offset[2]); + math::Vector<3> rates; + if (_selected_gyro == 0) { + rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0]; + rates(1) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1]; + rates(2) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2]; + } else if (_selected_gyro == 1) { + rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_1[0]) * _sensor_correction.gyro_scale_1[0]; + rates(1) = (_sensor_gyro.x - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1]; + rates(2) = (_sensor_gyro.x - _sensor_correction.gyro_offset_1[2]) * _sensor_correction.gyro_scale_1[2]; + } else if (_selected_gyro == 2) { + rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0]; + rates(1) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1]; + rates(2) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2]; + } else { + rates(0) = _sensor_gyro.x; + rates(1) = _sensor_gyro.y; + rates(2) = _sensor_gyro.z; + } // rotate corrected measurements from sensor to body frame rates = _board_rotation * rates;