Diff: main.cpp

Revision:

0:5090d2766d5e

Child:

1:2d4aa9e794a5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jun 20 13:56:50 2021 +0000
@@ -0,0 +1,92 @@
+#include "mbed.h"
+#include "HSCDTD008A.h"
+
+#define M_PI  3.14159265358979323846
+
+HSCDTD008A  compass(PB_7, PB_6, PA_8);
+Watchdog&   watchdog = Watchdog::get_instance();
+
+int main()
+{
+    float   x_sum;
+    float   y_sum;
+    float   z_sum;
+    float   x_avg;
+    float   y_avg;
+    float   z_avg;
+    float   bearing;
+
+    printf("Starting selftest.. ");
+    if (compass.selftest() == OK) {
+        printf("passed\r\n");
+    }
+    else {
+        printf("failed\r\n");
+    }
+
+    printf("Calibrating offsets.. ");
+    compass.calibrateOffsets();
+
+    printf("done\r\n");
+
+    printf("Compensating temperature.. ");
+    compass.compensateTemp();
+    printf("done\r\n");
+
+    ThisThread::sleep_for(1000ms);
+
+    compass.setResolution(true);    // set 15bit resolution
+    printf("res = %d\r\n", compass.getResolution());
+
+    compass.enableFifo();
+    compass.normalMode(0b10);
+
+    watchdog.start(3000);  // clears WDTOF
+
+    while (true) {
+        printf("-----------------\r\n");
+        printf("Measuring.. \r\n");
+
+        if (compass.isFifoFull()) {
+           watchdog.kick();
+
+            //printf("FIFO pointer = %d\r\n", compass.fifoPointer());
+            //printf("FIFO full = %d\r\n", compass.isFifoFull());
+
+           // get data and calculate sums
+            x_sum = 0;
+            y_sum = 0;
+            z_sum = 0;
+
+            for (uint8_t i = 0; i < 8; i++) {
+                compass.readData();
+                x_sum += compass.x();
+                y_sum += compass.y();
+                z_sum += compass.z();
+            }
+
+            // calculate averages
+            x_avg = x_sum / 8;
+            y_avg = y_sum / 8;
+            z_avg = z_sum / 8;
+
+            //printf("FIFO pointer = %d\r\n", compass.fifoPointer());
+            //printf("FIFO full = %d\r\n", compass.isFifoFull());
+            //printf("\r\n");
+
+            printf("x_avg = %f\r\n", x_avg);
+            printf("y_avg = %f\r\n", y_avg);
+            printf("z_avg = %f\r\n", z_avg);
+            printf("\r\n");
+
+            bearing = atan2(y_avg, x_avg) * 180 / M_PI;
+            if (bearing < 0) {
+                bearing = 360 + bearing;
+            };
+
+            printf("bearing = %f\r\n", bearing);
+        }
+
+        ThisThread::sleep_for(1s);
+    }
+}