added a class to calibrate the compass

2023-05-23 23:24:38 +02:00
parent 7418adb027
commit 8199b47089
2 changed files with 146 additions and 0 deletions
@@ -0,0 +1,98 @@
+/**
+ * @file calibrateCompass.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2023-05-23
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+
+#include "calibrateCompass.h"
+
+CalibrateCompass::CalibrateCompass(QMC5883LCompass* compass) {
+    this->compass = compass;
+    this->state = State::Ready;
+    this->clearData();
+}
+
+void CalibrateCompass::loop() {
+    if (this->state != State::Calibrating)
+        return;
+
+    bool changed = false;
+
+    this->compass->read();
+    int x = this->compass->getX();
+    int y = this->compass->getY();
+    int z = this->compass->getZ();
+
+    if(x < this->data.data[0][0]) {
+        this->data.data[0][0] = x;
+        changed = true;
+    }
+
+    if(x > this->data.data[0][1]) {
+        this->data.data[0][1] = x;
+        changed = true;
+    }
+
+    if(y < this->data.data[1][0]) {
+        this->data.data[1][0] = y;
+        changed = true;
+    }
+
+    if(y > this->data.data[1][1]) {
+        this->data.data[1][1] = y;
+        changed = true;
+    }
+
+    if(z < this->data.data[2][0]) {
+        this->data.data[2][0] = z;
+        changed = true;
+    }
+
+    if(z > this->data.data[2][1]) {
+        this->data.data[2][1] = z;
+        changed = true;
+    }
+
+    if (changed)
+        this->lastChange = millis();
+
+    if (millis() - this->lastChange > this->maxTimeWithoutChange)
+        this->state = State::Finished;
+}
+
+void CalibrateCompass::start() {
+    this->clearData();
+    this->state = State::Calibrating;
+    this->compass->setCalibration(0, 0, 0, 0, 0, 0);
+    this->lastChange = millis();
+}
+
+void CalibrateCompass::useData() {
+    if (this->state != State::Finished)
+        return;
+    
+    this->compass->setCalibration(  this->data.data[0][0],
+                                    this->data.data[0][1],
+                                    this->data.data[1][0],
+                                    this->data.data[1][1],
+                                    this->data.data[2][0],
+                                    this->data.data[2][1]
+                                    )
+}
+
+void CalibrateCompass::reset() {
+    this->start();
+    this->state = State::Ready;
+}
+
+void CalibrateCompass::clearData() {
+    for (uint8_t i = 0; i < 3; i++) {
+        this->data.data[i][0] = 0;
+        this->data.data[i][1] = 0;
+    }
+}
@@ -0,0 +1,48 @@
+/**
+ * @file calibrateCompass.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2023-05-23
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+
+#pragma once
+
+#include <QMC5883LCompass.h>
+
+class CalibrateCompass {
+    public:
+        enum State {
+            None,
+            Ready,
+            Calibrating,
+            Finished
+        };
+
+        struct CallibrationData {
+            int data[3][2];
+        };
+
+        CalibrateCompass(QMC5883LCompass* compass);
+
+        void loop();
+        void start();
+        void useData();
+        void reset();
+
+        State getState() const { return this->state; }
+        CallibrationData getCallibrationData() const { return this->data; }
+
+    private:
+        QMC5883LCompass* compass;
+        State state = State::None;
+        CallibrationData data;
+
+        void clearData();
+
+        const uint16_t maxTimeWithoutChange = 5000;
+        uint32_t lastChange = 0;
+};