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

added interface for calced azimuth

Browse Source
This commit is contained in:
kleiax
2023-08-08 18:56:36 +02:00
parent 115da0f8a8
commit 620812b5f8
2 changed files with 88 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+66 -8
View File
@@ -98,14 +98,11 @@ void Navigation::loop() {
this->ntripClient->loop();
if (millis() - this->lastMillis > AZIMUTH_UPDATE_DELAY) {
// if (millis() - this->lastMillis > 500) {
this->compass->read();
this->azimuth = this->compass->getAzimuth();
// std::cout << "Compass x: " << compass->getX()
// << " y: " << compass->getY()
// << " z: " << compass->getZ()
// << " Azi: " << compass->getAzimuth()
// << std::endl;
this->realAzimuth = this->compass->getAzimuth();
this->updateMagneticDeclination();
this->lastMillis = millis();
}
}
@@ -124,6 +121,14 @@ bool Navigation::startNavigation() {
return this->navigationStarted;
}
void Navigation::drivingDirectionChange() {
Point tmp = this->currentPosition;
if (tmp.isInit() && tmp.isValid()) {
this->directionChangeMode = true;
this->lastPointDrivingDirectionChange = tmp;
}
}
Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) {
if (this->navigationFinished)
return Status::Complete;
@@ -190,9 +195,62 @@ void Navigation::updateCurrentLocation() {
this->currentPosition = Point(coords, this->ubxData->hAcc);
}
void Navigation::updateMagneticDeclination() {
if (!this->directionChangeMode
|| this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) < 1.0)
{
this->calcAzimuthState = CalcAzimuthState::Invalid;
return;
}
this->calcAzimuth = this->lastPointDrivingDirectionChange.courseTo(this->currentPosition);
// Map point accuracy to CalcAzimuthState
if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::oneDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM)
{
this->calcAzimuthState = CalcAzimuthState::Good;
}
else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::twoDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM)
{
this->calcAzimuthState = CalcAzimuthState::Ok;
}
else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::threeDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM)
{
this->calcAzimuthState = CalcAzimuthState::Bad;
}
else
{
this->calcAzimuthState = CalcAzimuthState::Invalid;
}
// Upgrade quality if the range grows up
if (this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) > 2.0) {
switch (this->calcAzimuthState) {
case CalcAzimuthState::Bad :
this->calcAzimuthState = CalcAzimuthState::Ok;
break;
case CalcAzimuthState::Ok :
this->calcAzimuthState = CalcAzimuthState::Good;
break;
case CalcAzimuthState::Good :
this->calcAzimuthState = CalcAzimuthState::Super;
break;
default:
break;
}
}
}
int16_t Navigation::calculateCourseCorrection(Point& point) {
int16_t targetCourse = point.courseTo(this->targetPoint);
int16_t correctionCourse = targetCourse - this->azimuth;
int16_t correctionCourse = targetCourse - this->realAzimuth;
if (correctionCourse > 180)
correctionCourse -= 360;
else if (correctionCourse < -180)
lib/Navigation/navigation.h
+22 -22
View File
@@ -62,6 +62,14 @@ class Navigation {
Complete
};
enum CalcAzimuthState {
Invalid,
Bad,
Ok,
Good,
Super
};
/**
* @brief Construct a new Navigation object and using I2C
@@ -118,6 +126,8 @@ class Navigation {
*/
bool startNavigation();
void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
void drivingDirectionChange();
void dissableCalcAzimuth() { this->directionChangeMode = false; }
double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
@@ -175,14 +185,14 @@ class Navigation {
Point getCurrentPosition() const { return this->currentPosition; }
/**
* @brief Get azimuth
* @brief Get realAzimuth
*
* This value represents the angle between north and
* the line of sight. Clockwise.
*
* @return uint16_t degree
*/
int16_t getAzimuth() const { return this->azimuth; }
int16_t getAzimuth() const { return this->realAzimuth; }
QMC5883LCompass* getCompass() const { return this->compass; }
Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
@@ -200,26 +210,11 @@ class Navigation {
private:
void updateCurrentLocation();
int16_t calculateCourseCorrection(Point& point);
/**
* @brief Set the next point as target
*
* @return true
* @return false
*/
bool nextPoint();
/**
* @brief Set the target point
*
* @param target
* @return true
* @return false
*/
bool setTargetPoint(Point target);
void updateMagneticDeclination();
void init(Route* route);
bool nextPoint();
bool setTargetPoint(Point target);
int16_t calculateCourseCorrection(Point& point);
SFE_UBLOX_GNSS* gps;
UBX_NAV_PVT_data_t* ubxData = nullptr;
@@ -229,22 +224,27 @@ class Navigation {
Point lastPointRouteInsert;
Point lastPointCalcCorrection;
Point lastPointDrivingDirectionChange;
Point targetPoint;
Point currentPosition;
Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid;
bool navigationStarted = false;
bool navigationFinished = false;
bool isNtripInit = false;
bool preventNextPoint = false;
bool directionChangeMode = false;
char* host;
char* mountPoint;
char* user;
char* password;
int16_t azimuth = INT16_MAX;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
uint8_t timeToWait = 200;
uint16_t port;