122 lines
2.6 KiB
C++
122 lines
2.6 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)
|
|
: lastChangePoint{point}, currentPosition{point}
|
|
{
|
|
Component::loopDelay = CalcAzimuth::loopDelay;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
String CalcAzimuth::stateToString(State state)
|
|
{
|
|
switch (state)
|
|
{
|
|
case State::Invalid:
|
|
return "Invalid";
|
|
|
|
case State::Bad:
|
|
return "Bad";
|
|
|
|
case State::Ok:
|
|
return "Ok";
|
|
|
|
case State::Good:
|
|
return "Good";
|
|
|
|
case State::Super:
|
|
return "Super";
|
|
|
|
default:
|
|
return "UNKOWN";
|
|
}
|
|
}
|
|
|
|
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 = INT16_MIN;
|
|
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) > this->minDistanceForBetterQuality)
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
}
|