135 lines
3.7 KiB
C++
135 lines
3.7 KiB
C++
/**
|
|
* @file autopilot.h
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief Contains a class which use the navigate class to drive automaticaly
|
|
* @version 0.1
|
|
* @date 2022-02-02
|
|
*
|
|
* @copyright Copyright (c) 2022
|
|
*
|
|
*/
|
|
|
|
#ifndef AUTOPILOT_H
|
|
#define AUTOPILOT_H
|
|
|
|
|
|
#include "navigation.h"
|
|
#include "moveControl.h"
|
|
#include "driveModi/Modi/Autopilot/autopilotConfig.h"
|
|
#include "driveModi/Modi/ManualControl/manualControl.h"
|
|
|
|
/**
|
|
* @brief This class use the navigate class to drive automaticaly
|
|
*
|
|
* This class get the information from the navigate class. When an
|
|
* object of this class is constructed the rover can be driven manually.
|
|
* When the Rover is near to the first position of the Route, you can
|
|
* switch to automatic drive.
|
|
*
|
|
*/
|
|
class Autopilot : public ManualControl {
|
|
public:
|
|
enum State {
|
|
NoNtrip = -2,
|
|
NoRoute = -1,
|
|
None = 0,
|
|
NavigationStarted,
|
|
GetToStartPoint,
|
|
SelfDrivingAvailable,
|
|
SelfDriving,
|
|
TargetReached
|
|
};
|
|
|
|
/**
|
|
* @brief Construct a new Autopilot object
|
|
*
|
|
* @param moveControl for ManualControl
|
|
* @param navigation for route instructions
|
|
*/
|
|
Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation);
|
|
|
|
/**
|
|
* @brief Destroy the Autopilot object
|
|
*
|
|
* Disconnect the NTRIP-Client
|
|
*/
|
|
~Autopilot();
|
|
|
|
/**
|
|
* @brief Calls runAutopilot or ManualControl loop
|
|
*
|
|
* This function should be called every main loop. If self
|
|
* driving is activated the function call run Autopilot. If
|
|
* the loopDelayMillis is not reached the function returns immediately.
|
|
*
|
|
* If self driving is not activated this functions calls the
|
|
* ManualControl loop additionally.
|
|
*/
|
|
void loop();
|
|
|
|
/**
|
|
* @brief Manges the autopilot
|
|
*
|
|
* If the first Point is near to current location you can turn
|
|
* the autopilot on.
|
|
* Gets the course correction and decide what to do.
|
|
*/
|
|
void runAutopilot();
|
|
|
|
void restart();
|
|
|
|
/**
|
|
* @brief Get the Route Info object
|
|
*
|
|
* @return RouteInfo
|
|
*/
|
|
RouteInfo getRouteInfo() const { return this->routeInfo; }
|
|
|
|
/**
|
|
* @brief Get the Course Correction object
|
|
*
|
|
* @return CourseCorrection
|
|
*/
|
|
CourseCorrection getCourseCorrection() const { return this->courseCorrection; }
|
|
|
|
/**
|
|
* @brief Get the State object
|
|
*
|
|
* @return State
|
|
*/
|
|
State getState() const { return this->state; }
|
|
|
|
/**
|
|
* @brief Tells if there are new informations to display
|
|
*
|
|
* @return true
|
|
* @return false
|
|
*/
|
|
bool shouldUpdate();
|
|
|
|
private:
|
|
void init();
|
|
void checkNtrip();
|
|
void setSpeedInRelToDistance(double distance);
|
|
void setRotInRelToDistance(int16_t course);
|
|
|
|
Navigation* navigation;
|
|
CourseCorrection courseCorrection;
|
|
RouteInfo routeInfo;
|
|
State state = State::None;
|
|
NTRIPClientStates lastState;
|
|
|
|
bool updateDisplay = false;
|
|
|
|
uint8_t loopDelayMillis = 40;
|
|
uint8_t ntripReconnectAttemps = 0;
|
|
uint8_t ntripReconnectMaxAttemps = 10;
|
|
uint16_t displayUpdateDelayMillis = 1000;
|
|
uint16_t reconnectNtripDelayMillis = 1000;
|
|
uint32_t displayUpdateLastMillis = 0;
|
|
uint32_t loopLastMillis = 0;
|
|
uint32_t reconnectNtripLastMillis = 0;
|
|
};
|
|
|
|
#endif // AUTOPILOT_H
|