Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: AP_Compass_test: fix style 

Used uncrustify and some manual changes.
This commit is contained in:
Gustavo Jose de Sousa 2016-01-22 14:40:43 -02:00 committed by Lucas De Marchi
parent 0efbe8c80c
commit 16acca865f
1 changed files with 34 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.cpp
View File

@ -17,12 +17,15 @@ void setup() {
if (!compass.init()) {
hal.console->println("compass initialisation failed!");
while (1) ;
while (1)
;
}
hal.console->printf("init done - %u compasses detected\n", compass.get_count());
compass.set_and_save_offsets(0,0,0,0); // set offsets to account for surrounding interference
compass.set_declination(ToRad(0.0f)); // set local difference between magnetic north and true north
// set offsets to account for surrounding interference
compass.set_and_save_offsets(0, 0, 0, 0);
// set local difference between magnetic north and true north
compass.set_declination(ToRad(0.0f));
hal.scheduler->delay(1000);
timer = AP_HAL::micros();
@ -34,8 +37,7 @@ void loop()
compass.accumulate();
if((AP_HAL::micros()- timer) > 100000L)
{
if ((AP_HAL::micros() - timer) > 100000L) {
timer = AP_HAL::micros();
compass.read();
unsigned long read_time = AP_HAL::micros() - timer;
@ -45,51 +47,58 @@ void loop()
hal.console->println("not healthy");
return;
}
Matrix3f dcm_matrix;
// use roll = 0, pitch = 0 for this example
dcm_matrix.from_euler(0, 0, 0);
Matrix3f dcm_matrix;
// use roll = 0, pitch = 0 for this example
dcm_matrix.from_euler(0, 0, 0);
heading = compass.calculate_heading(dcm_matrix);
compass.learn_offsets();
// capture min
const Vector3f &mag = compass.get_field();
if( mag.x < min[0] )
// capture min
if (mag.x < min[0]) {
min[0] = mag.x;
if( mag.y < min[1] )
}
if (mag.y < min[1]) {
min[1] = mag.y;
if( mag.z < min[2] )
}
if (mag.z < min[2]) {
min[2] = mag.z;
}
// capture max
if( mag.x > max[0] )
if (mag.x > max[0]) {
max[0] = mag.x;
if( mag.y > max[1] )
}
if (mag.y > max[1]) {
max[1] = mag.y;
if( mag.z > max[2] )
}
if (mag.z > max[2]) {
max[2] = 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;
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
hal.console->printf("Heading: %.2f (%3d,%3d,%3d) i2c error: %u",
ToDeg(heading),
(int)mag.x,
(int)mag.y,
(int)mag.z,
(unsigned)hal.i2c->lockup_count());
ToDeg(heading),
(int)mag.x,
(int)mag.y,
(int)mag.z,
(unsigned)hal.i2c->lockup_count());
// display offsets
hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
offset[0], offset[1], offset[2]);
offset[0], offset[1], offset[2]);
hal.console->printf(" t=%u", (unsigned)read_time);
hal.console->println();
} else {
hal.scheduler->delay(1);
hal.scheduler->delay(1);
}
}