From 38c8a58988d1eb7a1d604579af47aeeb49f933b4 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Wed, 31 May 2023 12:48:41 +0200 Subject: [PATCH] - 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 | 19 ++- lib/Navigation/navigation.h | 4 +- lib/calibrateCompass/calibrateCompass.cpp | 29 +++- lib/calibrateCompass/calibrateCompass.h | 6 +- .../CalibrateCompass/menuCalibrateCompass.cpp | 155 ++++++++++++++++++ .../CalibrateCompass/menuCalibrateCompass.h | 51 ++++++ .../driveModi/ManualDrive/menuManualDrive.cpp | 55 +------ src/driveModi/Modi/Autopilot/autopilot.cpp | 2 +- .../Modi/ManualControl/manualControl.cpp | 2 +- src/main.cpp | 11 +- 10 files changed, 263 insertions(+), 71 deletions(-) create mode 100644 src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp create mode 100644 src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index 903261e..cdae8df 100644 --- a/lib/Navigation/navigation.cpp +++ b/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); - // caliCompass.loadData(); - // caliCompass.useData(); + CalibrateCompass caliCompass(this->compass); + caliCompass.loadData(); + 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 targetCourse = this->currentPosition.courseTo(this->targetPoint); +int16_t Navigation::calculateCourseCorrection(Point& point) { + 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; diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index 17866af..56f4366 100644 --- a/lib/Navigation/navigation.h +++ b/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 diff --git a/lib/calibrateCompass/calibrateCompass.cpp b/lib/calibrateCompass/calibrateCompass.cpp index a90f453..4d7a1c2 100644 --- a/lib/calibrateCompass/calibrateCompass.cpp +++ b/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() { - this->reset(); +void CalibrateCompass::removeCalibration() { 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) { diff --git a/lib/calibrateCompass/calibrateCompass.h b/lib/calibrateCompass/calibrateCompass.h index 4f726fe..8a8d786 100644 --- a/lib/calibrateCompass/calibrateCompass.h +++ b/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; }; diff --git a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp new file mode 100644 index 0000000..82c18f1 --- /dev/null +++ b/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; + } +} diff --git a/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h b/src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h new file mode 100644 index 0000000..f17438b --- /dev/null +++ b/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 diff --git a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp index 8d7e75a..dffebd8 100644 --- a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp +++ b/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 { @@ -61,32 +59,10 @@ void MenuManualControl::printPage() const { lineOne = "Dutycycle Right:"; lineTwo.concat(this->manualControl->getDutycycleRight()); break; - + case 7: - 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 8: - lineOne = "Save compass"; - lineTwo = "data in flash"; + lineOne = "Azimuth:"; + lineTwo.concat(this->driveManager->getNavigation()->getAzimuth()); 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; } @@ -123,27 +99,6 @@ void MenuManualControl::runCommand() const { case 4: 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; diff --git a/src/driveModi/Modi/Autopilot/autopilot.cpp b/src/driveModi/Modi/Autopilot/autopilot.cpp index 62b926d..46efabe 100644 --- a/src/driveModi/Modi/Autopilot/autopilot.cpp +++ b/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; diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp index f34e603..385f06b 100644 --- a/src/driveModi/Modi/ManualControl/manualControl.cpp +++ b/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) { diff --git a/src/main.cpp b/src/main.cpp index 3f9ca94..a02a11b 100644 --- a/src/main.cpp +++ b/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); - mode_m->addEntry(captureRoute_e); + MenuAction* caliComp_e = new MenuAction("Gauge Compass", comp_m); 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);