Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <AP_DCM_HIL.h>
00018
00019 #define ToRad(x) (x*0.01745329252) // *pi/180
00020 #define ToDeg(x) (x*57.2957795131) // *180/pi
00021
00022
00023 void
00024 AP_DCM_HIL::setHil(float _roll, float _pitch, float _yaw,
00025 float _rollRate, float _pitchRate, float _yawRate)
00026 {
00027 roll = _roll;
00028 pitch = _pitch;
00029 yaw = _yaw;
00030
00031 _omega_integ_corr.x = _rollRate;
00032 _omega_integ_corr.y = _pitchRate;
00033 _omega_integ_corr.z = _yawRate;
00034
00035 roll_sensor =ToDeg(roll)*100;
00036 pitch_sensor =ToDeg(pitch)*100;
00037 yaw_sensor =ToDeg(yaw)*100;
00038
00039
00040
00041 float sRoll = sin(roll), cRoll = cos(roll);
00042 float sPitch = sin(pitch), cPitch = cos(pitch);
00043 float sYaw = sin(yaw), cYaw = cos(yaw);
00044 _dcm_matrix.a.x = cPitch*cYaw;
00045 _dcm_matrix.a.y = cPitch*sYaw;
00046 _dcm_matrix.a.z = -sPitch;
00047 _dcm_matrix.b.x = -cRoll*sYaw+sRoll*sPitch*cYaw;
00048 _dcm_matrix.b.y = cRoll*cYaw+sRoll*sPitch*sYaw;
00049 _dcm_matrix.b.z = sRoll*cPitch;
00050 _dcm_matrix.c.x = sRoll*sYaw+cRoll*sPitch*cYaw;
00051 _dcm_matrix.c.y = -sRoll*cYaw+cRoll*sPitch*sYaw;
00052 _dcm_matrix.c.z = cRoll*cPitch;
00053 }