From f0474027be0bad1aca6dee3af9080b8eb16d3a7d Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Thu, 12 Oct 2023 20:58:30 +0200 Subject: [PATCH] remove CalcAzimuth --- lib/CalcAzimuth/calcAzimuth.cpp | 121 ------------------ lib/CalcAzimuth/calcAzimuth.h | 93 -------------- lib/MotorControl/motorControl.cpp | 3 - lib/Sensors/sensorData.cpp | 15 --- lib/Sensors/sensorData.h | 27 ---- .../SensorData/menuSensorData.cpp | 15 +-- src/driveModi/Modi/TestMode/testMode.cpp | 2 - 7 files changed, 4 insertions(+), 272 deletions(-) delete mode 100644 lib/CalcAzimuth/calcAzimuth.cpp delete mode 100644 lib/CalcAzimuth/calcAzimuth.h diff --git a/lib/CalcAzimuth/calcAzimuth.cpp b/lib/CalcAzimuth/calcAzimuth.cpp deleted file mode 100644 index f7f3c4a..0000000 --- a/lib/CalcAzimuth/calcAzimuth.cpp +++ /dev/null @@ -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; - } - } -} diff --git a/lib/CalcAzimuth/calcAzimuth.h b/lib/CalcAzimuth/calcAzimuth.h deleted file mode 100644 index 4b18315..0000000 --- a/lib/CalcAzimuth/calcAzimuth.h +++ /dev/null @@ -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 diff --git a/lib/MotorControl/motorControl.cpp b/lib/MotorControl/motorControl.cpp index 6ff901f..82a82ba 100644 --- a/lib/MotorControl/motorControl.cpp +++ b/lib/MotorControl/motorControl.cpp @@ -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"); diff --git a/lib/Sensors/sensorData.cpp b/lib/Sensors/sensorData.cpp index 97c731f..622a554 100644 --- a/lib/Sensors/sensorData.cpp +++ b/lib/Sensors/sensorData.cpp @@ -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(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; diff --git a/lib/Sensors/sensorData.h b/lib/Sensors/sensorData.h index f22e5b5..d95cae7 100644 --- a/lib/Sensors/sensorData.h +++ b/lib/Sensors/sensorData.h @@ -21,7 +21,6 @@ #include #include -#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 diff --git a/src/SpecialMenus/SensorData/menuSensorData.cpp b/src/SpecialMenus/SensorData/menuSensorData.cpp index 8cf4340..7148cf0 100644 --- a/src/SpecialMenus/SensorData/menuSensorData.cpp +++ b/src/SpecialMenus/SensorData/menuSensorData.cpp @@ -30,19 +30,12 @@ void MenuSensorData::printPage() const switch (this->getCurrentPage()) { - case 1: + case 0: lineOne = "Real Azimuth:"; lineTwo.concat(this->sensorData->getRealAzimuth()); break; - case 2: - lineOne = "Calc Azimuth:"; - lineTwo.concat(this->sensorData->getCalcAzimuth()); - lineTwo.concat(" "); - lineTwo.concat(CalcAzimuth::stateToString(this->sensorData->getCalcAzimuthState())); - break; - - case 3: + case 1: { lineOne = "Lat:"; lineTwo = "Lon:"; @@ -52,7 +45,7 @@ void MenuSensorData::printPage() const } break; - case 4: + case 2: lineOne = "Time: "; lineTwo = ""; if (static_cast(fixType)) @@ -82,7 +75,7 @@ void MenuSensorData::printPage() const break; - case 6: + case 3: lineOne = "HAcc: "; lineTwo = "Sats: "; if (static_cast(fixType)) diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp index afda3dd..e7845e2 100644 --- a/src/driveModi/Modi/TestMode/testMode.cpp +++ b/src/driveModi/Modi/TestMode/testMode.cpp @@ -82,8 +82,6 @@ bool TestMode::drive(int16_t cmDistance, int16_t degree) } else { - // forward or backward and left or right - // TODO: Calculate roationspeed if (cmDistance < 0) { this->moveControl->setSpeed(-this->maxSpeeds.x);