ardupilot/libraries/AP_DCM/AP_DCM_HIL.cpp

54 lines
1.8 KiB
C++

/*
APM_DCM_HIL.cpp - DCM AHRS Library, for Ardupilot Mega, Hardware in the Loop Model
Code by James Goppert. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
Methods:
update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data
get_gyro() : Returns gyro vector corrected for bias
get_accel() : Returns accelerometer vector
get_dcm_matrix() : Returns dcm matrix
*/
#include <AP_DCM_HIL.h>
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
/**************************************************/
void
AP_DCM_HIL::setHil(float _roll, float _pitch, float _yaw,
float _rollRate, float _pitchRate, float _yawRate)
{
roll = _roll;
pitch = _pitch;
yaw = _yaw;
_omega_integ_corr.x = _rollRate;
_omega_integ_corr.y = _pitchRate;
_omega_integ_corr.z = _yawRate;
roll_sensor =ToDeg(roll)*100;
pitch_sensor =ToDeg(pitch)*100;
yaw_sensor =ToDeg(yaw)*100;
// Need the standard C_body<-nav dcm from navigation frame to body frame
// Strapdown Inertial Navigation Technology / Titterton/ pg. 41
float sRoll = sin(roll), cRoll = cos(roll);
float sPitch = sin(pitch), cPitch = cos(pitch);
float sYaw = sin(yaw), cYaw = cos(yaw);
_dcm_matrix.a.x = cPitch*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*cYaw+cRoll*sPitch*sYaw;
_dcm_matrix.c.x = -sPitch;
_dcm_matrix.c.y = sRoll*cPitch;
_dcm_matrix.c.z = cRoll*cPitch;
}