kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

- added data validation in calibrate compass

Browse Source 
- activatre auto load calibration data
- give possibility to force update in navigation getCourseCorrection
- added drive mode for compass calibration
- removed compass calibration manualControl
This commit is contained in:
kleiax
2023-05-31 12:48:41 +02:00
parent e0d6853961
commit 38c8a58988
10 changed files with 263 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+11 -8
View File
@@ -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;
lib/Navigation/navigation.h
+2 -2
View File
@@ -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
+25 -4
View File
@@ -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) {
lib/calibrateCompass/calibrateCompass.h
+4 -2
View File
@@ -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
+155
View File
@@ -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
+51
View File
@@ -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
+5 -50
View File
@@ -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;
src/driveModi/Modi/Autopilot/autopilot.cpp
+1 -1
View File
@@ -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
+1 -1
View File
@@ -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
+8 -3
View File
@@ -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);