164 lines
4.0 KiB
C++
164 lines
4.0 KiB
C++
/**
|
|
* @file navigation.h
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @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
|
|
#define NAVIGATION_H
|
|
|
|
#include <Arduino.h>
|
|
#include <iostream>
|
|
|
|
#include "route.h"
|
|
#include "sensorData.h"
|
|
#include "component.h"
|
|
|
|
/**
|
|
* @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
|
|
{
|
|
int16_t correction;
|
|
double distance;
|
|
};
|
|
|
|
/**
|
|
* @brief This class navigate an object
|
|
*
|
|
* The class use the given Route and the gnss 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
|
|
};
|
|
|
|
/**
|
|
* @brief Construct a new Navigation object
|
|
*
|
|
* @param sensorData
|
|
* @param route
|
|
*/
|
|
Navigation(const SensorData *sensorData, Route *route = nullptr);
|
|
|
|
/**
|
|
* @brief Destroy the Navigation object
|
|
*
|
|
*/
|
|
~Navigation();
|
|
|
|
/**
|
|
* @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; }
|
|
|
|
// TODO: Dokumentation korrigieren.
|
|
/**
|
|
* @brief Get the Course Correction object
|
|
*
|
|
* @param correction
|
|
* @param forceUpdate
|
|
* @return Status
|
|
*/
|
|
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();
|
|
|
|
/**
|
|
* @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; }
|
|
|
|
// 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);
|
|
|
|
static constexpr uint8_t loopDelay = 20;
|
|
static constexpr uint8_t maxDisBetweenPoints = 10;
|
|
static constexpr float minDisBetweenPoints = 0.3;
|
|
|
|
const SensorData *sensorData;
|
|
Route *route = nullptr;
|
|
|
|
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;
|
|
|
|
uint8_t timeToWait = 200;
|
|
uint32_t lastMillis = 0;
|
|
uint32_t ubxUpdateTime = 0;
|
|
|
|
double minDistanceToReachPoint = 0.5;
|
|
};
|
|
|
|
#endif // NAVIGATION_H
|