mirror of https://github.com/ArduPilot/ardupilot
uncrustify libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde
This commit is contained in:
uncrustify
Pat Hickey
parent
4b60e53736
commit
53ef9e8b9e
|
|
@ -1,12 +1,12 @@
|
|||
/*
|
||||
Example of APM_Compass library (HMC5843 sensor).
|
||||
Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
|
||||
*/
|
||||
* Example of APM_Compass library (HMC5843 sensor).
|
||||
* Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
|
||||
*/
|
||||
|
||||
#include <FastSerial.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Compass.h> // Compass Library
|
||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <I2C.h>
|
||||
|
||||
FastSerialPort0(Serial);
|
||||
|
|
@ -21,93 +21,93 @@ unsigned long timer;
|
|||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial.println("Compass library test (HMC5843 and HMC5883L)");
|
||||
I2c.begin();
|
||||
I2c.timeOut(20);
|
||||
Serial.begin(115200);
|
||||
Serial.println("Compass library test (HMC5843 and HMC5883L)");
|
||||
I2c.begin();
|
||||
I2c.timeOut(20);
|
||||
|
||||
// I2c.setSpeed(true);
|
||||
// I2c.setSpeed(true);
|
||||
|
||||
if (!compass.init()) {
|
||||
Serial.println("compass initialisation failed!");
|
||||
while (1) ;
|
||||
}
|
||||
if (!compass.init()) {
|
||||
Serial.println("compass initialisation failed!");
|
||||
while (1) ;
|
||||
}
|
||||
|
||||
compass.set_orientation(AP_COMPASS_COMPONENTS_DOWN_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
|
||||
compass.set_orientation(AP_COMPASS_COMPONENTS_DOWN_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
|
||||
|
||||
Serial.print("Compass auto-detected as: ");
|
||||
switch( compass.product_id ) {
|
||||
case AP_COMPASS_TYPE_HIL:
|
||||
Serial.println("HIL");
|
||||
break;
|
||||
case AP_COMPASS_TYPE_HMC5843:
|
||||
Serial.println("HMC5843");
|
||||
break;
|
||||
case AP_COMPASS_TYPE_HMC5883L:
|
||||
Serial.println("HMC5883L");
|
||||
break;
|
||||
default:
|
||||
Serial.println("unknown");
|
||||
break;
|
||||
}
|
||||
|
||||
delay(3000);
|
||||
timer = micros();
|
||||
Serial.print("Compass auto-detected as: ");
|
||||
switch( compass.product_id ) {
|
||||
case AP_COMPASS_TYPE_HIL:
|
||||
Serial.println("HIL");
|
||||
break;
|
||||
case AP_COMPASS_TYPE_HMC5843:
|
||||
Serial.println("HMC5843");
|
||||
break;
|
||||
case AP_COMPASS_TYPE_HMC5883L:
|
||||
Serial.println("HMC5883L");
|
||||
break;
|
||||
default:
|
||||
Serial.println("unknown");
|
||||
break;
|
||||
}
|
||||
|
||||
delay(3000);
|
||||
timer = micros();
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
static float min[3], max[3], offset[3];
|
||||
static float min[3], max[3], offset[3];
|
||||
|
||||
if((micros()- timer) > 100000L)
|
||||
{
|
||||
timer = micros();
|
||||
compass.read();
|
||||
unsigned long read_time = micros() - timer;
|
||||
float heading;
|
||||
if((micros()- timer) > 100000L)
|
||||
{
|
||||
timer = micros();
|
||||
compass.read();
|
||||
unsigned long read_time = micros() - timer;
|
||||
float heading;
|
||||
|
||||
if (!compass.healthy) {
|
||||
Serial.println("not healthy");
|
||||
return;
|
||||
if (!compass.healthy) {
|
||||
Serial.println("not healthy");
|
||||
return;
|
||||
}
|
||||
heading = compass.calculate_heading(0,0); // roll = 0, pitch = 0 for this example
|
||||
|
||||
// capture min
|
||||
if( compass.mag_x < min[0] )
|
||||
min[0] = compass.mag_x;
|
||||
if( compass.mag_y < min[1] )
|
||||
min[1] = compass.mag_y;
|
||||
if( compass.mag_z < min[2] )
|
||||
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.printf("Heading: %.2f (%3u,%3u,%3u) ",
|
||||
ToDeg(heading),
|
||||
compass.mag_x,
|
||||
compass.mag_y,
|
||||
compass.mag_z);
|
||||
|
||||
// display offsets
|
||||
Serial.printf("\t offsets(%.2f, %.2f, %.2f)",
|
||||
offset[0], offset[1], offset[2]);
|
||||
|
||||
Serial.printf(" t=%u", (unsigned)read_time);
|
||||
|
||||
Serial.println();
|
||||
}
|
||||
heading = compass.calculate_heading(0,0); // roll = 0, pitch = 0 for this example
|
||||
|
||||
// capture min
|
||||
if( compass.mag_x < min[0] )
|
||||
min[0] = compass.mag_x;
|
||||
if( compass.mag_y < min[1] )
|
||||
min[1] = compass.mag_y;
|
||||
if( compass.mag_z < min[2] )
|
||||
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.printf("Heading: %.2f (%3u,%3u,%3u) ",
|
||||
ToDeg(heading),
|
||||
compass.mag_x,
|
||||
compass.mag_y,
|
||||
compass.mag_z);
|
||||
|
||||
// display offsets
|
||||
Serial.printf("\t offsets(%.2f, %.2f, %.2f)",
|
||||
offset[0], offset[1], offset[2]);
|
||||
|
||||
Serial.printf(" t=%u", (unsigned)read_time);
|
||||
|
||||
Serial.println();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue