147 lines
3.9 KiB
C++
147 lines
3.9 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/ManualControl/manualControl.h"
|
|
|
|
class DirectionChangeSignal : public DirectionChangeWrapper {
|
|
public:
|
|
DirectionChangeSignal(Navigation* navigation);
|
|
~DirectionChangeSignal();
|
|
void action() override;
|
|
|
|
private:
|
|
Navigation* navigation;
|
|
};
|
|
|
|
/**
|
|
* @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 {
|
|
InsufficientAccuracy = -2,
|
|
NoRoute = -1,
|
|
None = 0,
|
|
NavigationStarted,
|
|
GetToStartPoint,
|
|
SelfDrivingAvailable,
|
|
SelfDriving,
|
|
SelfDrivingRotate,
|
|
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();
|
|
|
|
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();
|
|
void testRotate(int16_t degree);
|
|
void endRotate();
|
|
void switchLoopMode() { this->loopMode = !this->loopMode; }
|
|
bool getLoopMode() const { return this->loopMode; }
|
|
|
|
private:
|
|
void init();
|
|
void drive();
|
|
void beginRotate();
|
|
void rotate();
|
|
void run() override;
|
|
void checkButtonInput();
|
|
void askNavigationForOrder();
|
|
void selfDriving();
|
|
void restartLoop();
|
|
|
|
Navigation* navigation;
|
|
CourseCorrection courseCorrection;
|
|
RouteInfo routeInfo;
|
|
State state = State::None;
|
|
State lastState = State::None;
|
|
Navigation::Status lastOrderStatus;
|
|
DirectionChangeSignal* directionChangeSignal;
|
|
|
|
bool updateDisplay = false;
|
|
bool loopMode = false;
|
|
|
|
uint8_t maxCourseDeviationBeforeAct = 5;
|
|
uint16_t autopilotChangeDelayMillis = 500;
|
|
uint32_t lastAutopilotChangeMillis = 0;
|
|
|
|
int16_t rotationAimAzimuth;
|
|
|
|
double minRemainingDistance = 0.25;
|
|
};
|
|
|
|
#endif // AUTOPILOT_H
|