kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

compilation works again

Browse Source
This commit is contained in:
kleiax
2023-09-04 13:30:34 +02:00
parent 5f650982a7
commit 6a76470f45
23 changed files with 285 additions and 321 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/driveModi/Modi/Autopilot/autopilot.cpp
+12 -12
View File
@@ -12,24 +12,24 @@
#include "driveModi/Modi/Autopilot/autopilot.h"
#include "autopilot.h"
DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) {
this->navigation = navigation;
DirectionChangeSignal::DirectionChangeSignal(Autopilot* pilot) {
this->pilot = pilot;
this->action();
}
DirectionChangeSignal::~DirectionChangeSignal() {
navigation->disableCalcAzimuth();
pilot->getSensorData()->getCalcCompass()->disableCalcAzimuth();
}
void DirectionChangeSignal::action() {
this->navigation->drivingDirectionChange();
pilot->getSensorData()->getCalcCompass()->drivingDirectionChange(pilot->getSensorData()->getCurrentPos());
}
Autopilot::Autopilot(DriveModiParams params, Navigation* navigation)
: ManualControl(params) {
this->setInputMode(ManualControl::InputMode::Digital);
this->directionChangeSignal = new DirectionChangeSignal(navigation);
this->directionChangeSignal = new DirectionChangeSignal(this);
this->setDirectionChangeCallback(this->directionChangeSignal);
this->navigation = navigation;
this->init();
@@ -119,7 +119,7 @@ void Autopilot::init() {
else
this->state = State::NoRoute;
this->routeInfo = this->navigation->getRouteInfo();
this->navigation->getNTRIPClient()->setAutoReconnect(true);
this->sensorData->getNtripClient()->setAutoReconnect(true);
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
@@ -143,7 +143,7 @@ void Autopilot::beginRotate() {
if (this->state != State::SelfDrivingRotate) {
this->lastState = this->state;
this->state = State::SelfDrivingRotate;
this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction;
this->rotationAimAzimuth = this->getSensorData()->getRealAzimuth() + this->courseCorrection.correction;
this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth);
this->moveControl->setSpeed(0);
@@ -155,17 +155,17 @@ void Autopilot::beginRotate() {
}
void Autopilot::rotate() {
if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) {
if (abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) {
this->endRotate();
return;
}
if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3)
if ((abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3)
&&
((this->courseCorrection.correction > 0
&& this->rotationAimAzimuth < this->navigation->getAzimuth())
&& this->rotationAimAzimuth < this->getSensorData()->getRealAzimuth())
|| (this->courseCorrection.correction < 0
&& this->rotationAimAzimuth > this->navigation->getAzimuth())))
&& this->rotationAimAzimuth > this->getSensorData()->getRealAzimuth())))
{
this->endRotate();
std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl;
@@ -177,7 +177,7 @@ void Autopilot::endRotate() {
return;
this->state = this->lastState;
this->navigation->drivingDirectionChange();
this->sensorData->getCalcCompass()->drivingDirectionChange(this->sensorData->getCurrentPos());
this->moveControl->setRotationSpeed(0);
this->moveControl->emergencyStop();
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);