clangtidy corrections part 1
This commit is contained in:
@@ -4,138 +4,161 @@
|
||||
* @brief Contains the implementation of the class Autopilot
|
||||
* @version 0.1
|
||||
* @date 2022-02-02
|
||||
*
|
||||
*
|
||||
* @copyright Copyright (c) 2022
|
||||
*
|
||||
*
|
||||
*/
|
||||
|
||||
#include "driveModi/Modi/Autopilot/autopilot.h"
|
||||
#include "autopilot.h"
|
||||
|
||||
DirectionChangeSignal::DirectionChangeSignal(Autopilot* pilot) {
|
||||
this->pilot = pilot;
|
||||
DirectionChangeSignal::DirectionChangeSignal(Autopilot *pilot)
|
||||
: pilot {pilot}
|
||||
{
|
||||
this->action();
|
||||
}
|
||||
|
||||
DirectionChangeSignal::~DirectionChangeSignal() {
|
||||
CalcAzimuth* calcAzimuth = pilot->getSensorData()->getCalcCompass();
|
||||
if (calcAzimuth)
|
||||
DirectionChangeSignal::~DirectionChangeSignal()
|
||||
{
|
||||
CalcAzimuth *calcAzimuth = pilot->getSensorData()->getCalcCompass();
|
||||
if (static_cast<bool>(calcAzimuth))
|
||||
{
|
||||
calcAzimuth->disableCalcAzimuth();
|
||||
}
|
||||
}
|
||||
|
||||
void DirectionChangeSignal::action() {
|
||||
CalcAzimuth* calcAzimuth = pilot->getSensorData()->getCalcCompass();
|
||||
if (calcAzimuth)
|
||||
void DirectionChangeSignal::action()
|
||||
{
|
||||
CalcAzimuth *calcAzimuth = pilot->getSensorData()->getCalcCompass();
|
||||
if (static_cast<bool>(calcAzimuth))
|
||||
{
|
||||
calcAzimuth->drivingDirectionChange(pilot->getSensorData()->getCurrentPos());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Autopilot::Autopilot() {
|
||||
|
||||
}
|
||||
|
||||
Autopilot::~Autopilot() {
|
||||
Autopilot::~Autopilot()
|
||||
{
|
||||
delete this->directionChangeSignal;
|
||||
// this->navigation->getNTRIPClient()->setActivated(false);
|
||||
}
|
||||
|
||||
void Autopilot::run() {
|
||||
void Autopilot::run()
|
||||
{
|
||||
this->routeInfo = this->navigation->getRouteInfo();
|
||||
|
||||
switch (this->state) {
|
||||
case State::InsufficientAccuracy:
|
||||
this->askNavigationForOrder();
|
||||
return;
|
||||
switch (this->state)
|
||||
{
|
||||
case State::InsufficientAccuracy:
|
||||
this->askNavigationForOrder();
|
||||
return;
|
||||
|
||||
case State::NoRoute:
|
||||
return;
|
||||
case State::NoRoute:
|
||||
return;
|
||||
|
||||
case State::None:
|
||||
return;
|
||||
case State::None:
|
||||
return;
|
||||
|
||||
case State::NavigationStarted:
|
||||
this->askNavigationForOrder();
|
||||
this->checkButtonInput();
|
||||
break;
|
||||
case State::NavigationStarted:
|
||||
this->askNavigationForOrder();
|
||||
this->checkButtonInput();
|
||||
break;
|
||||
|
||||
case State::GetToStartPoint:
|
||||
ManualControl::run();
|
||||
this->askNavigationForOrder();
|
||||
if (this->routeInfo.currentPoint >= 2)
|
||||
this->state = State::SelfDrivingAvailable;
|
||||
break;
|
||||
case State::GetToStartPoint:
|
||||
ManualControl::run();
|
||||
this->askNavigationForOrder();
|
||||
if (this->routeInfo.currentPoint >= 2){
|
||||
this->state = State::SelfDrivingAvailable;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::SelfDrivingAvailable:
|
||||
ManualControl::run();
|
||||
this->askNavigationForOrder();
|
||||
this->checkButtonInput();
|
||||
break;
|
||||
case State::SelfDrivingAvailable:
|
||||
ManualControl::run();
|
||||
this->askNavigationForOrder();
|
||||
this->checkButtonInput();
|
||||
break;
|
||||
|
||||
case State::SelfDriving:
|
||||
this->askNavigationForOrder();
|
||||
this->checkButtonInput();
|
||||
this->selfDriving();
|
||||
break;
|
||||
case State::SelfDriving:
|
||||
this->askNavigationForOrder();
|
||||
this->checkButtonInput();
|
||||
this->selfDriving();
|
||||
break;
|
||||
|
||||
case SelfDrivingRotate:
|
||||
this->checkButtonInput();
|
||||
this->rotate();
|
||||
break;
|
||||
case SelfDrivingRotate:
|
||||
this->checkButtonInput();
|
||||
this->rotate();
|
||||
break;
|
||||
|
||||
case State::TargetReached:
|
||||
if (this->loopMode)
|
||||
this->restartLoop();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
case State::TargetReached:
|
||||
if (this->loopMode){
|
||||
this->restartLoop();
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::restart() {
|
||||
void Autopilot::restart()
|
||||
{
|
||||
this->init();
|
||||
}
|
||||
|
||||
bool Autopilot::shouldUpdate() {
|
||||
if (this->updateDisplay) {
|
||||
bool Autopilot::shouldUpdate()
|
||||
{
|
||||
if (this->updateDisplay)
|
||||
{
|
||||
this->updateDisplay = false;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void Autopilot::testRotate(int16_t degree) {
|
||||
if (!degree)
|
||||
void Autopilot::testRotate(int16_t degree)
|
||||
{
|
||||
if (!static_cast<bool>(degree))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
this->courseCorrection.correction = degree;
|
||||
this->beginRotate();
|
||||
}
|
||||
|
||||
void Autopilot::init() {
|
||||
void Autopilot::init()
|
||||
{
|
||||
if (this->navigation->startNavigation())
|
||||
this->state = State::NavigationStarted;
|
||||
else
|
||||
this->state = State::NoRoute;
|
||||
this->routeInfo = this->navigation->getRouteInfo();
|
||||
this->sensorData->getNtripClient()->setAutoReconnect(true);
|
||||
{
|
||||
this->state = State::NavigationStarted;
|
||||
}
|
||||
else
|
||||
{
|
||||
this->state = State::NoRoute;
|
||||
}
|
||||
this->routeInfo = this->navigation->getRouteInfo();
|
||||
this->sensorData->getNtripClient()->setAutoReconnect(true);
|
||||
|
||||
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
|
||||
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
|
||||
|
||||
this->courseCorrection.correction = 0;
|
||||
this->courseCorrection.distance = 0;
|
||||
this->updateDisplay = true;
|
||||
this->courseCorrection.correction = 0;
|
||||
this->courseCorrection.distance = 0;
|
||||
this->updateDisplay = true;
|
||||
}
|
||||
|
||||
void Autopilot::drive() {
|
||||
void Autopilot::drive()
|
||||
{
|
||||
DrivingSpeeds speeds = {0, 0};
|
||||
if (this->courseCorrection.distance >= this->minRemainingDistance)
|
||||
{
|
||||
speeds.x = this->maxSpeeds.x;
|
||||
}
|
||||
this->moveControl->setSpeeds(speeds);
|
||||
}
|
||||
|
||||
void Autopilot::beginRotate() {
|
||||
if (this->state != State::SelfDrivingRotate) {
|
||||
void Autopilot::beginRotate()
|
||||
{
|
||||
if (this->state != State::SelfDrivingRotate)
|
||||
{
|
||||
this->lastState = this->state;
|
||||
this->state = State::SelfDrivingRotate;
|
||||
this->rotationAimAzimuth = this->getSensorData()->getRealAzimuth() + this->courseCorrection.correction;
|
||||
@@ -143,35 +166,40 @@ void Autopilot::beginRotate() {
|
||||
|
||||
DrivingSpeeds speeds = {0, 0};
|
||||
if (this->courseCorrection.correction > 0)
|
||||
{
|
||||
speeds.rot = -this->maxSpeeds.rot;
|
||||
}
|
||||
else
|
||||
{
|
||||
speeds.rot = this->maxSpeeds.rot;
|
||||
}
|
||||
|
||||
this->moveControl->setSpeeds(speeds);
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::rotate() {
|
||||
if (abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) {
|
||||
void Autopilot::rotate()
|
||||
{
|
||||
if (abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct)
|
||||
{
|
||||
this->endRotate();
|
||||
return;
|
||||
}
|
||||
|
||||
if ((abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3)
|
||||
&&
|
||||
((this->courseCorrection.correction > 0
|
||||
&& this->rotationAimAzimuth < this->getSensorData()->getRealAzimuth())
|
||||
|| (this->courseCorrection.correction < 0
|
||||
&& this->rotationAimAzimuth > this->getSensorData()->getRealAzimuth())))
|
||||
if ((abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3) &&
|
||||
((this->courseCorrection.correction > 0 && this->rotationAimAzimuth < this->getSensorData()->getRealAzimuth()) || (this->courseCorrection.correction < 0 && this->rotationAimAzimuth > this->getSensorData()->getRealAzimuth())))
|
||||
{
|
||||
this->endRotate();
|
||||
std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::endRotate() {
|
||||
void Autopilot::endRotate()
|
||||
{
|
||||
if (this->state != State::SelfDrivingRotate)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
this->state = this->lastState;
|
||||
this->sensorData->getCalcCompass()->drivingDirectionChange(this->sensorData->getCurrentPos());
|
||||
@@ -180,65 +208,85 @@ void Autopilot::endRotate() {
|
||||
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
|
||||
}
|
||||
|
||||
void Autopilot::checkButtonInput() {
|
||||
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
|
||||
&& millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
|
||||
|
||||
if (this->state == State::SelfDrivingAvailable)
|
||||
void Autopilot::checkButtonInput()
|
||||
{
|
||||
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action) && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis)
|
||||
{
|
||||
|
||||
if (this->state == State::SelfDrivingAvailable)
|
||||
{
|
||||
this->state = State::SelfDriving;
|
||||
}
|
||||
else if (this->state == State::SelfDriving || this->state == State::SelfDrivingRotate)
|
||||
{
|
||||
this->state = State::SelfDrivingAvailable;
|
||||
}
|
||||
else if (this->state == State::NavigationStarted)
|
||||
{
|
||||
this->state = State::GetToStartPoint;
|
||||
}
|
||||
this->updateDisplay = true;
|
||||
this->lastAutopilotChangeMillis = millis();
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::askNavigationForOrder() {
|
||||
void Autopilot::askNavigationForOrder()
|
||||
{
|
||||
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection);
|
||||
switch (this->lastOrderStatus) {
|
||||
case Navigation::Status::Complete:
|
||||
this->state = State::TargetReached;
|
||||
this->moveControl->setSpeed(0);
|
||||
this->moveControl->setRotationSpeed(0);
|
||||
break;
|
||||
|
||||
case Navigation::Status::InsufficientAccuracy:
|
||||
if (this->state == State::InsufficientAccuracy)
|
||||
break;
|
||||
this->lastState = this->state;
|
||||
this->state = State::InsufficientAccuracy;
|
||||
this->moveControl->setSpeed(0);
|
||||
this->moveControl->setRotationSpeed(0);
|
||||
break;
|
||||
switch (this->lastOrderStatus)
|
||||
{
|
||||
case Navigation::Status::Complete:
|
||||
this->state = State::TargetReached;
|
||||
this->moveControl->setSpeed(0);
|
||||
this->moveControl->setRotationSpeed(0);
|
||||
break;
|
||||
|
||||
case Navigation::Status::Unchanged:
|
||||
if (this->state == State::InsufficientAccuracy)
|
||||
this->state = this->lastState;
|
||||
case Navigation::Status::InsufficientAccuracy:
|
||||
if (this->state == State::InsufficientAccuracy){
|
||||
break;
|
||||
}
|
||||
this->lastState = this->state;
|
||||
this->state = State::InsufficientAccuracy;
|
||||
this->moveControl->setSpeed(0);
|
||||
this->moveControl->setRotationSpeed(0);
|
||||
break;
|
||||
|
||||
case Navigation::Status::Updated:
|
||||
if (this->state == State::InsufficientAccuracy)
|
||||
this->state = this->lastState;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
case Navigation::Status::Unchanged:
|
||||
if (this->state == State::InsufficientAccuracy){
|
||||
this->state = this->lastState;
|
||||
}
|
||||
break;
|
||||
|
||||
case Navigation::Status::Updated:
|
||||
if (this->state == State::InsufficientAccuracy){
|
||||
this->state = this->lastState;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::selfDriving() {
|
||||
void Autopilot::selfDriving()
|
||||
{
|
||||
if (this->state != State::SelfDriving)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBeforeAct)
|
||||
{
|
||||
this->beginRotate();
|
||||
}
|
||||
else
|
||||
{
|
||||
this->drive();
|
||||
}
|
||||
}
|
||||
|
||||
void Autopilot::restartLoop() {
|
||||
void Autopilot::restartLoop()
|
||||
{
|
||||
this->navigation->startNavigation();
|
||||
this->state = State::SelfDriving;
|
||||
this->routeInfo = this->navigation->getRouteInfo();
|
||||
@@ -246,7 +294,8 @@ void Autopilot::restartLoop() {
|
||||
this->updateDisplay = true;
|
||||
}
|
||||
|
||||
void Autopilot::afterActivate() {
|
||||
void Autopilot::afterActivate()
|
||||
{
|
||||
this->setInputMode(ManualControl::InputMode::Digital);
|
||||
this->directionChangeSignal = new DirectionChangeSignal(this);
|
||||
this->setDirectionChangeCallback(this->directionChangeSignal);
|
||||
|
||||
Reference in New Issue
Block a user