Added AP_DCM_HIL class.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1103 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/AP_DCM/AP_DCM.h

@@ -1,6 +1,12 @@
 #ifndef AP_DCM_h
 #define AP_DCM_h
 
+// teporarily include all other classes here
+// since this naming is a bit off from the
+// convention and the AP_DCM should be the top
+// header file
+#include "AP_DCM_HIL.h"
+
 #include <FastSerial.h>
 #include <AP_Math.h>
 #include <inttypes.h>

libraries/AP_DCM/AP_DCM_HIL.cpp

@@ -0,0 +1,53 @@
+/*
+	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(_G_Dt)	: Updates the AHRS by integrating the rotation matrix over time _G_Dt 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;
+}

libraries/AP_DCM/AP_DCM_HIL.h

@@ -0,0 +1,61 @@
+#ifndef AP_DCM_HIL_H
+#define AP_DCM_HIL_H
+
+#include <FastSerial.h>
+#include <AP_Math.h>
+#include <inttypes.h>
+#include "WProgram.h"
+#include <AP_Compass.h>
+#include <AP_ADC.h>
+#include <AP_GPS.h>
+#include <AP_IMU.h>
+
+
+class AP_DCM_HIL
+{
+public:
+	// Constructors
+	AP_DCM_HIL() :
+		_dcm_matrix(1, 0, 0,
+					0, 1, 0,
+					0, 0, 1)
+	{}
+
+	// Accessors
+	Vector3f	get_gyro(void) {return _omega_integ_corr; }
+	Vector3f	get_accel(void) { return _accel_vector; }
+	Matrix3f	get_dcm_matrix(void) {return _dcm_matrix; }
+	
+	void		set_centripetal(bool b) {}
+	void		set_compass(Compass *compass) {}
+
+	// Methods
+	void 		update_DCM(float _G_Dt) {}
+
+	float		get_health(void) { return 1.0; }
+
+	long		roll_sensor;					// Degrees * 100
+	long		pitch_sensor;					// Degrees * 100
+	long		yaw_sensor;						// Degrees * 100
+
+	float		roll;							// Radians
+	float		pitch;							// Radians
+	float		yaw;							// Radians
+
+	uint8_t 	gyro_sat_count;
+	uint8_t 	renorm_sqrt_count;
+	uint8_t 	renorm_blowup_count;
+
+	void 		setHil(float roll, float pitch, float yaw,
+					float rollRate, float pitchRate, float yawRate);
+private:
+	// Methods
+		Matrix3f	_dcm_matrix;
+	Vector3f 	_omega_integ_corr;
+	Vector3f 	_accel_vector;
+};
+
+#endif
+
+
+