Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 10:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: AP_Compass_test: report multiple compasses

Browse Source 
Report data from all available compasses, not just the primary one.
This commit is contained in:
Gustavo Jose de Sousa 2016-01-22 15:29:41 -02:00 committed by Lucas De Marchi
parent 16acca865f
commit a61f9855d3
1 changed files with 64 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -33,7 +33,10 @@ void setup() {
void loop()
{
static float min[3], max[3], offset[3];
static const uint8_t compass_count = compass.get_count();
static float min[COMPASS_MAX_INSTANCES][3];
static float max[COMPASS_MAX_INSTANCES][3];
static float offset[COMPASS_MAX_INSTANCES][3];
compass.accumulate();
@ -41,62 +44,68 @@ void loop()
timer = AP_HAL::micros();
compass.read();
unsigned long read_time = AP_HAL::micros() - timer;
float heading;
if (!compass.healthy()) {
hal.console->println("not healthy");
return;
for (uint8_t i = 0; i < compass_count; i++) {
float heading;
hal.console->printf("Compass #%u: ", i);
if (!compass.healthy()) {
hal.console->println("not healthy");
continue;
}
Matrix3f dcm_matrix;
// use roll = 0, pitch = 0 for this example
dcm_matrix.from_euler(0, 0, 0);
heading = compass.calculate_heading(dcm_matrix, i);
compass.learn_offsets();
const Vector3f &mag = compass.get_field(i);
// capture min
if (mag.x < min[i][0]) {
min[i][0] = mag.x;
}
if (mag.y < min[i][1]) {
min[i][1] = mag.y;
}
if (mag.z < min[i][2]) {
min[i][2] = mag.z;
}
// capture max
if (mag.x > max[i][0]) {
max[i][0] = mag.x;
}
if (mag.y > max[i][1]) {
max[i][1] = mag.y;
}
if (mag.z > max[i][2]) {
max[i][2] = mag.z;
}
// calculate offsets
offset[i][0] = -(max[i][0] + min[i][0]) / 2;
offset[i][1] = -(max[i][1] + min[i][1]) / 2;
offset[i][2] = -(max[i][2] + min[i][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());
// display offsets
hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
offset[i][0], offset[i][1], offset[i][2]);
hal.console->printf(" t=%u", (unsigned)read_time);
hal.console->println();
}
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();
const Vector3f &mag = compass.get_field();
// capture min
if (mag.x < min[0]) {
min[0] = mag.x;
}
if (mag.y < min[1]) {
min[1] = mag.y;
}
if (mag.z < min[2]) {
min[2] = mag.z;
}
// capture max
if (mag.x > max[0]) {
max[0] = mag.x;
}
if (mag.y > max[1]) {
max[1] = mag.y;
}
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;
// 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());
// display offsets
hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
offset[0], offset[1], offset[2]);
hal.console->printf(" t=%u", (unsigned)read_time);
hal.console->println();
} else {
hal.scheduler->delay(1);
}