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

remove CalcAzimuth

Browse Source
This commit is contained in:
kleiax
2023-10-12 20:58:30 +02:00
parent 12c338def1
commit f0474027be
7 changed files with 4 additions and 272 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/CalcAzimuth/calcAzimuth.cpp
-121
View File
@@ -1,121 +0,0 @@
/**
* @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;
}
}
}
lib/CalcAzimuth/calcAzimuth.h
-93
View File
@@ -1,93 +0,0 @@
/**
* @file calcAzimuth.h
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains a class that calculates the azimuth from a last and a current position
* @version 0.1
* @date 2023-09-03
*
* @copyright Copyright (c) 2023
*
*/
#ifndef CALC_AZIMUTH_H
#define CALC_AZIMUTH_H
#include "component.h"
#include "point.h"
/**
* @brief A class to calculate an azimuth
*
* This class calculates the current Azimuth with the last position
* where the rover has been rotated and the current position
*/
class CalcAzimuth : public Component
{
public:
/**
* @brief States which represent the quality of the current calculated azimuth
*/
enum State
{
Invalid,
Bad,
Ok,
Good,
Super
};
/**
* @brief Construct a new Calc Azimuth object
*
* @param point current position
*/
CalcAzimuth(Point point);
/**
* @brief Have to be called if the rover rotates
*
* @param point current position
*/
void drivingDirectionChange(Point point);
/**
* @brief update the current position
*
* This function should be called if the rover has moved in
* a straight direction, to calculated the current Azimuth.
* More distance to the point given to drivingDirectionChange()
* increase the accuracy of the calculation.
*
* @param point current position
*/
void updateCurrentPosition(Point point);
void disableCalcAzimuth() { this->directionChangeMode = false; }
int16_t getAzimuth() const { return this->calcAzimuth; }
/**
* @brief Get the State struct
*
* @return State current quality of the calculation
*/
State getState() const { return this->state; }
static String stateToString(State state);
private:
void run() override;
void updateAzimuth();
State state = State::Invalid;
Point lastChangePoint;
Point currentPosition;
bool positionChanged = false;
bool directionChangeMode = false;
int16_t calcAzimuth = INT16_MAX;
double minDistanceForBetterQuality = 2;
static constexpr uint8_t loopDelay = 50;
};
#endif // CALC_AZIMUTH_H
lib/MotorControl/motorControl.cpp
-3
View File
@@ -158,7 +158,6 @@ bool MotorControl::isAccelerationNegative() const
void MotorControl::setRealPower(int8_t power)
{
// TODO: Exceptionhandling
if (power <= 100 && power >= -100)
{
this->power = power;
@@ -199,8 +198,6 @@ void MotorControl::setRealPower(int8_t power)
void MotorControl::increasePower(int8_t power)
{
// TODO: Exceptionhandling
// TODO: make a stop befor a direction change
if (abs(power) > 2 * MotorControl::powerSteps)
{
Serial.println("Invalid Argument in MotorControl::increasePower");
lib/Sensors/sensorData.cpp
-15
View File
@@ -53,7 +53,6 @@ void SensorData::enableRealCompass()
this->realCompass = new QMC5883LCompass();
// Init Compass
Wire.beginTransmission(address);
// TODO: describe Bytes !!!
Wire.write(0x0b);
Wire.write(0x01);
Wire.endTransmission();
@@ -63,20 +62,6 @@ void SensorData::enableRealCompass()
caliCompass.useData();
}
void SensorData::enableCalcCompass()
{
// TODO: !!! implementieren
}
CalcAzimuth::State SensorData::getCalcAzimuthState() const
{
if (static_cast<bool>(this->calcCompass))
{
return this->calcCompass->getState();
}
return CalcAzimuth::State::Invalid;
}
void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct)
{
static constexpr uint8_t stringSize = 32;
lib/Sensors/sensorData.h
-27
View File
@@ -21,7 +21,6 @@
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <QMC5883LCompass.h>
#include "calcAzimuth.h"
#include "point.h"
@@ -60,33 +59,9 @@ public:
*/
int16_t getRealAzimuth() const { return this->realAzimuth; }
/**
* @brief Get the azimuth calculated by CalcAzimuth
*
* Consider to call getCalcAzimuthState() to check, if the data is valid.
*
* @return int16_t
*/
int16_t getCalcAzimuth() const { return this->calcAzimuth; }
/**
* @brief Get the CalcAzimuth::State object
*
* Needed to check the quality of calculated azimuth
*
* @return CalcAzimuth::State
*/
CalcAzimuth::State getCalcAzimuthState() const;
Point getCurrentPos() const { return this->currentPosition; }
const UBX_NAV_PVT_data_t *getGnssData() const { return this->gnssData; };
/**
* @brief Get the CalcCompass object
* @return CalcAzimuth*
*/
CalcAzimuth *getCalcCompass() const { return this->calcCompass; }
/**
* @brief Get the RealCompass object
* @return QMC5883LCompass*
@@ -111,14 +86,12 @@ private:
void updateUbxData();
QMC5883LCompass *realCompass = nullptr;
CalcAzimuth *calcCompass = nullptr;
SFE_UBLOX_GNSS *gnss = nullptr;
UBX_NAV_PVT_data_t *gnssData = nullptr;
Point currentPosition;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
uint32_t lastUbxUpdate = 0;
// static