93 lines
2.3 KiB
C++
93 lines
2.3 KiB
C++
/**
|
|
* @file calcAzimuth.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief
|
|
* @version 0.1
|
|
* @date 2023-09-03
|
|
*
|
|
* @copyright Copyright (c) 2023
|
|
*
|
|
*/
|
|
|
|
#include "calcAzimuth.h"
|
|
|
|
CalcAzimuth::CalcAzimuth(Point point) {
|
|
this->lastChangePoint = point;
|
|
this->currentPosition = point;
|
|
this->loopDelay = 50;
|
|
}
|
|
|
|
void CalcAzimuth::drivingDirectionChange(Point point) {
|
|
if (point.isInit() && point.isValid()) {
|
|
this->directionChangeMode = true;
|
|
this->lastChangePoint = point;
|
|
this->state = State::Invalid;
|
|
}
|
|
}
|
|
|
|
void CalcAzimuth::updateCurrentPosition(Point point) {
|
|
this->currentPosition = point;
|
|
this->positionChanged = true;
|
|
}
|
|
|
|
void CalcAzimuth::run() {
|
|
if (!this->positionChanged)
|
|
return;
|
|
|
|
this->positionChanged = false;
|
|
this->updateAzimuth();
|
|
}
|
|
|
|
void CalcAzimuth::updateAzimuth() {
|
|
if (!this->directionChangeMode
|
|
|| this->lastChangePoint.distanceTo(this->currentPosition) < 1.0)
|
|
{
|
|
this->state = State::Invalid;
|
|
this->calcAzimuth = 999;
|
|
return;
|
|
}
|
|
|
|
this->calcAzimuth = this->lastChangePoint.courseTo(this->currentPosition);
|
|
|
|
// Map point accuracy to State
|
|
if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM
|
|
|| this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM)
|
|
{
|
|
this->state = State::Good;
|
|
}
|
|
else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM
|
|
|| this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM)
|
|
{
|
|
this->state = State::Ok;
|
|
}
|
|
else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM
|
|
|| this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM)
|
|
{
|
|
this->state = State::Bad;
|
|
}
|
|
else
|
|
{
|
|
this->state = State::Invalid;
|
|
}
|
|
|
|
// Upgrade quality if the range grows up
|
|
if (this->lastChangePoint.distanceTo(this->currentPosition) > 2.0) {
|
|
switch (this->state) {
|
|
case State::Bad :
|
|
this->state = State::Ok;
|
|
break;
|
|
|
|
case State::Ok :
|
|
this->state = State::Good;
|
|
break;
|
|
|
|
case State::Good :
|
|
this->state = State::Super;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|