new Compass Lib

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

libraries/AP_Compass/AP_Compass.cpp

@@ -0,0 +1,114 @@
+/*
+	AP_Compass.cpp - Arduino Library for HMC5843 I2C Magnetometer
+	Code by Jordi Muñoz and Jose Julio. 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.
+
+	Sensor is conected to I2C port
+	Sensor is initialized in Continuos mode (10Hz)
+	
+	Variables:
+		heading : Magnetic heading
+		heading_X : Magnetic heading X component
+		heading_Y : Magnetic heading Y component
+		mag_X : Raw X axis magnetometer data
+		mag_Y : Raw Y axis magnetometer data
+		mag_Z : Raw Z axis magnetometer data		
+	
+	Methods:
+		init() : initialization of I2C and sensor
+		update() : update Sensor data		
+
+	To do : Calibration of the sensor, code optimization
+	Mount position : UPDATED
+	Big capacitor pointing backward, connector forward
+		
+*/
+
+
+extern "C" {
+	// AVR LibC Includes
+	#include <math.h>
+	#include "WConstants.h"
+}
+
+#include <Wire.h>
+#include "AP_Compass.h"
+
+#define COMPASS_ADDRESS 0x1E
+
+// Constructors ////////////////////////////////////////////////////////////////
+AP_Compass::AP_Compass()
+{
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+void
+AP_Compass::init(void)
+{
+	Wire.begin();	
+	Wire.beginTransmission(COMPASS_ADDRESS);
+	Wire.send(0x02); 
+	Wire.send(0x00);				// Set continouos mode (default to 10Hz)
+	Wire.endTransmission(); // end transmission
+}
+
+// update Sensor data
+void
+AP_Compass::update()
+{
+	int i = 0;
+	byte buff[6];
+ 
+	Wire.beginTransmission(COMPASS_ADDRESS); 
+	Wire.send(0x03);							// sends address to read from
+	Wire.endTransmission(); 					// end transmission
+	
+	//Wire.beginTransmission(COMPASS_ADDRESS); 
+	Wire.requestFrom(COMPASS_ADDRESS, 6);		// request 6 bytes from device
+	while(Wire.available()){ 
+		buff[i] = Wire.receive();				// receive one byte
+		i++;
+	}
+	Wire.endTransmission(); 					// end transmission
+	
+	// All bytes received?
+	if (i == 6)	{
+		// MSB byte first, then LSB, X,Y,Z
+		mag_X = -((((int)buff[0]) << 8) | buff[1]);		// X axis
+		mag_Y =  ((((int)buff[2]) << 8) | buff[3]);		// Y axis
+		mag_Z = -((((int)buff[4]) << 8) | buff[5]);		// Z axis
+	}
+}
+
+void
+AP_Compass::calculate(float roll, float pitch)
+{
+	float head_X;
+	float head_Y;
+	float cos_roll;
+	float sin_roll;
+	float cos_pitch;
+	float sin_pitch;
+	
+	cos_roll = cos(roll);							// Optimization, you can get this out of the matrix DCM?
+	sin_roll = sin(roll);
+	cos_pitch = cos(pitch);
+	sin_pitch = sin(pitch);
+	
+	// Tilt compensated Magnetic field X component:
+	head_X = mag_X * cos_pitch + mag_Y * sin_roll * sin_pitch + mag_Z * cos_roll * sin_pitch;
+	
+	// Tilt compensated Magnetic field Y component:
+	head_Y = mag_Y * cos_roll - mag_Z * sin_roll;
+	
+	// Magnetic heading
+	heading = atan2(-head_Y, head_X);
+	
+	// Optimization for external DCM use. calculate normalized components
+	heading_X = cos(heading);
+	heading_Y = sin(heading);
+}

libraries/AP_Compass/AP_Compass.h

@@ -0,0 +1,17 @@
+#ifndef AP_Compass_h
+#define AP_Compass_h
+
+#include <Compass.h>
+
+class AP_Compass : public Compass
+{
+  public:
+	AP_Compass();  // Constructor
+	void init();
+	void update();
+	void calculate(float roll, float pitch);
+	
+  private:	
+};
+
+#endif

libraries/AP_Compass/Compass.h

@@ -0,0 +1,22 @@
+#ifndef Compass_h
+#define Compass_h
+
+#include <inttypes.h>
+
+class Compass
+{
+  public:
+	virtual void init();
+	virtual void update();
+	virtual void calculate(float roll, float pitch);
+
+	int16_t mag_X;
+	int16_t mag_Y;
+	int16_t mag_Z;
+	float heading;
+	float heading_X;
+	float heading_Y;
+
+};
+
+#endif

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@@ -0,0 +1,40 @@
+/*
+	Example of APM_Compass library (HMC5843 sensor).
+	Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
+*/
+
+#include <Wire.h>
+#include <AP_Compass.h> // Compass Library
+
+AP_Compass compass;
+
+unsigned long timer;
+
+void setup()
+{	
+	compass.init();	 // Initialization
+	Serial.begin(38400);
+  Serial.println("AP Compass library test (HMC5843)");
+	delay(1000);
+	timer = millis();
+}
+
+void loop()
+{
+	float tmp;
+	
+	if((millis()- timer) > 100){
+		timer = millis();
+		compass.update();
+		compass.calculate(0, 0);	// roll = 0, pitch = 0 for this example
+	    Serial.print("Heading:");
+		Serial.print(degrees(compass.heading));
+    	Serial.print("  (");
+		Serial.print(compass.mag_X);
+	    Serial.print(",");
+		Serial.print(compass.mag_Y);
+    	Serial.print(",");
+		Serial.print(compass.mag_Z);
+		Serial.println();
+	}
+}

libraries/AP_Compass/keywords.txt

@@ -0,0 +1,15 @@
+Compass	KEYWORD1
+AP_Compass	KEYWORD1
+APM_Compass	KEYWORD1
+init	KEYWORD2
+update	KEYWORD2
+calculate	KEYWORD2
+heading	KEYWORD2
+heading_X	KEYWORD2
+heading_Y	KEYWORD2
+mag_X	KEYWORD2
+mag_Y	KEYWORD2
+mag_Z	KEYWORD2
+
+
+