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

Merge branch 'main' of git.kleiax.de:kleiax/Projektarbeit-Rover into main

Browse Source
This commit is contained in:
kleiax
2023-08-14 07:15:26 +02:00
parent 147eea1f92 bad84b1ef3
commit bf4c1dd49f
39 changed files with 763 additions and 315 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+90 -19
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,15 @@ bool Navigation::startNavigation() {
return this->navigationStarted;
}
void Navigation::drivingDirectionChange() {
Point tmp = this->currentPosition;
if (tmp.isInit() && tmp.isValid()) {
this->directionChangeMode = true;
this->lastPointDrivingDirectionChange = tmp;
this->calcAzimuthState = CalcAzimuthState::Invalid;
}
}
Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) {
if (this->navigationFinished)
return Status::Complete;
@@ -134,6 +140,7 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction,
if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint / 2.0)
&& !forceUpdate) {
correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint);
return Status::Unchanged;
}
@@ -189,21 +196,75 @@ 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;
this->calcAzimuth = 999;
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);
// correction = targetCourse - currentCourse
int16_t signedAzimuth = this->azimuth > 180 ? this->azimuth - 360 : this->azimuth;
int16_t correctionCourse = targetCourse - signedAzimuth;
if (correctionCourse > 180)
correctionCourse -= 360;
else if (correctionCourse < -180)
correctionCourse += 360;
int16_t correctionCourse;
//Rotate result by 180° to corrigate Azimuth
correctionCourse += 180;
correctionCourse = correctionCourse > 180 ? correctionCourse -360 : correctionCourse;
return correctionCourse;
if (this->calcAzimuthState == CalcAzimuthState::Good
|| this->calcAzimuthState == CalcAzimuthState::Super)
{
correctionCourse = targetCourse - this->calcAzimuth;
this->lastUsedCalcAzimuth = true;
} else {
correctionCourse = targetCourse - this->realAzimuth;
this->lastUsedCalcAzimuth = false;
}
return Navigation::fixDegree(correctionCourse);
}
bool Navigation::nextPoint() {
@@ -224,6 +285,16 @@ void Navigation::setOutputStatusPrintPVTdata(bool status) {
Navigation::outputStatusPrintPVTdata = status;
}
int16_t Navigation::fixDegree(int16_t degree) {
while (degree < -180 || degree > 180) {
if (degree > 180)
degree -= 360;
else if (degree < -180)
degree += 360;
}
return degree;
}
void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
if (!Navigation::outputStatusPrintPVTdata)
return;
lib/Navigation/navigation.h
+32 -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; }
@@ -178,15 +188,21 @@ 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
*/
uint16_t getAzimuth() const { return this->azimuth; }
int16_t getAzimuth() const { return this->realAzimuth; }
QMC5883LCompass* getCompass() const { return this->compass; }
int16_t getCalcAzimuth() const { return this->calcAzimuth; }
CalcAzimuthState getCalcAzimuthState() const { return this->calcAzimuthState; }
bool getLastUsedCalcAzimuth() const { return this->lastUsedCalcAzimuth; }
Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
/**
* @brief Set the output status for PVTdata.
@@ -197,29 +213,16 @@ class Navigation {
* @param status
*/
static void setOutputStatusPrintPVTdata(bool status);
// map input in range from -180 to 180 degree
static int16_t fixDegree(int16_t degree);
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,24 +232,31 @@ 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;
bool lastUsedCalcAzimuth = false;
char* host;
char* mountPoint;
char* user;
char* password;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
uint8_t timeToWait = 200;
uint16_t port;
uint16_t azimuth = UINT16_MAX;
uint32_t lastMillis = 0;
uint32_t ubxUpdateTime = 0;
lib/Navigation/route.cpp
-181
View File
@@ -1,181 +0,0 @@
/**
* @file route.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Implements the class Route and Point
* @version 0.1
* @date 2022-01-31
*
* @copyright Copyright (c) 2022
*
*/
#include "route.h"
Point::Point(double lat, double lon, uint32_t horizontalAccuracy) {
this->coordinates.lat = lat;
this->coordinates.lon = lon;
this->init(horizontalAccuracy);
}
Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy) {
this->coordinates.lat = lat / 10000000.0;
this->coordinates.lon = lon / 10000000.0;
this->init(horizontalAccuracy);
}
Point::Point(Coordinates coords, uint32_t horizontalAccuracy) {
this->coordinates = coords;
this->init(horizontalAccuracy);
}
Point::Point(Coordinates coords, bool imported) {
this->coordinates = coords;
if (imported)
this->init(UINT32_MAX);
else
this->init(0);
}
Point::Point() {
this->coordinates.lat = 0;
this->coordinates.lon = 0;
this->init(0);
}
bool Point::operator==(const Point& rhs) const {
return this->coordinates == rhs.getCoordinates();
}
// distance = sqrt(dx * dx + dy * dy)
// mit distance: Entfernung in km
// dx = 111.3 * cos(lat) * (lon1 - lon2)
// lat = (lat1 + lat2) / 2 * 0.01745
// dy = 111.3 * (lat1 - lat2)
// lat1, lat2, lon1, lon2: Breite, Länge in Grad
double Point::distanceTo(const Coordinates& point) const {
Coordinates begin = this->coordinates;
Coordinates end = point;
double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
return sqrt(dx * dx + dy * dy);
}
double Point::distanceTo(const Point &point) const {
return this->distanceTo(point.getCoordinates());
}
int16_t Point::courseTo(const Coordinates& point) const {
Coordinates begin = this->coordinates;
Coordinates end = point;
double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + ROUTE_PI / 4) );
double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
int16_t res = (int16_t) atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT;
// if (res < 0)
// res += 360;
return res;
}
int16_t Point::courseTo(const Point &point) const {
return this->courseTo(point.getCoordinates());
}
void Point::init(uint32_t horizontalAccuracy) {
this->creationTime = millis();
if (horizontalAccuracy == UINT32_MAX)
this->accuracy = Accuracy::imported;
else if (horizontalAccuracy > 9999)
this->accuracy = Accuracy::fourDigOfCM;
else if (horizontalAccuracy > 999)
this->accuracy = Accuracy::threeDigOfCM;
else if (horizontalAccuracy > 99)
this->accuracy = Accuracy::twoDigOfCM;
else if (horizontalAccuracy > 1)
this->accuracy = Accuracy::oneDigOfCM;
else
this->accuracy = Accuracy::none;
}
Route::Route() {
}
void Route::addPointToRoute(Point point) {
this->points.push_back(point);
}
void Route::clear() {
this->points.clear();
this->currentPoint = 0;
this->started = false;
}
Point Route::startRoute() {
if (this->points.size() < 1) {
this->started = false;
return Point();
}
this->it = this->points.begin();
this->currentPoint = 1;
this->started = true;
return *this->it;
}
Point Route::endRoute() {
if (this->points.size() < 1) {
this->started = false;
return Point();
}
this->it = this->points.end();
// TODO: Understand why i have to decrement the iterator first to get realy the last element.
this->it--;
this->currentPoint = this->points.size();
this->started = true;
return *this->it;
}
Point Route::getNextPoint() {
if (!this->started)
return Point();
if (this->it != --this->points.end()) {
this->it++;
this->currentPoint++;
return *this->it;
// } else if (this->it == this->points.end() && this->currentPoint != this->points.size()) {
// this->currentPoint++;
// return *this->it;
}
return Point();
}
Point Route::getPreviousPoint() {
if (!this->started)
return Point();
if (this->it != this->points.begin()) {
this->it--;
this->currentPoint--;
return *this->it;
} else {
return Point();
}
}
RouteInfo Route::getRouteInfo() {
RouteInfo info;
info.totalPoints = this->points.size();
info.currentPoint = this->currentPoint;
return info;
}
lib/Navigation/route.h
-240
View File
@@ -1,240 +0,0 @@
/**
* @file route.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains the class Point and Route
* @version 0.1
* @date 2022-01-31
*
* @copyright Copyright (c) 2022
*
*/
#ifndef ROUTE_H
#define ROUTE_H
#include <Arduino.h>
#include <cstdint>
#include <list>
#include <cmath>
#define ROUTE_DEGREE_TO_RADIANT 0.01745
#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300
#define ROUTE_PI 3.14159265358979323846
/**
* @brief A to handle points on the earth
*
* The points inherits latidue and longitude as doubles
*
*/
class Point{
public:
/**
* @brief Hold the data longitude and latitude
*
*/
struct Coordinates {
double lon;
double lat;
bool operator==(const Coordinates rhs) const {
return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon );
}
};
/**
* @brief The Accuracy is set by the constructor
*
*/
enum Accuracy {
none,
fourDigOfCM,
threeDigOfCM,
twoDigOfCM,
oneDigOfCM,
imported
};
/**
* @brief Construct a new Point object
*
* @param lat latitude
* @param lon longitude
* @param horizontalAccuracy mm
* @param coords Coordinates
* @param imported if true than highest accuracy
*/
Point(double lat, double lon, uint32_t horizontalAccuracy = 0);
Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0);
Point(Coordinates coords, uint32_t horizontalAccuracy = 0);
Point(Coordinates coords, bool imported);
Point();
/**
* @brief Checks if to points are equal.
*
* @param rhs
* @return true
* @return false
*/
bool operator==(const Point& rhs) const;
/**
* @brief Checks if the point is initalized.
*
* @return true
* @return false
*/
bool isInit() const { return this->coordinates.lat + this->coordinates.lon; }
/**
* @brief Checks if the point is valid.
*
* If the accuracy is higher than zero, true will be returned.
*
* @return true
* @return false
*/
bool isValid() const { return (this->accuracy > 0) ? true : false; }
/**
* @brief Calculates the distance between to points.
*
* @param point
* @return double meter
*/
double distanceTo(const Coordinates& point) const;
double distanceTo(const Point& point) const;
/**
* @brief Calculates the course to an other point.
*
* @param point
* @return int16_t degree
*/
int16_t courseTo(const Coordinates& point) const;
int16_t courseTo(const Point& point) const;
uint32_t getCreationTime() const { return this->creationTime; }
double getLongitude() const { return this->coordinates.lon; }
double getLatitude() const { return this->coordinates.lat; }
Coordinates getCoordinates() const { return this->coordinates; }
/**
* @brief Get the Accuracy object
*
* The higher the value, the greater the accuracy.
* You can check it by Accuracy.
*
* @return Accuracy
*/
Accuracy getAccuracy() { return this->accuracy; }
private:
void init(uint32_t horizontalAccuracy);
Accuracy accuracy = Accuracy::none;
Coordinates coordinates;
uint32_t creationTime = 0;
};
/**
* @brief Holds some route information
*
*/
struct RouteInfo{
/**
* @brief Selected number of Points
*/
uint16_t currentPoint;
/**
* @brief Total points stored in route
*/
uint16_t totalPoints;
};
/**
* @brief A class to manage multiple points
*
* The list of point presents a route which can be driven
*/
class Route {
public:
/**
* @brief Construct a new Route object.
*/
Route();
/**
* @brief Adds a point to the list.
*
* @param point
*/
void addPointToRoute(Point point);
/**
* @brief Delete all points.
*
*/
void clear();
/**
* @brief Select the first point as target.
*
* @return Point
*/
Point startRoute();
/**
* @brief Select the last point as target.
*
* @return Point
*/
Point endRoute();
/**
* @brief Get the next point and set it as target.
*
* @return Point is zero if there are no more Points.
*/
Point getNextPoint();
/**
* @brief Get the previous point and set it as target.
*
* @return Point
*/
Point getPreviousPoint();
/**
* @brief Get the Route Info object
*
* @return RouteInfo
*/
RouteInfo getRouteInfo();
/**
* @brief Get the Started object
*
* The Route will be marked as started when startRoute or
* endRoute has been called.
*
* @return true
* @return false
*/
bool getStarted() const { return this->started; }
private:
uint16_t currentPoint = 0;
bool started = false;
std::list<Point> points;
std::list<Point>::iterator it;
};
#endif // ROUTE_H