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 8e939618fe..493ab037d0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -996,16 +996,16 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) 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]; + rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1]; + rates(2) = (_sensor_gyro.z - _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]; + rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1]; + rates(2) = (_sensor_gyro.z - _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]; + rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1]; + rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2]; } else { rates(0) = _sensor_gyro.x;