- added data validation in calibrate compass

- activatre auto load calibration data
- give possibility to force update in navigation getCourseCorrection
- added drive mode for compass calibration
- removed compass calibration manualControl

lib/Navigation/navigation.cpp

@@ -64,9 +64,9 @@ void Navigation::init(Route* route) {
     Wire.write(0x01);
     Wire.endTransmission();
     this->compass->setMode(0x01,0x0C,0x10,0X00);
-    // CalibrateCompass caliCompass(this->compass);
+    CalibrateCompass caliCompass(this->compass);
-    // caliCompass.loadData();
+    caliCompass.loadData();
-    // caliCompass.useData();
+    caliCompass.useData();
     // std::cout << "Navigation::init compass correction data: " << caliCompass << std::endl;
 }
 
@@ -124,15 +124,18 @@ bool Navigation::startNavigation() {
     return this->navigationStarted;
 }
 
-Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction) {
+Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) {
     if (this->navigationFinished)
         return Status::Complete;
 
     if (this->currentPosition.getAccuracy() <= this->minAccuracy)
         return Status::InsufficientAccuracy;
 
-    if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0))
+    if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0)
+        && !forceUpdate) {
+        correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
         return Status::Unchanged;
+    }
 
     double distance = this->currentPosition.distanceTo(this->targetPoint);
 
@@ -146,7 +149,7 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction)
         distance = this->currentPosition.distanceTo(this->targetPoint);
     }
 
-    correction.correction = this->calculateCourseCorrection();
+    correction.correction = this->calculateCourseCorrection(this->currentPosition);
     correction.distance = distance;
     this->lastPointCalcCorrection = this->currentPosition;
     return Status::Updated;
@@ -186,8 +189,8 @@ void Navigation::updateCurrentLocation() {
     this->currentPosition = Point(coords, this->ubxData->hAcc);
 }
 
-int16_t Navigation::calculateCourseCorrection() {
+int16_t Navigation::calculateCourseCorrection(Point& point) {
-    int16_t targetCourse = this->currentPosition.courseTo(this->targetPoint);
+    int16_t targetCourse = point.courseTo(this->targetPoint);
     // correction = targetCourse - currentCourse
     int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
     int16_t correctionCourse = targetCourse - signedAzimuth;

lib/Navigation/navigation.h

@@ -131,7 +131,7 @@ class Navigation {
          * @return true if new correction data provided
          * @return false if route is finished 
          */
-        Status getCourseCorrection(CourseCorrection& correction);
+        Status getCourseCorrection(CourseCorrection& correction, bool forceUpdate = false);
 
         /**
          * @brief Tries to add the current Position to the route
@@ -197,7 +197,7 @@ class Navigation {
 
     private:
         void updateCurrentLocation();
-        int16_t calculateCourseCorrection();
+        int16_t calculateCourseCorrection(Point& point);
 
         /**
          * @brief Set the next point as target

lib/calibrateCompass/calibrateCompass.cpp

@@ -63,7 +63,7 @@ void CalibrateCompass::loop() {
 
     if (millis() - this->lastChange > this->maxTimeWithoutChange) {
         this->state = State::Finished;
-        this->newData = true;
+        this->checkDataValidity();
     }
 }
 
@@ -78,6 +78,11 @@ void CalibrateCompass::start() {
 }
 
 void CalibrateCompass::useData() {
+    if (!this->dataValid) {
+        std::cout << "CalibrateCompass::useData - Data not valid" << std::endl;
+        return;
+    }
+
     this->compass->setCalibration(  this->data.data[0][0],
                                     this->data.data[0][1],
                                     this->data.data[1][0],
@@ -89,8 +94,7 @@ void CalibrateCompass::useData() {
     std::cout << "CalibrateCompass::useData " << *this << std::endl;                                    
 }
 
-void CalibrateCompass::resetData() {
+void CalibrateCompass::removeCalibration() {
-    this->reset();
     this->compass->removeCalibration();
 }
 
@@ -100,7 +104,7 @@ void CalibrateCompass::reset() {
 }
 
 void CalibrateCompass::saveData() {
-    if (!this->newData)
+    if (!this->dataValid)
         return;
 
     Preferences preferences;
@@ -128,6 +132,7 @@ void CalibrateCompass::loadData() {
     this->data.data[2][1] = preferences.getInt("zHigh", 0);
 
     preferences.end();
+    this->checkDataValidity();
 }
 
 void CalibrateCompass::clearData() {
@@ -135,6 +140,22 @@ void CalibrateCompass::clearData() {
         this->data.data[i][0] = 0;
         this->data.data[i][1] = 0;
     }
+    this->dataValid = false;
+}
+
+void CalibrateCompass::checkDataValidity() {
+    int sum = 0;
+    for (uint8_t i = 0; i < 3; i++) {
+        if (this->data.data[i][0] > INT16_MAX || this->data.data[i][0] < INT16_MIN
+            || this->data.data[i][1] > INT16_MAX || this->data.data[i][1] < INT16_MIN) 
+            {
+            this->dataValid = false;
+            return;
+        }
+        sum += this->data.data[i][0];
+        sum += this->data.data[i][1];
+    }
+    this->dataValid = sum;
 }
 
 std::ostream& operator<<(std::ostream& os, const CalibrateCompass& caliComp) {

lib/calibrateCompass/calibrateCompass.h

@@ -32,7 +32,7 @@ class CalibrateCompass {
         void loop();
         void start();
         void useData();
-        void resetData();
+        void removeCalibration();
         void reset();
         void saveData();
         void loadData();
@@ -43,13 +43,15 @@ class CalibrateCompass {
         friend std::ostream& operator<<(std::ostream& os, const CalibrateCompass& caliComp);
 
     private:
+        void checkDataValidity();
+
         QMC5883LCompass* compass;
         State state;
         CallibrationData data;
 
         void clearData();
 
-        bool newData = false;
+        bool dataValid = false;
         const uint16_t maxTimeWithoutChange = 5000;
         uint32_t lastChange = 0;
 };

src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp

@@ -0,0 +1,155 @@
+/**
+ * @file menuCalibrateCompass.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains an implementation of the class MenuManualDrive
+ * @version 0.1
+ * @date 2022-01-20
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+#include "menuCalibrateCompass.h"
+
+MenuCalibrateCompass::MenuCalibrateCompass(DriveManager* driveManager) : MenuDriveMode(driveManager) {
+    this->caliCompass = new CalibrateCompass(this->driveManager->getNavigation()->getCompass());
+}
+
+MenuCalibrateCompass::~MenuCalibrateCompass() {
+    delete this->caliCompass;
+    this->manualControl->setCalibrateCompass();
+}
+
+void MenuCalibrateCompass::printPage() const {
+    String lineOne = "";
+    String lineTwo = "";
+
+    switch (this->getCurrentPage()) {
+        case 0:
+            lineOne = "-Ready to drive-";
+            lineTwo = "Compass Mode";
+            break;
+
+        case 1:
+            lineOne = "Azimuth:";
+            lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
+            break;        
+        
+        case 2:
+            switch (this->caliCompass->getState()) {
+                case CalibrateCompass::State::Ready :
+                    lineOne = "Start compass";
+                    lineTwo = "calibration";
+                    break;
+
+                case CalibrateCompass::State::Calibrating :
+                    lineOne = "Calibrating...";
+                    lineTwo = "Move around";
+                    break;
+
+                case CalibrateCompass::State::Finished:
+                    lineOne = "Calibration";
+                    lineTwo = "finished";
+                    break;
+                
+                default:
+                    break;
+            }
+            break;
+
+        case 3:
+            lineOne = "Save compass";
+            lineTwo = "data in flash";
+            break;
+
+        case 4:
+            lineOne = "Load data";
+            lineTwo = "from flash";
+            break;
+
+        case 5:
+            lineOne = "Remove";
+            lineTwo = "calibration data";
+            break;
+
+        case 6:
+            lineOne = "Use current";
+            lineTwo = "calibration data";
+            break;
+
+        case 7:
+            lineOne = "X min: ";
+            lineOne.concat(this->caliCompass->getCallibrationData().data[0][0]);
+            lineTwo = "X max: ";
+            lineTwo.concat(this->caliCompass->getCallibrationData().data[0][1]);
+            break;
+
+        case 8:
+            lineOne = "Y min: ";
+            lineOne.concat(this->caliCompass->getCallibrationData().data[1][0]);
+            lineTwo = "Y max: ";
+            lineTwo.concat(this->caliCompass->getCallibrationData().data[1][1]);
+            break;
+
+        case 9:
+            lineOne = "Z min: ";
+            lineOne.concat(this->caliCompass->getCallibrationData().data[2][0]);
+            lineTwo = "Z max: ";
+            lineTwo.concat(this->caliCompass->getCallibrationData().data[2][1]);
+            break;
+
+        default:
+            this->printDefault();
+        return;
+    }
+
+    this->print(lineOne, lineTwo);
+}
+
+void MenuCalibrateCompass::init() {
+    this->firstPrint = false;
+    this->driveManager->changeModus(Modi::ManualControl);
+    this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
+    this->setCountPages(10);
+    this->updateDelay = 500; 
+}
+
+void MenuCalibrateCompass::runCommand() const {
+    switch (this->getCurrentPage()) {
+        case 2:
+            switch (this->caliCompass->getState()) {
+                case CalibrateCompass::State::Ready :
+                    this->manualControl->setCalibrateCompass(this->caliCompass);
+                    this->caliCompass->start();
+                    break;
+
+                case CalibrateCompass::State::Finished:
+                    this->manualControl->setCalibrateCompass();
+                    this->caliCompass->useData();
+                    this->caliCompass->reset();
+                    break;
+                
+                default:
+                    break;
+            }
+            break;
+
+        case 3:
+            this->caliCompass->saveData();
+            break;
+
+        case 4:
+            this->caliCompass->loadData();
+            break;
+
+        case 5:
+            this->caliCompass->removeCalibration();
+            break;
+
+        case 6:
+            this->caliCompass->useData();
+            break;        
+        
+        default:
+            break;
+    }
+}

src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h

@@ -0,0 +1,51 @@
+/**
+ * @file menuCalibrateCompass.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class to print information about the class ManualControl
+ * @version 0.1
+ * @date 2022-01-20
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+
+#ifndef MENU_CALIBRATE_COMPASS_H
+#define MENU_CALIBRATE_COMPASS_H
+
+#include "SpecialMenus/driveModi/menuDriveMode.h"
+#include "calibrateCompass.h"
+
+/**
+ * @brief A class to print informations about the class ManualControl
+ * 
+ */
+class MenuCalibrateCompass : public MenuDriveMode {
+    public:
+        /**
+         * @brief Construct a new Menu Manual Control object
+         * 
+         * @param driveManager 
+         */
+        MenuCalibrateCompass(DriveManager* driveManager);
+        ~MenuCalibrateCompass();
+
+        /**
+    	 * @brief Prints the Information to display and console
+    	 * 
+         * The informations are only printed to the display if it
+         * set.
+         * 
+         * Changes the DriveModi to Autopilot if it is the first time called.
+         */
+        void printPage() const override;
+
+    protected:
+        void init() override;
+        void runCommand() const override;
+
+    private:
+        ManualControl* manualControl;
+        CalibrateCompass* caliCompass;
+};
+
+#endif // MENU_CALIBRATE_COMPASS_H

src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp

@@ -11,12 +11,10 @@
 #include "menuManualDrive.h"
 
 MenuManualControl::MenuManualControl(DriveManager* driveManager) : MenuDriveMode(driveManager) {
-    this->caliCompass = new CalibrateCompass(this->driveManager->getNavigation()->getCompass());
+
 }
 
 MenuManualControl::~MenuManualControl() {
-    delete this->caliCompass;
-    this->manualControl->setCalibrateCompass();
 }
 
 void MenuManualControl::printPage() const {
@@ -63,30 +61,8 @@ void MenuManualControl::printPage() const {
             break;
 
         case 7:
-            switch (this->caliCompass->getState()) {
+            lineOne = "Azimuth:";
-                case CalibrateCompass::State::Ready :
+            lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
-                    lineOne = "Start compass";
-                    lineTwo = "calibration";
-                    break;
-
-                case CalibrateCompass::State::Calibrating :
-                    lineOne = "Calibrating...";
-                    lineTwo = "Move around";
-                    break;
-
-                case CalibrateCompass::State::Finished:
-                    lineOne = "Calibration";
-                    lineTwo = "finished";
-                    break;
-                
-                default:
-                    break;
-            }
-            break;
-
-        case 8:
-            lineOne = "Save compass";
-            lineTwo = "data in flash";
             break;
 
         default:
@@ -101,7 +77,7 @@ void MenuManualControl::init() {
     this->firstPrint = false;
     this->driveManager->changeModus(Modi::ManualControl);
     this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
-    this->setCountPages(9);
+    this->setCountPages(8);
     this->updateDelay = 500; 
 }
 
@@ -124,27 +100,6 @@ void MenuManualControl::runCommand() const {
             this->manualControl->increaseMaxRotation();
             break;
         
-        case 7:
-            switch (this->caliCompass->getState()) {
-                case CalibrateCompass::State::Ready :
-                    this->manualControl->setCalibrateCompass(this->caliCompass);
-                    this->caliCompass->start();
-                    break;
-
-                case CalibrateCompass::State::Finished:
-                    this->manualControl->setCalibrateCompass();
-                    this->caliCompass->useData();
-                    this->caliCompass->reset();
-                    break;
-                
-                default:
-                    break;
-            }
-            break;
-
-        case 8:
-            this->caliCompass->saveData();
-        
         default:
             break;
     }

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -100,7 +100,7 @@ void Autopilot::init() {
         this->routeInfo = this->navigation->getRouteInfo();
         this->navigation->getNTRIPClient()->setAutoReconnect(true);
 
-        this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection);
+        this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
 
         this->courseCorrection.correction = 0;
         this->courseCorrection.distance = 0;

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -23,7 +23,7 @@ ManualControl::~ManualControl() {
 }
 
 void ManualControl::loop() {
-    if (caliCompass)
+    if (this->caliCompass)
         this->caliCompass->loop();
 
     if (millis() - this->lastMillis < delay) {

src/main.cpp

@@ -38,6 +38,7 @@
 #include "SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.h"
 #include "SpecialMenus/driveModi/Autopilot/menuAutopilot.h"
 #include "SpecialMenus/driveModi/TestMode/menuTestMode.h"
+#include "SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h"
 #include "SpecialMenus/Systeminformation/menuSysteminformation.h"
 #include "SpecialMenus/PID/menuPidSettings.h"
 #include "SpecialMenus/Route/menuRoute.h"
@@ -208,6 +209,7 @@ void makeMenu() {
     MenuCaptureRoute* cap_m = new MenuCaptureRoute(driveManager);
     MenuAutopilot* auto_m = new MenuAutopilot(driveManager);
     MenuTestMode* testM_m = new MenuTestMode(driveManager);
+    MenuCalibrateCompass* comp_m = new MenuCalibrateCompass(driveManager);
     MenuGPS* gps_m = new MenuGPS(driveManager);
     MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery);
     MenuRoute* rout_m = new MenuRoute(driveManager->getNavigation()->getRoute());
@@ -222,6 +224,7 @@ void makeMenu() {
     cap_m->setLcd(lcdWrapper);
     auto_m->setLcd(lcdWrapper);
     testM_m->setLcd(lcdWrapper);
+    comp_m->setLcd(lcdWrapper);
     gps_m->setLcd(lcdWrapper);
     gps_m->setUpdateDelay(1000);
     sys_m->setLcd(lcdWrapper);
@@ -250,11 +253,13 @@ void makeMenu() {
     MenuAction* consolControl_e = new MenuAction("Consol Control", dummy);
     MenuAction* manualControl_e = new MenuAction("Manual Control", man_m);
     MenuAction* testMode_e = new MenuAction("Test Mode", testM_m);
-    mode_m->addEntry(autopilot_e);
+    MenuAction* caliComp_e = new MenuAction("Gauge Compass", comp_m);
-    mode_m->addEntry(captureRoute_e);
     mode_m->addEntry(manualControl_e);
-    mode_m->addEntry(consolControl_e);
+    mode_m->addEntry(captureRoute_e);
+    mode_m->addEntry(autopilot_e);
+    mode_m->addEntry(caliComp_e);
     mode_m->addEntry(testMode_e);
+    mode_m->addEntry(consolControl_e);
 
     // Entry for the PID Menu
     MenuAction* pidl_e = new MenuAction("Left", pidl_m);