From a61f9855d37a8911066a315745ab26881ae27b0e Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Fri, 22 Jan 2016 15:29:41 -0200
Subject: [PATCH] AP_Compass: AP_Compass_test: report multiple compasses

Report data from all available compasses, not just the primary one.
---
 .../AP_Compass_test/AP_Compass_test.cpp       | 119 ++++++++++--------
 1 file changed, 64 insertions(+), 55 deletions(-)

diff --git a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.cpp b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.cpp
index bc06bb10ef..0f667e3bc6 100644
--- a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.cpp
+++ b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.cpp
@@ -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);
     }