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;
+        }
+    }
+}

lib/CalcAzimuth/calcAzimuth.h

@@ -0,0 +1,50 @@
+/**
+ * @file calcAzimuth.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2023-09-03
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+
+#ifndef CALC_AZIMUTH_H
+#define CALC_AZIMUTH_H
+
+#include "component.h"
+#include "point.h"
+
+class CalcAzimuth : public Component {
+    public:
+        enum CalcAzimuthState {
+            Invalid,
+            Bad,
+            Ok,
+            Good,
+            Super
+        };
+
+        CalcAzimuth(Point point);
+
+        void drivingDirectionChange(Point point);
+        void updateCurrentPosition(Point point);
+        void disableCalcAzimuth() { this->directionChangeMode = false; }
+
+        int16_t getAzimuth() const { return this->calcAzimuth; }
+        CalcAzimuthState getState() const { return this->state; }
+
+    private:
+        void run() override;
+        void updateAzimuth();
+
+        CalcAzimuthState state = CalcAzimuthState::Invalid;
+        Point lastChangePoint;
+        Point currentPosition;
+
+        bool positionChanged = false;
+        bool directionChangeMode = false;
+        int16_t calcAzimuth = INT16_MAX;
+};
+
+#endif //CALC_AZIMUTH_H