Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix HIL DCM to match actual

This commit is contained in:
Michael Oborne 2011-10-28 17:30:53 +08:00
parent 8eb74df024
commit 1c72598393
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_DCM/AP_DCM_HIL.cpp
View File

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