• Main Page
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

/home/jgoppert/Projects/ap/libraries/AP_DCM/AP_DCM_HIL.cpp

Go to the documentation of this file.
00001 /*
00002         APM_DCM_HIL.cpp - DCM AHRS Library, for Ardupilot Mega, Hardware in the Loop Model
00003                 Code by James Goppert. DIYDrones.com
00004 
00005         This library is free software; you can redistribute it and/or
00006     modify it under the terms of the GNU Lesser General Public
00007     License as published by the Free Software Foundation; either
00008     version 2.1 of the License, or (at your option) any later version.
00009 
00010         Methods:
00011                                 update_DCM(_G_Dt)       : Updates the AHRS by integrating the rotation matrix over time _G_Dt using the IMU object data
00012                                 get_gyro()                      : Returns gyro vector corrected for bias
00013                                 get_accel()             : Returns accelerometer vector
00014                                 get_dcm_matrix()        : Returns dcm matrix
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         // Need the standard C_body<-nav dcm from navigation frame to body frame
00040         // Strapdown Inertial Navigation Technology / Titterton/ pg. 41
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 }

Generated for ArduPilot Libraries by doxygen