refactore

lib/CalcAzimuth/calcAzimuth.cpp

@@ -0,0 +1,92 @@
+/**
+ * @file calcAzimuth.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2023-09-03
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+
+#include "calcAzimuth.h"
+
+CalcAzimuth::CalcAzimuth(Point point) {
+    this->lastChangePoint = point;
+    this->currentPosition = point;
+    this->loopDelay = 50;
+}
+
+void CalcAzimuth::drivingDirectionChange(Point point) {
+    if (point.isInit() && point.isValid()) {
+        this->directionChangeMode = true;
+        this->lastChangePoint = point;
+        this->state = CalcAzimuthState::Invalid;
+    }
+}
+
+void CalcAzimuth::updateCurrentPosition(Point point) {
+    this->currentPosition = point;
+    this->positionChanged = true;
+}
+
+void CalcAzimuth::CalcAzimuth::run() {
+    if (!this->positionChanged)
+        return
+
+    this->positionChanged = false;
+    this->updateAzimuth();
+}
+
+void CalcAzimuth::updateAzimuth() {
+    if (!this->directionChangeMode
+        || this->lastChangePoint.distanceTo(this->currentPosition) < 1.0) 
+    {
+        this->state = CalcAzimuthState::Invalid;
+        this->calcAzimuth = 999;
+        return;
+    }
+
+    this->calcAzimuth = this->lastChangePoint.courseTo(this->currentPosition);
+
+    // Map point accuracy to CalcAzimuthState
+    if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM) 
+    {
+        this->state = CalcAzimuthState::Good;
+    } 
+    else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM) 
+    {
+        this->state = CalcAzimuthState::Ok;
+    } 
+    else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM
+        || this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM) 
+    {
+        this->state = CalcAzimuthState::Bad;
+    } 
+    else 
+    {
+        this->state = CalcAzimuthState::Invalid;
+    }
+
+    // Upgrade quality if the range grows up
+    if (this->lastChangePoint.distanceTo(this->currentPosition) > 2.0) {
+        switch (this->state) {
+            case CalcAzimuthState::Bad :
+                this->state = CalcAzimuthState::Ok;
+                break;
+            
+            case CalcAzimuthState::Ok :
+                this->state = CalcAzimuthState::Good;
+                break;
+
+            case CalcAzimuthState::Good :
+                this->state = CalcAzimuthState::Super;
+                break;
+
+            default:
+                break;
+        }
+    }
+}