Added sensor level HIL support.

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

libraries/APM_ADC/APM_ADC.cpp

@@ -145,5 +145,32 @@ int APM_ADC_Class::Ch(unsigned char ch_num)
   return(result);
 }
 
-// make one instance for the user to use
-APM_ADC_Class APM_ADC;
+// Constructors ////////////////////////////////////////////////////////////////
+APM_ADC_HIL_Class::APM_ADC_HIL_Class()
+{
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+void APM_ADC_HIL_Class::Init(void)
+{
+}
+
+// Read one channel value
+int APM_ADC_HIL_Class::Ch(unsigned char ch_num)         
+{
+  return adc_value[ch_num];
+}
+
+// Set one channel value
+int APM_ADC_HIL_Class::setHIL(float p, float q, float r, float gyroTemp,
+    float aX, float aY, float aZ, float diffPress)
+{
+    // TODO: map floats to raw
+    adc_value[0] = r;
+    adc_value[1] = p;
+    adc_value[2] = q;
+    adc_value[4] = gyroTemp; 
+    adc_value[5] = aX;
+    adc_value[6] = aY;
+    adc_value[7] = aZ;
+}

libraries/APM_ADC/APM_ADC.h

@@ -19,6 +19,15 @@ class APM_ADC_Class
 	int Ch(unsigned char ch_num);     
 };
 
-extern APM_ADC_Class APM_ADC;
+class APM_ADC_HIL_Class
+{
+  private:
+  public:
+	APM_ADC_HIL_Class();  // Constructor
+	void Init();
+	int Ch(unsigned char ch_num);     
+	int setHIL(float p, float q, float r, float gyroTemp,
+            float aX, float aY, float aZ, float diffPress);     
+};
 
-#endif
+#endif

libraries/APM_ADC/examples/APM_ADC_test/APM_ADC_test.pde

@@ -7,6 +7,8 @@
 
 unsigned long timer;
 
+APM_ADC_Class APM_ADC;
+
 void setup()
 {  
   APM_ADC.Init();   // APM ADC initialization
@@ -30,4 +32,4 @@ void loop()
       }
     Serial.println();
     }
-}
+}

libraries/APM_BMP085/APM_BMP085.cpp

@@ -238,6 +238,47 @@ void APM_BMP085_Class::Calculate()
   Press = p + ((x1 + x2 + 3791) >> 4);
 }
 
+// Constructors ////////////////////////////////////////////////////////////////
+APM_BMP085_HIL_Class::APM_BMP085_HIL_Class()
+{
+}
 
-// make one instance for the user to use
-APM_BMP085_Class APM_BMP085;
+// Public Methods //////////////////////////////////////////////////////////////
+void APM_BMP085_HIL_Class::Init(void)
+{
+  BMP085_State=1;
+}
+
+
+// Read the sensor. This is a state machine
+// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
+uint8_t APM_BMP085_HIL_Class::Read()
+{
+    uint8_t result=0;
+
+	if (BMP085_State==1)
+	{
+        BMP085_State++;
+	}
+	else
+	{
+		if (BMP085_State==5)
+		{
+            BMP085_State = 1;    // Start again from state=1
+            result=1;            // New pressure reading
+		}
+		else
+		{
+            BMP085_State++;
+            result=1;            // New pressure reading
+		}
+	}
+    return(result);
+}
+
+void APM_BMP085_HIL_Class::setHIL(float _Temp, float _Press)
+{
+    // TODO: map floats to raw
+    Temp = _Temp;
+    Press = _Press;
+}

libraries/APM_BMP085/APM_BMP085.h

@@ -29,6 +29,21 @@ class APM_BMP085_Class
 	uint8_t Read();	
 };
 
-extern APM_BMP085_Class APM_BMP085;
+class APM_BMP085_HIL_Class
+{
+  private:
+    uint8_t BMP085_State;
+  public:
+	int32_t RawPress;
+	int32_t RawTemp;
+	int16_t Temp;
+	int32_t Press;
+	//int Altitude;
+	uint8_t oss;
+	APM_BMP085_HIL_Class();  // Constructor
+	void Init();
+	uint8_t Read();	
+    void setHIL(float Temp, float Press);
+};
 
-#endif
+#endif

libraries/APM_BMP085/examples/APM_BMP085_test/APM_BMP085_test.pde

@@ -6,6 +6,8 @@
 #include <Wire.h>
 #include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
 
+APM_BMP085_Class APM_BMP085;
+
 unsigned long timer;
 
 void setup()
@@ -38,4 +40,4 @@ void loop()
     Serial.print(Altitude);
     Serial.println();
     }
-}
+}

libraries/APM_Compass/APM_Compass.cpp

@@ -231,5 +231,128 @@ void APM_Compass_Class::SetDeclination(float radians)
     declination = radians;
 }
 
-// make one instance for the user to use
-APM_Compass_Class APM_Compass;
+
+// Constructors ////////////////////////////////////////////////////////////////
+APM_Compass_HIL_Class::APM_Compass_HIL_Class() : orientation(0), declination(0.0)
+{
+  // mag x y z offset initialisation
+  offset[0] = 0;
+  offset[1] = 0;
+  offset[2] = 0;
+  
+  // initialise orientation matrix
+  orientationMatrix = ROTATION_NONE;
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+bool APM_Compass_HIL_Class::Init(void)
+{
+  unsigned long currentTime = millis();  // record current time
+  int numAttempts = 0;
+  int success = 0;
+  
+  // calibration initialisation
+  calibration[0] = 1.0;
+  calibration[1] = 1.0;
+  calibration[2] = 1.0;
+  
+  while( success == 0 && numAttempts < 5 )
+  {
+      // record number of attempts at initialisation
+	  numAttempts++;
+  
+	  // read values from the compass
+	  Read();
+	  delay(10);
+
+	  // calibrate
+	  if( abs(Mag_X) > 500 && abs(Mag_X) < 1000 && abs(Mag_Y) > 500 && abs(Mag_Y) < 1000 && abs(Mag_Z) > 500 && abs(Mag_Z) < 1000)
+	  {
+		  calibration[0] = abs(715.0 / Mag_X);
+		  calibration[1] = abs(715.0 / Mag_Y);
+		  calibration[2] = abs(715.0 / Mag_Z);
+		  
+		  // mark success
+		  success = 1;
+	  }
+  }
+  return(success);
+}
+
+// Read Sensor data
+void APM_Compass_HIL_Class::Read()
+{
+    // values set by setHIL function
+}
+
+void APM_Compass_HIL_Class::Calculate(float roll, float pitch)
+{
+  float Head_X;
+  float Head_Y;
+  float cos_roll;
+  float sin_roll;
+  float cos_pitch;
+  float sin_pitch;
+  Vector3f rotMagVec;
+  
+  cos_roll = cos(roll);  // Optimizacion, se puede sacar esto de la matriz DCM?
+  sin_roll = sin(roll);
+  cos_pitch = cos(pitch);
+  sin_pitch = sin(pitch);
+  
+  // rotate the magnetometer values depending upon orientation
+  if( orientation == 0 )
+      rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]);  
+  else
+      rotMagVec = orientationMatrix*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); 
+  
+  // Tilt compensated Magnetic field X component:
+  Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
+  // Tilt compensated Magnetic field Y component:
+  Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
+  // Magnetic Heading
+  Heading = atan2(-Head_Y,Head_X);
+  
+  // Declination correction (if supplied)
+  if( declination != 0.0 ) 
+  {
+      Heading = Heading + declination;
+      if (Heading > M_PI)    // Angle normalization (-180 deg, 180 deg)
+          Heading -= (2.0 * M_PI);
+      else if (Heading < -M_PI)
+          Heading += (2.0 * M_PI);
+  }
+	
+  // Optimization for external DCM use. Calculate normalized components
+  Heading_X = cos(Heading);
+  Heading_Y = sin(Heading);
+}
+
+void APM_Compass_HIL_Class::SetOrientation(const Matrix3f &rotationMatrix)
+{
+    orientationMatrix = rotationMatrix;
+	if( orientationMatrix == ROTATION_NONE )
+	    orientation = 0;
+	else
+	    orientation = 1;
+}
+
+void APM_Compass_HIL_Class::SetOffsets(int x, int y, int z)
+{
+    offset[0] = x;
+	offset[1] = y;
+	offset[2] = z;
+}
+
+void APM_Compass_HIL_Class::SetDeclination(float radians)
+{
+    declination = radians;
+}
+
+void APM_Compass_HIL_Class::setHIL(float _Mag_X, float _Mag_Y, float _Mag_Z)
+{
+    // TODO: map floats to raw
+    Mag_X = _Mag_X;
+    Mag_Y = _Mag_Y;
+    Mag_Z = _Mag_Z;
+}

libraries/APM_Compass/APM_Compass.h

@@ -83,6 +83,31 @@ class APM_Compass_Class
 	void SetDeclination(float radians);
 };
 
-extern APM_Compass_Class APM_Compass;
+class APM_Compass_HIL_Class
+{
+  private:
+	int orientation;
+	Matrix3f orientationMatrix;
+	float calibration[3];
+	int offset[3];
+	float declination;
+  public:
+	int Mag_X;
+	int Mag_Y;
+	int Mag_Z;
+	float Heading;
+	float Heading_X;
+	float Heading_Y;
+	unsigned long lastUpdate;	
+	
+	APM_Compass_HIL_Class();  // Constructor
+	bool Init();
+	void Read();
+	void Calculate(float roll, float pitch);
+	void SetOrientation(const Matrix3f &rotationMatrix);
+	void SetOffsets(int x, int y, int z);
+	void SetDeclination(float radians);
+    void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
+};
 
 #endif

libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde

@@ -16,6 +16,7 @@
 #define ToDeg(x) (x*57.2957795131)  // *180/pi
 
 unsigned long timer;
+APM_Compass_Class APM_Compass;
 
 void setup()
 {  

libraries/AP_GPS/AP_GPS.h

@@ -11,3 +11,4 @@
 #include "AP_GPS_IMU.h"
 #include "AP_GPS_None.h"
 #include "AP_GPS_Auto.h"
+#include "AP_GPS_HIL.h"

libraries/AP_GPS/AP_GPS_HIL.cpp

@@ -0,0 +1,52 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+//
+//  Hardware in the loop gps class.
+//	Code by James Goppert
+//
+//	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.
+//
+//	GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
+//
+
+#include "AP_GPS_HIL.h"
+#include "WProgram.h"
+
+// Constructors ////////////////////////////////////////////////////////////////
+AP_GPS_HIL::AP_GPS_HIL(Stream *s) : GPS(s)
+{
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+void AP_GPS_HIL::init(void)
+{	
+}
+
+void AP_GPS_HIL::update(void)
+{
+}
+
+int AP_GPS_HIL::status(void)
+{
+    return 2; // gps locked
+    // TODO: show as locked after first packet received
+}
+
+void AP_GPS_HIL::setHIL(long _time, long _latitude, long _longitude, long _altitude,
+    long _ground_speed, long _ground_course, long _speed_3d, uint8_t _num_sats)
+{
+    time = _time;
+    latitude = _latitude;
+    longitude = _longitude;
+    altitude = _altitude;
+    ground_speed = _ground_speed;
+    ground_course = _ground_course;
+    speed_3d = _speed_3d;
+    num_sats = _num_sats;
+    new_data = true;
+    fix = true;
+    valid_read = true;
+}
+

libraries/AP_GPS/AP_GPS_HIL.h

@@ -0,0 +1,27 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+//
+//  Hardware in the loop gps class.
+//  Code by James Goppert
+//
+//	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.
+//
+//
+#ifndef AP_GPS_HIL_h
+#define AP_GPS_HIL_h
+
+#include <GPS.h>
+
+class AP_GPS_HIL : public GPS {
+public:
+	AP_GPS_HIL(Stream *s);
+	void init(void);
+	void update(void);
+    int status(void);
+    void setHIL(long time, long latitude, long longitude, long altitude,
+            long ground_speed, long ground_course, long speed_3d, uint8_t num_sats);
+};
+
+#endif	// AP_GPS_HIL_H