Browse Source

Fix HIL DCM to match actual

mission-4.1.18
Michael Oborne 13 years ago
parent
commit
818c41f743
  1. 12
      libraries/AP_DCM/AP_DCM_HIL.cpp

12
libraries/AP_DCM/AP_DCM_HIL.cpp

@ -42,12 +42,12 @@ AP_DCM_HIL::setHil(float _roll, float _pitch, float _yaw, @@ -42,12 +42,12 @@ AP_DCM_HIL::setHil(float _roll, float _pitch, float _yaw,
float sPitch = sin(pitch), cPitch = cos(pitch);
float sYaw = sin(yaw), cYaw = cos(yaw);
_dcm_matrix.a.x = cPitch*cYaw;
_dcm_matrix.a.y = cPitch*sYaw;
_dcm_matrix.a.z = -sPitch;
_dcm_matrix.b.x = -cRoll*sYaw+sRoll*sPitch*cYaw;
_dcm_matrix.a.y = -cRoll*sYaw+sRoll*sPitch*cYaw;
_dcm_matrix.a.z = sRoll*sYaw+cRoll*sPitch*cYaw;
_dcm_matrix.b.x = cPitch*sYaw;
_dcm_matrix.b.y = cRoll*cYaw+sRoll*sPitch*sYaw;
_dcm_matrix.b.z = sRoll*cPitch;
_dcm_matrix.c.x = sRoll*sYaw+cRoll*sPitch*cYaw;
_dcm_matrix.c.y = -sRoll*cYaw+cRoll*sPitch*sYaw;
_dcm_matrix.b.z = -sRoll*cYaw+cRoll*sPitch*sYaw;
_dcm_matrix.c.x = -sPitch;
_dcm_matrix.c.y = sRoll*cPitch;
_dcm_matrix.c.z = cRoll*cPitch;
}

Loading…
Cancel
Save