You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
53 lines
1.8 KiB
53 lines
1.8 KiB
/* |
|
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 = cPitch*sYaw; |
|
_dcm_matrix.a.z = -sPitch; |
|
_dcm_matrix.b.x = -cRoll*sYaw+sRoll*sPitch*cYaw; |
|
_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.c.z = cRoll*cPitch; |
|
}
|
|
|