mirror of https://github.com/ArduPilot/ardupilot
AP_Compass - moved example sketch from APM_Compass to AP_Compass
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1112 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
f94a126061
commit
af741882c6
|
@ -7,35 +7,79 @@
|
||||||
#include <AP_Compass_HMC5843.h> // Compass Library
|
#include <AP_Compass_HMC5843.h> // Compass Library
|
||||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||||
|
|
||||||
|
#define ToRad(x) (x*0.01745329252) // *pi/180
|
||||||
|
#define ToDeg(x) (x*57.2957795131) // *180/pi
|
||||||
|
|
||||||
AP_Compass_HMC5843 compass;
|
AP_Compass_HMC5843 compass;
|
||||||
|
|
||||||
|
|
||||||
unsigned long timer;
|
unsigned long timer;
|
||||||
|
|
||||||
void setup()
|
void setup()
|
||||||
{
|
{
|
||||||
compass.init(); // Initialization
|
Serial.begin(38400);
|
||||||
Serial.begin(38400);
|
Serial.println("Compass library test (HMC5843)");
|
||||||
Serial.println("AP Compass library test (HMC5843)");
|
compass.init(); // Initialization
|
||||||
delay(1000);
|
|
||||||
timer = millis();
|
compass.set_orientation(AP_COMPASS_COMPONENTS_UP_PINS_FORWARD); // set compass's orientation on aircraft
|
||||||
|
compass.set_offsets(0,0,0); // set offsets to account for surrounding interference
|
||||||
|
compass.set_declination(ToRad(0.0)); // set local difference between magnetic north and true north
|
||||||
|
|
||||||
|
delay(1000);
|
||||||
|
timer = millis();
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop()
|
void loop()
|
||||||
{
|
{
|
||||||
float tmp;
|
static float min[3], max[3], offset[3];
|
||||||
|
|
||||||
if((millis()- timer) > 100){
|
if((millis()- timer) > 100)
|
||||||
timer = millis();
|
{
|
||||||
compass.read();
|
timer = millis();
|
||||||
compass.calculate(0, 0); // roll = 0, pitch = 0 for this example
|
compass.read();
|
||||||
Serial.print("Heading:");
|
compass.calculate(0,0); // roll = 0, pitch = 0 for this example
|
||||||
Serial.print(compass.heading, DEC);
|
|
||||||
Serial.print(" (");
|
// capture min
|
||||||
Serial.print(compass.mag_x);
|
if( compass.mag_x < min[0] )
|
||||||
Serial.print(",");
|
min[0] = compass.mag_x;
|
||||||
Serial.print(compass.mag_y);
|
if( compass.mag_y < min[1] )
|
||||||
Serial.print(",");
|
min[1] = compass.mag_y;
|
||||||
Serial.print(compass.mag_z);
|
if( compass.mag_z < min[2] )
|
||||||
Serial.println(" )");
|
min[2] = compass.mag_z;
|
||||||
}
|
|
||||||
}
|
// capture max
|
||||||
|
if( compass.mag_x > max[0] )
|
||||||
|
max[0] = compass.mag_x;
|
||||||
|
if( compass.mag_y > max[1] )
|
||||||
|
max[1] = compass.mag_y;
|
||||||
|
if( compass.mag_z > max[2] )
|
||||||
|
max[2] = compass.mag_z;
|
||||||
|
|
||||||
|
// calculate offsets
|
||||||
|
offset[0] = -(max[0]+min[0])/2;
|
||||||
|
offset[1] = -(max[1]+min[1])/2;
|
||||||
|
offset[2] = -(max[2]+min[2])/2;
|
||||||
|
|
||||||
|
// display all to user
|
||||||
|
Serial.print("Heading:");
|
||||||
|
Serial.print(ToDeg(compass.heading));
|
||||||
|
Serial.print(" (");
|
||||||
|
Serial.print(compass.mag_x);
|
||||||
|
Serial.print(",");
|
||||||
|
Serial.print(compass.mag_y);
|
||||||
|
Serial.print(",");
|
||||||
|
Serial.print(compass.mag_z);
|
||||||
|
Serial.print(")");
|
||||||
|
|
||||||
|
// display offsets
|
||||||
|
Serial.print("\t offsets(");
|
||||||
|
Serial.print(offset[0]);
|
||||||
|
Serial.print(",");
|
||||||
|
Serial.print(offset[1]);
|
||||||
|
Serial.print(",");
|
||||||
|
Serial.print(offset[2]);
|
||||||
|
Serial.print(")");
|
||||||
|
|
||||||
|
Serial.println();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -2,14 +2,18 @@ Compass KEYWORD1
|
||||||
AP_Compass KEYWORD1
|
AP_Compass KEYWORD1
|
||||||
APM_Compass KEYWORD1
|
APM_Compass KEYWORD1
|
||||||
init KEYWORD2
|
init KEYWORD2
|
||||||
update KEYWORD2
|
read KEYWORD2
|
||||||
calculate KEYWORD2
|
calculate KEYWORD2
|
||||||
|
set_orientation KEYWORD2
|
||||||
|
set_offsets KEYWORD2
|
||||||
|
set_declination KEYWORD2
|
||||||
heading KEYWORD2
|
heading KEYWORD2
|
||||||
heading_X KEYWORD2
|
heading_X KEYWORD2
|
||||||
heading_Y KEYWORD2
|
heading_Y KEYWORD2
|
||||||
mag_x KEYWORD2
|
mag_x KEYWORD2
|
||||||
mag_y KEYWORD2
|
mag_y KEYWORD2
|
||||||
mag_z KEYWORD2
|
mag_z KEYWORD2
|
||||||
|
last_update KEYWORD2
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue