clangtidy corrections part 2
This commit is contained in:
@@ -4,56 +4,72 @@
|
||||
* @brief Contains the implementation of the class Navigation
|
||||
* @version 0.1
|
||||
* @date 2022-01-31
|
||||
*
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#include "navigation.h"
|
||||
|
||||
Navigation::Navigation(const SensorData* sensorData, Route* route) {
|
||||
this->sensorData = sensorData;
|
||||
Navigation::Navigation(const SensorData *sensorData, Route *route)
|
||||
: sensorData{sensorData}
|
||||
{
|
||||
this->init(route);
|
||||
}
|
||||
|
||||
void Navigation::init(Route* route) {
|
||||
if (route)
|
||||
void Navigation::init(Route *route)
|
||||
{
|
||||
if (static_cast<bool>(route))
|
||||
{
|
||||
this->route = route;
|
||||
}
|
||||
else
|
||||
{
|
||||
this->route = new Route();
|
||||
}
|
||||
|
||||
Component::loopDelay = Navigation::loopDelay;
|
||||
}
|
||||
|
||||
Navigation::~Navigation() {
|
||||
Navigation::~Navigation()
|
||||
{
|
||||
delete this->route;
|
||||
}
|
||||
|
||||
void Navigation::run() {}
|
||||
|
||||
void Navigation::newRoute() {
|
||||
if (this->route)
|
||||
delete this->route;
|
||||
void Navigation::newRoute()
|
||||
{
|
||||
delete this->route;
|
||||
this->route = new Route();
|
||||
}
|
||||
|
||||
bool Navigation::startNavigation() {
|
||||
Point newTargetPoint = this->route->startRoute();
|
||||
bool Navigation::startNavigation()
|
||||
{
|
||||
const Point newTargetPoint = this->route->startRoute();
|
||||
this->navigationStarted = this->setTargetPoint(newTargetPoint);
|
||||
if (this->navigationStarted)
|
||||
{
|
||||
this->navigationFinished = false;
|
||||
}
|
||||
return this->navigationStarted;
|
||||
}
|
||||
|
||||
Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) {
|
||||
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)
|
||||
&& !forceUpdate) {
|
||||
// Check if the Rover has moved, if the Rover hasnt moved this function will be return.
|
||||
if (this->currentPosition.distanceTo(this->lastPointCalcCorrection) < (this->minDistanceToReachPoint) / 2 && !forceUpdate)
|
||||
{
|
||||
correction.correction = this->calculateCourseCorrection(this->lastPointCalcCorrection);
|
||||
correction.distance = this->lastPointCalcCorrection.distanceTo(this->targetPoint);
|
||||
return Status::Unchanged;
|
||||
@@ -62,8 +78,10 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction,
|
||||
double distance = this->currentPosition.distanceTo(this->targetPoint);
|
||||
|
||||
// Check if I need a new Point
|
||||
if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false) {
|
||||
if (!this->nextPoint()) {
|
||||
if (distance < this->minDistanceToReachPoint && this->preventNextPoint == false)
|
||||
{
|
||||
if (!this->nextPoint())
|
||||
{
|
||||
this->navigationFinished = true;
|
||||
this->navigationStarted = false;
|
||||
return Status::Complete; // End of navigation
|
||||
@@ -77,21 +95,25 @@ Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction,
|
||||
return Status::Updated;
|
||||
}
|
||||
|
||||
Navigation::Status Navigation::addCurrentPosToRoute() {
|
||||
Navigation::Status Navigation::addCurrentPosToRoute()
|
||||
{
|
||||
if (this->currentPosition.getAccuracy() <= this->minAccuracy)
|
||||
{
|
||||
return Status::InsufficientAccuracy;
|
||||
}
|
||||
|
||||
// First Point
|
||||
if (this->route->getRouteInfo().totalPoints == 0) {
|
||||
if (this->route->getRouteInfo().totalPoints == 0)
|
||||
{
|
||||
this->route->addPointToRoute(this->currentPosition);
|
||||
this->lastPointRouteInsert = this->currentPosition;
|
||||
return Status::Updated;
|
||||
}
|
||||
|
||||
// Every Point after the first
|
||||
double distance = this->currentPosition.distanceTo(this->lastPointRouteInsert);
|
||||
if (Navigation::minDisBetweenPoints <= distance
|
||||
&& Navigation::maxDisBetweenPoints >= distance){
|
||||
const double distance = this->currentPosition.distanceTo(this->lastPointRouteInsert);
|
||||
if (Navigation::minDisBetweenPoints <= distance && Navigation::maxDisBetweenPoints >= distance)
|
||||
{
|
||||
this->route->addPointToRoute(this->currentPosition);
|
||||
this->lastPointRouteInsert = this->currentPosition;
|
||||
return Status::Updated;
|
||||
@@ -99,57 +121,58 @@ Navigation::Status Navigation::addCurrentPosToRoute() {
|
||||
return Status::Unchanged;
|
||||
}
|
||||
|
||||
// void Navigation::updateCurrentLocation() {
|
||||
// if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime)
|
||||
// return;
|
||||
int16_t Navigation::calculateCourseCorrection(Point &point)
|
||||
{
|
||||
const int16_t targetCourse = point.courseTo(this->targetPoint);
|
||||
int16_t correctionCourse = 0;
|
||||
|
||||
// this->ubxData = Navigation::ubxDataStatic;
|
||||
// this->ubxUpdateTime = Navigation::ubxUpdateTimeStatic;
|
||||
|
||||
// Point::Coordinates coords;
|
||||
// coords.lat = this->ubxData->lat / 10000000.0;
|
||||
// coords.lon = this->ubxData->lon / 10000000.0;
|
||||
|
||||
// this->currentPosition = Point(coords, this->ubxData->hAcc);
|
||||
// }
|
||||
|
||||
int16_t Navigation::calculateCourseCorrection(Point& point) {
|
||||
int16_t targetCourse = point.courseTo(this->targetPoint);
|
||||
int16_t correctionCourse;
|
||||
|
||||
if (this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Good
|
||||
|| this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Super)
|
||||
if (this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Good || this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Super)
|
||||
{
|
||||
correctionCourse = targetCourse - this->sensorData->getCalcAzimuth();
|
||||
this->lastUsedCalcAzimuth = true;
|
||||
} else {
|
||||
}
|
||||
else
|
||||
{
|
||||
correctionCourse = targetCourse - this->sensorData->getRealAzimuth();
|
||||
this->lastUsedCalcAzimuth = false;
|
||||
}
|
||||
|
||||
|
||||
return Navigation::fixDegree(correctionCourse);
|
||||
}
|
||||
|
||||
bool Navigation::nextPoint() {
|
||||
bool Navigation::nextPoint()
|
||||
{
|
||||
if (!this->navigationStarted)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return this->setTargetPoint(this->route->getNextPoint());
|
||||
}
|
||||
|
||||
bool Navigation::setTargetPoint(Point target) {
|
||||
if (target.isInit()) {
|
||||
bool Navigation::setTargetPoint(Point target)
|
||||
{
|
||||
if (target.isInit())
|
||||
{
|
||||
this->targetPoint = target;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
int16_t Navigation::fixDegree(int16_t degree) {
|
||||
while (degree < -180 || degree > 180) {
|
||||
if (degree > 180)
|
||||
degree -= 360;
|
||||
else if (degree < -180)
|
||||
degree += 360;
|
||||
int16_t Navigation::fixDegree(int16_t degree)
|
||||
{
|
||||
static constexpr uint16_t fullCircle = 360;
|
||||
|
||||
while (degree < -fullCircle / 2 || degree > fullCircle / 2)
|
||||
{
|
||||
if (degree > fullCircle / 2)
|
||||
{
|
||||
degree -= fullCircle;
|
||||
}
|
||||
else if (degree < -fullCircle / 2)
|
||||
{
|
||||
degree += fullCircle;
|
||||
}
|
||||
}
|
||||
return degree;
|
||||
}
|
||||
|
||||
+112
-111
@@ -4,9 +4,9 @@
|
||||
* @brief Contains a class which navigate an object by the given route
|
||||
* @version 0.1
|
||||
* @date 2022-01-10
|
||||
*
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATION_H
|
||||
@@ -21,143 +21,144 @@
|
||||
|
||||
/**
|
||||
* @brief This struct inherits the result of the navigation
|
||||
*
|
||||
*
|
||||
* The drive get objects of this struct and should
|
||||
* correct the direction in dependency on this.
|
||||
*
|
||||
*
|
||||
*/
|
||||
struct CourseCorrection {
|
||||
struct CourseCorrection
|
||||
{
|
||||
int16_t correction;
|
||||
double distance;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief This class navigate an object
|
||||
*
|
||||
*
|
||||
* The class use the given Route and the gps device
|
||||
* to tell the driver in which direction he have to
|
||||
* be drive and the distance to the next checkpoint.
|
||||
*
|
||||
*
|
||||
*/
|
||||
class Navigation : public Component {
|
||||
public:
|
||||
enum Status {
|
||||
InsufficientAccuracy,
|
||||
Unchanged,
|
||||
Updated,
|
||||
Complete
|
||||
};
|
||||
class Navigation : public Component
|
||||
{
|
||||
public:
|
||||
enum Status
|
||||
{
|
||||
InsufficientAccuracy,
|
||||
Unchanged,
|
||||
Updated,
|
||||
Complete
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Construct a new Navigation object and using I2C
|
||||
*
|
||||
* @param route with which to navigate
|
||||
*/
|
||||
Navigation(const SensorData* sensorData, Route* route = nullptr);
|
||||
/**
|
||||
* @brief Construct a new Navigation object and using I2C
|
||||
*
|
||||
* @param route with which to navigate
|
||||
*/
|
||||
Navigation(const SensorData *sensorData, Route *route = nullptr);
|
||||
|
||||
/**
|
||||
* @brief Destroy the Navigation object
|
||||
*
|
||||
*/
|
||||
~Navigation();
|
||||
/**
|
||||
* @brief Destroy the Navigation object
|
||||
*
|
||||
*/
|
||||
~Navigation();
|
||||
|
||||
/**
|
||||
* @brief creates a new empty route
|
||||
*
|
||||
*/
|
||||
void newRoute();
|
||||
/**
|
||||
* @brief creates a new empty route
|
||||
*
|
||||
*/
|
||||
void newRoute();
|
||||
|
||||
/**
|
||||
* @brief Tries to start the route
|
||||
*
|
||||
* For example the route can not be started
|
||||
* if there are no Points or wrong Points.
|
||||
*
|
||||
* @return true route is started
|
||||
* @return false route can not be started
|
||||
*/
|
||||
bool startNavigation();
|
||||
void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
|
||||
|
||||
|
||||
double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
|
||||
double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
|
||||
/**
|
||||
* @brief Tries to start the route
|
||||
*
|
||||
* For example the route can not be started
|
||||
* if there are no Points or wrong Points.
|
||||
*
|
||||
* @return true route is started
|
||||
* @return false route can not be started
|
||||
*/
|
||||
bool startNavigation();
|
||||
void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
|
||||
|
||||
// TODO: Dokumentation korrigieren.
|
||||
/**
|
||||
* @brief Get the Course Correction object
|
||||
*
|
||||
* This should be called by the driver to get new instructions.
|
||||
*
|
||||
* @param correction passed as refernce to get the data
|
||||
* @return true if new correction data provided
|
||||
* @return false if route is finished
|
||||
*/
|
||||
Status getCourseCorrection(CourseCorrection& correction, bool forceUpdate = false);
|
||||
double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
|
||||
double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
|
||||
|
||||
// TODO: Dokumentation korrigieren.
|
||||
/**
|
||||
* @brief Get the Course Correction object
|
||||
*
|
||||
* This should be called by the driver to get new instructions.
|
||||
*
|
||||
* @param correction passed as refernce to get the data
|
||||
* @return true if new correction data provided
|
||||
* @return false if route is finished
|
||||
*/
|
||||
Status getCourseCorrection(CourseCorrection &correction, bool forceUpdate = false);
|
||||
|
||||
// TODO: Dokumentation korrigieren.
|
||||
/**
|
||||
* @brief Tries to add the current Position to the route
|
||||
*
|
||||
* This can be go wrong if there is no valid GPS signal
|
||||
*
|
||||
* @return true successful added point
|
||||
* @return false no point added to route
|
||||
*/
|
||||
Status addCurrentPosToRoute();
|
||||
// TODO: Dokumentation korrigieren.
|
||||
/**
|
||||
* @brief Tries to add the current Position to the route
|
||||
*
|
||||
* This can be go wrong if there is no valid GPS signal
|
||||
*
|
||||
* @return true successful added point
|
||||
* @return false no point added to route
|
||||
*/
|
||||
Status addCurrentPosToRoute();
|
||||
|
||||
/**
|
||||
* @brief Get the Route Info object
|
||||
*
|
||||
* This object contains information about the route.
|
||||
* For example the stored points.
|
||||
*
|
||||
* @return RouteInfo
|
||||
*/
|
||||
RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }
|
||||
Route* getRoute() const { return this->route; }
|
||||
Point getCurrentPosition() const { return this->currentPosition; }
|
||||
/**
|
||||
* @brief Get the Route Info object
|
||||
*
|
||||
* This object contains information about the route.
|
||||
* For example the stored points.
|
||||
*
|
||||
* @return RouteInfo
|
||||
*/
|
||||
RouteInfo getRouteInfo() const { return this->route->getRouteInfo(); }
|
||||
Route *getRoute() const { return this->route; }
|
||||
Point getCurrentPosition() const { return this->currentPosition; }
|
||||
|
||||
Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
|
||||
void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
|
||||
Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
|
||||
void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
|
||||
|
||||
// map input in range from -180 to 180 degree
|
||||
static int16_t fixDegree(int16_t degree);
|
||||
// map input in range from -180 to 180 degree
|
||||
static int16_t fixDegree(int16_t degree);
|
||||
|
||||
private:
|
||||
void run() override;
|
||||
void init(Route* route);
|
||||
bool nextPoint();
|
||||
bool setTargetPoint(Point target);
|
||||
int16_t calculateCourseCorrection(Point& point);
|
||||
private:
|
||||
void run() override;
|
||||
void init(Route *route);
|
||||
bool nextPoint();
|
||||
bool setTargetPoint(Point target);
|
||||
int16_t calculateCourseCorrection(Point &point);
|
||||
|
||||
static constexpr uint8_t loopDelay = 20;
|
||||
static constexpr uint8_t maxDisBetweenPoints = 10;
|
||||
static constexpr float minDisBetweenPoints = 0.3;
|
||||
|
||||
const SensorData* sensorData;
|
||||
Route* route = nullptr;
|
||||
static constexpr uint8_t loopDelay = 20;
|
||||
static constexpr uint8_t maxDisBetweenPoints = 10;
|
||||
static constexpr float minDisBetweenPoints = 0.3;
|
||||
|
||||
Point lastPointRouteInsert;
|
||||
Point lastPointCalcCorrection;
|
||||
Point lastPointDrivingDirectionChange;
|
||||
Point targetPoint;
|
||||
Point currentPosition;
|
||||
Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
|
||||
|
||||
bool navigationStarted = false;
|
||||
bool navigationFinished = false;
|
||||
bool isNtripInit = false;
|
||||
bool preventNextPoint = false;
|
||||
bool directionChangeMode = false;
|
||||
bool lastUsedCalcAzimuth = false;
|
||||
const SensorData *sensorData;
|
||||
Route *route = nullptr;
|
||||
|
||||
uint8_t timeToWait = 200;
|
||||
uint32_t lastMillis = 0;
|
||||
uint32_t ubxUpdateTime = 0;
|
||||
Point lastPointRouteInsert;
|
||||
Point lastPointCalcCorrection;
|
||||
Point lastPointDrivingDirectionChange;
|
||||
Point targetPoint;
|
||||
Point currentPosition;
|
||||
Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
|
||||
|
||||
double minDistanceToReachPoint = 0.5;
|
||||
bool navigationStarted = false;
|
||||
bool navigationFinished = false;
|
||||
bool isNtripInit = false;
|
||||
bool preventNextPoint = false;
|
||||
bool directionChangeMode = false;
|
||||
bool lastUsedCalcAzimuth = false;
|
||||
|
||||
uint8_t timeToWait = 200;
|
||||
uint32_t lastMillis = 0;
|
||||
uint32_t ubxUpdateTime = 0;
|
||||
|
||||
double minDistanceToReachPoint = 0.5;
|
||||
};
|
||||
|
||||
#endif // NAVIGATION_H
|
||||
|
||||
Reference in New Issue
Block a user