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

refactore

Browse Source
This commit is contained in:
kleiax
2023-09-03 12:36:16 +02:00
parent faaa3603a7
commit 5f650982a7
30 changed files with 578 additions and 430 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/TODO allgemein.txt
+3 -5
View File
@@ -16,15 +16,12 @@ Do later:
-> Menu structure mit add functions for each menu mit pointer return to config (additional not replace)
-> Menu Display from parent as run() to make Menu as Component
-> Racing Mode
-> ConsolControl
Do now:
Code:
Doxygen Kommentare aktualisieren
Allgemeine Klasse für Sensordaten
DriveModi soll diese Klasse übergeben bekommen
DriveModi bekommt Benutzereinagebn Objekt / Oder extra Objekt was diese Daten speichert als extra Klasse damit weniger parameter
Geschwindigkeiten außerhalb von Modi einstellen, Manager kann die Werte einstellen da Interface
Git Branch ohne Navigation
Automat in MoveControl weil jetzt in Arbeit beschrieben
Liste mit Betriebsmodi, automatisch in Menü einfügen
Betriebmodi dem Drivemanger ohne switchCase geben, aus Liste oder so
@@ -33,8 +30,9 @@ Code:
battery methode für daten erzeugen und in eeporm speichern
battery bruch mit R werten nur einmal berechnen
lange kein daten von fernbedienung dann?
compass calibrieren eigener Betriebsmodus
Input pointer von fernbedienung nicht veränderbar doppel const
trennen funktion und ui route menü
DriveManager Mode als Template übergeben
Latex:
Genaue Beschreibung von PulseCounter HardwareUnit wenn möglich
lib/CalcAzimuth/calcAzimuth.cpp
+92
View File
@@ -0,0 +1,92 @@
/**
* @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 = CalcAzimuthState::Invalid;
}
}
void CalcAzimuth::updateCurrentPosition(Point point) {
this->currentPosition = point;
this->positionChanged = true;
}
void CalcAzimuth::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 = CalcAzimuthState::Invalid;
this->calcAzimuth = 999;
return;
}
this->calcAzimuth = this->lastChangePoint.courseTo(this->currentPosition);
// Map point accuracy to CalcAzimuthState
if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM)
{
this->state = CalcAzimuthState::Good;
}
else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM)
{
this->state = CalcAzimuthState::Ok;
}
else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM)
{
this->state = CalcAzimuthState::Bad;
}
else
{
this->state = CalcAzimuthState::Invalid;
}
// Upgrade quality if the range grows up
if (this->lastChangePoint.distanceTo(this->currentPosition) > 2.0) {
switch (this->state) {
case CalcAzimuthState::Bad :
this->state = CalcAzimuthState::Ok;
break;
case CalcAzimuthState::Ok :
this->state = CalcAzimuthState::Good;
break;
case CalcAzimuthState::Good :
this->state = CalcAzimuthState::Super;
break;
default:
break;
}
}
}
lib/CalcAzimuth/calcAzimuth.h
+50
View File
@@ -0,0 +1,50 @@
/**
* @file calcAzimuth.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @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"
class CalcAzimuth : public Component {
public:
enum CalcAzimuthState {
Invalid,
Bad,
Ok,
Good,
Super
};
CalcAzimuth(Point point);
void drivingDirectionChange(Point point);
void updateCurrentPosition(Point point);
void disableCalcAzimuth() { this->directionChangeMode = false; }
int16_t getAzimuth() const { return this->calcAzimuth; }
CalcAzimuthState getState() const { return this->state; }
private:
void run() override;
void updateAzimuth();
CalcAzimuthState state = CalcAzimuthState::Invalid;
Point lastChangePoint;
Point currentPosition;
bool positionChanged = false;
bool directionChangeMode = false;
int16_t calcAzimuth = INT16_MAX;
};
#endif //CALC_AZIMUTH_H
lib/Navigation/navigation.cpp
-63
View File
@@ -65,15 +65,6 @@ bool Navigation::startNavigation() {
return this->navigationStarted;
}
void Navigation::drivingDirectionChange() {
Point tmp = this->currentPosition;
if (tmp.isInit() && tmp.isValid()) {
this->directionChangeMode = true;
this->lastPointDrivingDirectionChange = tmp;
this->calcAzimuthState = CalcAzimuthState::Invalid;
}
}
Navigation::Status Navigation::getCourseCorrection(CourseCorrection& correction, bool forceUpdate) {
if (this->navigationFinished)
return Status::Complete;
@@ -142,60 +133,6 @@ void Navigation::updateCurrentLocation() {
this->currentPosition = Point(coords, this->ubxData->hAcc);
}
void Navigation::updateMagneticDeclination() {
if (!this->directionChangeMode
|| this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) < 1.0)
{
this->calcAzimuthState = CalcAzimuthState::Invalid;
this->calcAzimuth = 999;
return;
}
this->calcAzimuth = this->lastPointDrivingDirectionChange.courseTo(this->currentPosition);
// Map point accuracy to CalcAzimuthState
if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::oneDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM)
{
this->calcAzimuthState = CalcAzimuthState::Good;
}
else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::twoDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM)
{
this->calcAzimuthState = CalcAzimuthState::Ok;
}
else if (this->lastPointDrivingDirectionChange.getAccuracy() == Point::Accuracy::threeDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM)
{
this->calcAzimuthState = CalcAzimuthState::Bad;
}
else
{
this->calcAzimuthState = CalcAzimuthState::Invalid;
}
// Upgrade quality if the range grows up
if (this->lastPointDrivingDirectionChange.distanceTo(this->currentPosition) > 2.0) {
switch (this->calcAzimuthState) {
case CalcAzimuthState::Bad :
this->calcAzimuthState = CalcAzimuthState::Ok;
break;
case CalcAzimuthState::Ok :
this->calcAzimuthState = CalcAzimuthState::Good;
break;
case CalcAzimuthState::Good :
this->calcAzimuthState = CalcAzimuthState::Super;
break;
default:
break;
}
}
}
int16_t Navigation::calculateCourseCorrection(Point& point) {
int16_t targetCourse = point.courseTo(this->targetPoint);
int16_t correctionCourse;
lib/Navigation/navigation.h
+1 -19
View File
@@ -90,8 +90,7 @@ class Navigation : public Component {
*/
bool startNavigation();
void freezeTargetPoint(bool val = true) { this->preventNextPoint = val; };
void drivingDirectionChange();
void disableCalcAzimuth() { this->directionChangeMode = false; }
double increaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint += 0.1; }
double decreaseMinDistanceToReachPoint() { return this->minDistanceToReachPoint -= 0.1; }
@@ -148,20 +147,6 @@ class Navigation : public Component {
Route* getRoute() const { return this->route; }
Point getCurrentPosition() const { return this->currentPosition; }
/**
* @brief Get realAzimuth
*
* This value represents the angle between north and
* the line of sight. Clockwise.
*
* @return uint16_t degree
*/
int16_t getAzimuth() const { return this->realAzimuth; }
QMC5883LCompass* getCompass() const { return this->compass; }
int16_t getCalcAzimuth() const { return this->calcAzimuth; }
CalcAzimuthState getCalcAzimuthState() const { return this->calcAzimuthState; }
bool getLastUsedCalcAzimuth() const { return this->lastUsedCalcAzimuth; }
Point::Accuracy getMinAccuracy() const { return this->minAccuracy; }
void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; }
@@ -170,7 +155,6 @@ class Navigation : public Component {
private:
void run() override;
void updateMagneticDeclination();
void init(Route* route);
bool nextPoint();
bool setTargetPoint(Point target);
@@ -206,8 +190,6 @@ class Navigation : public Component {
char* user;
char* password;
int16_t calcAzimuth = INT16_MAX;
uint8_t timeToWait = 200;
uint16_t port;
uint32_t lastMillis = 0;
lib/Point/point.cpp
+102
View File
@@ -0,0 +1,102 @@
/**
* @file point.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-09-03
*
* @copyright Copyright (c) 2023
*
*/
Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates.lat = lat;
this->coordinates.lon = lon;
this->init(horizontalAccuracy, creationTime);
}
Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates.lat = lat / 10000000.0;
this->coordinates.lon = lon / 10000000.0;
this->init(horizontalAccuracy, creationTime);
}
Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates = coords;
this->init(horizontalAccuracy, creationTime);
}
Point::Point(Coordinates coords, bool imported) {
this->coordinates = coords;
if (imported)
this->init(UINT32_MAX, 0);
else
this->init(0, 0);
}
Point::Point() {
this->coordinates.lat = 0;
this->coordinates.lon = 0;
this->init(0, 0);
}
bool Point::operator==(const Point& rhs) const {
return this->coordinates == rhs.getCoordinates();
}
// distance = sqrt(dx * dx + dy * dy)
// mit distance: Entfernung in km
// dx = 111.3 * cos(lat) * (lon1 - lon2)
// lat = (lat1 + lat2) / 2 * 0.01745
// dy = 111.3 * (lat1 - lat2)
// lat1, lat2, lon1, lon2: Breite, Länge in Grad
double Point::distanceTo(const Coordinates& point) const {
Coordinates begin = this->coordinates;
Coordinates end = point;
double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
return sqrt(dx * dx + dy * dy);
}
double Point::distanceTo(const Point &point) const {
return this->distanceTo(point.getCoordinates());
}
int16_t Point::courseTo(const Coordinates& point) const {
Coordinates begin = this->coordinates;
Coordinates end = point;
double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) );
double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
int16_t res = static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
// if (res < 0)
// res += 360;
return res;
}
int16_t Point::courseTo(const Point &point) const {
return this->courseTo(point.getCoordinates());
}
void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) {
this->creationTime = creationTime;
if (horizontalAccuracy == UINT32_MAX)
this->accuracy = Accuracy::imported;
else if (horizontalAccuracy > 9999)
this->accuracy = Accuracy::fourDigOfCM;
else if (horizontalAccuracy > 999)
this->accuracy = Accuracy::threeDigOfCM;
else if (horizontalAccuracy > 99)
this->accuracy = Accuracy::twoDigOfCM;
else if (horizontalAccuracy > 1)
this->accuracy = Accuracy::oneDigOfCM;
else
this->accuracy = Accuracy::none;
}
lib/Point/point.h
+139
View File
@@ -0,0 +1,139 @@
/**
* @file point.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-09-03
*
* @copyright Copyright (c) 2023
*
*/
#ifndef POINT_H
#define POINT_H
#include <cmath>
#define ROUTE_DEGREE_TO_RADIANT 0.01745
#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300
/**
* @brief A to handle points on the earth
*
* The points inherits latidue and longitude as doubles
*
*/
class Point{
public:
/**
* @brief Hold the data longitude and latitude
*
*/
struct Coordinates {
double lon;
double lat;
bool operator==(const Coordinates rhs) const {
return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon );
}
};
/**
* @brief The Accuracy is set by the constructor
*
*/
enum Accuracy {
none,
fourDigOfCM,
threeDigOfCM,
twoDigOfCM,
oneDigOfCM,
imported
};
/**
* @brief Construct a new Point object
*
* @param lat latitude
* @param lon longitude
* @param horizontalAccuracy mm
* @param coords Coordinates
* @param imported if true than highest accuracy
*/
Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
Point(Coordinates coords, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
Point(Coordinates coords, bool imported);
Point();
/**
* @brief Checks if to points are equal.
*
* @param rhs
* @return true
* @return false
*/
bool operator==(const Point& rhs) const;
/**
* @brief Checks if the point is initalized.
*
* @return true
* @return false
*/
bool isInit() const { return this->coordinates.lat + this->coordinates.lon; }
/**
* @brief Checks if the point is valid.
*
* If the accuracy is higher than zero, true will be returned.
*
* @return true
* @return false
*/
bool isValid() const { return (this->accuracy > 0) ? true : false; }
/**
* @brief Calculates the distance between to points.
*
* @param point
* @return double meter
*/
double distanceTo(const Coordinates& point) const;
double distanceTo(const Point& point) const;
/**
* @brief Calculates the course to an other point.
*
* @param point
* @return int16_t degree
*/
int16_t courseTo(const Coordinates& point) const;
int16_t courseTo(const Point& point) const;
uint32_t getCreationTime() const { return this->creationTime; }
double getLongitude() const { return this->coordinates.lon; }
double getLatitude() const { return this->coordinates.lat; }
Coordinates getCoordinates() const { return this->coordinates; }
/**
* @brief Get the Accuracy object
*
* The higher the value, the greater the accuracy.
* You can check it by Accuracy.
*
* @return Accuracy
*/
Accuracy getAccuracy() const { return this->accuracy; }
private:
void init(uint32_t horizontalAccuracy, uint32_t creationTime);
Accuracy accuracy = Accuracy::none;
Coordinates coordinates;
uint32_t creationTime = 0;
};
#endif //POINT_H
lib/Route/route.cpp
-93
View File
@@ -11,99 +11,6 @@
#include "route.h"
Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates.lat = lat;
this->coordinates.lon = lon;
this->init(horizontalAccuracy, creationTime);
}
Point::Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates.lat = lat / 10000000.0;
this->coordinates.lon = lon / 10000000.0;
this->init(horizontalAccuracy, creationTime);
}
Point::Point(Coordinates coords, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates = coords;
this->init(horizontalAccuracy, creationTime);
}
Point::Point(Coordinates coords, bool imported) {
this->coordinates = coords;
if (imported)
this->init(UINT32_MAX, 0);
else
this->init(0, 0);
}
Point::Point() {
this->coordinates.lat = 0;
this->coordinates.lon = 0;
this->init(0, 0);
}
bool Point::operator==(const Point& rhs) const {
return this->coordinates == rhs.getCoordinates();
}
// distance = sqrt(dx * dx + dy * dy)
// mit distance: Entfernung in km
// dx = 111.3 * cos(lat) * (lon1 - lon2)
// lat = (lat1 + lat2) / 2 * 0.01745
// dy = 111.3 * (lat1 - lat2)
// lat1, lat2, lon1, lon2: Breite, Länge in Grad
double Point::distanceTo(const Coordinates& point) const {
Coordinates begin = this->coordinates;
Coordinates end = point;
double lat = (begin.lat + end.lat) / 2 * ROUTE_DEGREE_TO_RADIANT;
double dy = ROUTE_DISTANCE_BETWEEN_LATITUDE * (begin.lat - end.lat);
double dx = ROUTE_DISTANCE_BETWEEN_LATITUDE * cos(lat) * (begin.lon - end.lon);
return sqrt(dx * dx + dy * dy);
}
double Point::distanceTo(const Point &point) const {
return this->distanceTo(point.getCoordinates());
}
int16_t Point::courseTo(const Coordinates& point) const {
Coordinates begin = this->coordinates;
Coordinates end = point;
double phi = log( tan(end.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) / tan(begin.lat * ROUTE_DEGREE_TO_RADIANT / 2 + M_PI / 4) );
double lon = (begin.lon * ROUTE_DEGREE_TO_RADIANT - end.lon * ROUTE_DEGREE_TO_RADIANT);
int16_t res = static_cast<int16_t>(atan2(lon, phi) / ROUTE_DEGREE_TO_RADIANT) * -1;
// if (res < 0)
// res += 360;
return res;
}
int16_t Point::courseTo(const Point &point) const {
return this->courseTo(point.getCoordinates());
}
void Point::init(uint32_t horizontalAccuracy, uint32_t creationTime) {
this->creationTime = creationTime;
if (horizontalAccuracy == UINT32_MAX)
this->accuracy = Accuracy::imported;
else if (horizontalAccuracy > 9999)
this->accuracy = Accuracy::fourDigOfCM;
else if (horizontalAccuracy > 999)
this->accuracy = Accuracy::threeDigOfCM;
else if (horizontalAccuracy > 99)
this->accuracy = Accuracy::twoDigOfCM;
else if (horizontalAccuracy > 1)
this->accuracy = Accuracy::oneDigOfCM;
else
this->accuracy = Accuracy::none;
}
Route::Route() {
}
lib/Route/route.h
+1 -122
View File
@@ -14,129 +14,8 @@
#include <cstdint>
#include <list>
#include <cmath>
#define ROUTE_DEGREE_TO_RADIANT 0.01745
#define ROUTE_DISTANCE_BETWEEN_LATITUDE 111300
/**
* @brief A to handle points on the earth
*
* The points inherits latidue and longitude as doubles
*
*/
class Point{
public:
/**
* @brief Hold the data longitude and latitude
*
*/
struct Coordinates {
double lon;
double lat;
bool operator==(const Coordinates rhs) const {
return ( this->lon == rhs.lon ) && ( this->lon == rhs.lon );
}
};
/**
* @brief The Accuracy is set by the constructor
*
*/
enum Accuracy {
none,
fourDigOfCM,
threeDigOfCM,
twoDigOfCM,
oneDigOfCM,
imported
};
/**
* @brief Construct a new Point object
*
* @param lat latitude
* @param lon longitude
* @param horizontalAccuracy mm
* @param coords Coordinates
* @param imported if true than highest accuracy
*/
Point(double lat, double lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
Point(int32_t lat, int32_t lon, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
Point(Coordinates coords, uint32_t horizontalAccuracy = 0, uint32_t creationTime = 0);
Point(Coordinates coords, bool imported);
Point();
/**
* @brief Checks if to points are equal.
*
* @param rhs
* @return true
* @return false
*/
bool operator==(const Point& rhs) const;
/**
* @brief Checks if the point is initalized.
*
* @return true
* @return false
*/
bool isInit() const { return this->coordinates.lat + this->coordinates.lon; }
/**
* @brief Checks if the point is valid.
*
* If the accuracy is higher than zero, true will be returned.
*
* @return true
* @return false
*/
bool isValid() const { return (this->accuracy > 0) ? true : false; }
/**
* @brief Calculates the distance between to points.
*
* @param point
* @return double meter
*/
double distanceTo(const Coordinates& point) const;
double distanceTo(const Point& point) const;
/**
* @brief Calculates the course to an other point.
*
* @param point
* @return int16_t degree
*/
int16_t courseTo(const Coordinates& point) const;
int16_t courseTo(const Point& point) const;
uint32_t getCreationTime() const { return this->creationTime; }
double getLongitude() const { return this->coordinates.lon; }
double getLatitude() const { return this->coordinates.lat; }
Coordinates getCoordinates() const { return this->coordinates; }
/**
* @brief Get the Accuracy object
*
* The higher the value, the greater the accuracy.
* You can check it by Accuracy.
*
* @return Accuracy
*/
Accuracy getAccuracy() const { return this->accuracy; }
private:
void init(uint32_t horizontalAccuracy, uint32_t creationTime);
Accuracy accuracy = Accuracy::none;
Coordinates coordinates;
uint32_t creationTime = 0;
};
#include "point.h"
/**
* @brief Holds some route information
lib/Sensors/senors.cpp
+15 -11
View File
@@ -11,17 +11,8 @@
#include "sensors.h"
void Sensors::run() {
this->compass->read();
this->realAzimuth = this->compass->getAzimuth();
}
void Sensors::runAsChild() {
this->gnss->checkUblox();
this->gnss->checkCallbacks();
if (Sensors::newData)
Sensors::newData = false;
Sensors::Sensors() {
this->sensorData = new SensorData(this);
}
void Sensors::enableGnss(SPIClass* spiPort, uint8_t csPin) {
@@ -59,6 +50,19 @@ void Sensors::setOutputStatusPrintPVTdata(bool status) {
Sensors::outputStatusPrintPVTdata = status;
}
void Sensors::run() {
this->compass->read();
this->realAzimuth = this->compass->getAzimuth();
}
void Sensors::runAsChild() {
this->gnss->checkUblox();
this->gnss->checkCallbacks();
if (Sensors::newData)
Sensors::newData = false;
}
void Sensors::initGnss() {
uint8_t versionHigh = this->gnss->getProtocolVersionHigh();
uint8_t versionLow = this->gnss->getProtocolVersionLow();
lib/Sensors/sensorData.h
+40
View File
@@ -0,0 +1,40 @@
/**
* @file sensorData.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-09-02
*
* @copyright Copyright (c) 2023
*
*/
#ifndef SENSOR_DATA_H
#define SENSOR_DATA_H
#include "sensors.h"
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <QMC5883LCompass.h>
// Gyroskop
#include "calcAzimuth.h"
class SensorData {
public:
SensorData(Sensors *senors);
int16_t getRealAzimuth() const { return 0; }
int16_t getCalcAzimuth() const { return 0; }
CalcAzimuth::CalcAzimuthState getCalcAzimuthState() const { return CalcAzimuth::CalcAzimuthState::Invalid; }
const UBX_NAV_PVT_data_t* const getGnssData() const;
const void* const getGyroData() const;
private:
Sensors* sensors;
};
SensorData::SensorData(Sensors* senors) {
this->sensors = sensors;
}
#endif //SENSOR_DATA_H
lib/Sensors/sensors.h
+5 -11
View File
@@ -25,24 +25,15 @@
class Sensors : public Component {
public:
enum CalcAzimuthState {
Invalid,
Bad,
Ok,
Good,
Super
};
Sensors();
void run() override;
void runAsChild() override;
void enableGnss(SPIClass* spiPort, uint8_t csPin);
void enableGnss();
void enableCompass();
void enableGyroskop();
const SensorData const getSensorData() const { return this->sensorData; }
/**
* @brief Set the output status for PVTdata.
*
@@ -54,10 +45,13 @@ class Sensors : public Component {
static void setOutputStatusPrintPVTdata(bool status);
private:
void run() override;
void runAsChild() override;
void initGnss();
QMC5883LCompass* compass = nullptr;
SFE_UBLOX_GNSS* gnss = nullptr;
SensorData* sensorData;
CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid;
src/driveModi/Modi/Autopilot/autopilot.cpp
+2 -2
View File
@@ -26,8 +26,8 @@ void DirectionChangeSignal::action() {
}
Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
: ManualControl(moveControl, input) {
Autopilot::Autopilot(DriveModiParams params, Navigation* navigation)
: ManualControl(params) {
this->setInputMode(ManualControl::InputMode::Digital);
this->directionChangeSignal = new DirectionChangeSignal(navigation);
this->setDirectionChangeCallback(this->directionChangeSignal);
src/driveModi/Modi/Autopilot/autopilot.h
+1 -2
View File
@@ -14,7 +14,6 @@
#include "navigation.h"
#include "moveControl.h"
#include "driveModi/Modi/ManualControl/manualControl.h"
class DirectionChangeSignal : public DirectionChangeWrapper {
@@ -56,7 +55,7 @@ class Autopilot : public ManualControl {
* @param moveControl for ManualControl
* @param navigation for route instructions
*/
Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation);
Autopilot(DriveModiParams params, Navigation* navigation);
/**
* @brief Destroy the Autopilot object
src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp
+24
View File
@@ -0,0 +1,24 @@
/**
* @file calibrateCompassM.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-09-03
*
* @copyright Copyright (c) 2023
*
*/
#include "calibrateCompassM.h"
CalibrateCompassM::CalibrateCompassM(DriveModiParams params) : ManualControl(params) {}
void CalibrateCompassM::setCalibrateCompass(CalibrateCompass *caliCompass) {
if (caliCompass) {
this->caliCompass = caliCompass;
this->addChildComponent(this->caliCompass);
} else if (this->caliCompass) {
this->removeChildComponent(this->caliCompass);
this->caliCompass = caliCompass;
}
}
src/driveModi/Modi/CalibrateCompass/calibrateCompassM.h
+29
View File
@@ -0,0 +1,29 @@
/**
* @file calibrateCompassM.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-09-03
*
* @copyright Copyright (c) 2023
*
*/
#ifndef CALIBRATE_COMPASS_M_H
#define CALIBRATE_COMPASS_M_H
#include "calibrateCompass.h"
#include "driveModi/Modi/ManualControl/manualControl.h"
class CalibrateCompassM : public ManualControl {
public:
CalibrateCompassM(DriveModiParams params);
void setCalibrateCompass(CalibrateCompass* caliCompass = nullptr);
private:
CalibrateCompass* caliCompass = nullptr;
};
#endif //CALIBRATE_COMPASS_M_H
src/driveModi/Modi/CaptureRoute/captureRoute.cpp
+2 -2
View File
@@ -11,8 +11,8 @@
#include "driveModi/Modi/CaptureRoute/captureRoute.h"
CaptureRoute::CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
: ManualControl(moveControl, input) {
CaptureRoute::CaptureRoute(DriveModiParams params, Navigation* navigation)
: ManualControl(params) {
this->navigation = navigation;
this->navigation->getRoute()->clear();
this->navigation->getNTRIPClient()->setAutoReconnect(true);
src/driveModi/Modi/CaptureRoute/captureRoute.h
+1 -1
View File
@@ -33,7 +33,7 @@ class CaptureRoute : public ManualControl {
* @param moveControl for ManualControl
* @param navigation to add Points
*/
CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation);
CaptureRoute(DriveModiParams params, Navigation* navigation);
/**
* @brief Destroy the Capture Route object
src/driveModi/Modi/ConsolControl/consolControl.cpp
-13
View File
@@ -1,13 +0,0 @@
/**
* @file consolControl.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2022-02-15
*
* @copyright Copyright (c) 2022
*
*/
#include "consolControl.h"
src/driveModi/Modi/ConsolControl/consolControl.h
-20
View File
@@ -1,20 +0,0 @@
/**
* @file consolControl.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2022-02-15
*
* @copyright Copyright (c) 2022
*
*/
#ifndef CONSOL_CONTROL_H
#define CONSOL_CONTROL_H
#include "driveModi/driveModi.h"
class ConsolControl : DriveModi{
};
#endif // CONSOL_CONTROL_H
src/driveModi/Modi/ManualControl/manualControl.cpp
+1 -20
View File
@@ -10,15 +10,6 @@
*/
#include "manualControl.h"
ManualControl::ManualControl(MoveControl* moveControl, const ControlPadInput *input)
: DriveModi(moveControl) {
this->input = input;
}
ManualControl::~ManualControl() {
}
void ManualControl::run() {
switch (this->inputMode) {
case InputMode::Analog :
@@ -34,16 +25,6 @@ void ManualControl::run() {
}
}
void ManualControl::setCalibrateCompass(CalibrateCompass *caliCompass) {
if (caliCompass) {
this->caliCompass = caliCompass;
this->addChildComponent(this->caliCompass);
} else if (this->caliCompass) {
this->removeChildComponent(this->caliCompass);
this->caliCompass = caliCompass;
}
}
void ManualControl::switchInputMode() {
this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog;
}
@@ -51,7 +32,7 @@ void ManualControl::switchInputMode() {
void ManualControl::analogControl() {
// Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
int16_t x = this->input->x - 127;
int16_t y = this->input->y - 127;
int16_t y = this-> input->y - 127;
// static int counter = 0;
// if (counter % 60 == 0) {
src/driveModi/Modi/ManualControl/manualControl.h
+1 -9
View File
@@ -11,10 +11,7 @@
#ifndef MANUAL_CONTROL_H
#define MANUAL_CONTROL_H
#include "moveControl.h"
#include "driveModi/driveModi.h"
#include "controlPadInput.h"
#include "calibrateCompass.h"
class DirectionChangeWrapper {
public:
@@ -36,10 +33,7 @@ class ManualControl : public DriveModi {
Digital
};
ManualControl(MoveControl *moveControl, const ControlPadInput *input);
~ManualControl();
void setCalibrateCompass(CalibrateCompass* caliCompass = nullptr);
ManualControl(DriveModiParams params) : DriveModi(params){};
void switchInputMode();
void setInputMode(InputMode mode) { this->inputMode = mode; }
@@ -51,14 +45,12 @@ class ManualControl : public DriveModi {
protected:
void run() override;
const ControlPadInput* input;
private:
void analogControl();
void digitalControl();
bool lastLoopTurned = false;
CalibrateCompass* caliCompass = nullptr;
DirectionChangeWrapper* directionChangeWrapper = nullptr;
InputMode inputMode = InputMode::Analog;
};
src/driveModi/Modi/Racing/racing.cpp
View File
src/driveModi/Modi/Racing/racing.h
View File
src/driveModi/Modi/TestMode/testMode.cpp
+2 -2
View File
@@ -10,8 +10,8 @@
*/
#include "testMode.h"
TestMode::TestMode(MoveControl *moveControl, Navigation* navigation)
: DriveModi(moveControl) {
TestMode::TestMode(DriveModiParams params, Navigation* navigation)
: DriveModi(params) {
this->navigation = navigation;
}
src/driveModi/Modi/TestMode/testMode.h
+1 -1
View File
@@ -44,7 +44,7 @@ class TestMode : public DriveModi {
* @param moveControl
* @param navigation
*/
TestMode(MoveControl *moveControl, Navigation* navigation);
TestMode(DriveModiParams params, Navigation* navigation);
~TestMode();
bool drive(int16_t cm = 0, int16_t degree = 0);
src/driveModi/driveManager.cpp
+23 -28
View File
@@ -11,33 +11,22 @@
#include "driveModi/driveManager.h"
Modi& operator++(Modi& m, int) {
return m = (m == Modi::TestMode) ? Modi::Off : static_cast<Modi>(static_cast<int>(m)+1);
DriveManager::DriveManager(MoveControl *moveControl, SensorData* sensorData, ControlPadInput *input, bool wifi) {
this->driveModiParams.input = input;
this->driveModiParams.moveControl = moveControl;
this->driveModiParams.sensorData = sensorData;
this->init(wifi);
}
DriveManager::DriveManager(MoveControl *moveControl, SPIClass *spiPort, const ControlPadInput *input, bool wifi) {
this->moveControl = moveControl;
this->input = input;
DriveManager::DriveManager(DriveModiParams params, bool wifi){
this->driveModiParams = params;
this->spiPort = spiPort;
this->navigation = new Navigation(spiPort, PinNumbers::gnssSpiCs);
if (wifi)
this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD);
this->addChildComponent(this->moveControl);
this->addChildComponent(this->navigation);
this->activateOnlyChilds();
this->init(wifi);
}
DriveManager::~DriveManager() {
delete this->navigation;
delete this->spiPort;
}
void DriveManager::run() {}
void DriveManager::nextModus() {
changeModus(this->currentModus++);
}
void DriveManager::changeModus(Modi modus) {
@@ -56,26 +45,22 @@ void DriveManager::changeModus(Modi modus) {
break;
case Modi::ManualControl: {
this->currentModusPtr = new ManualControl(this->moveControl, this->input);
this->currentModusPtr = new ManualControl(this->driveModiParams);
}
break;
case Modi::CaptureRoute: {
this->currentModusPtr = new CaptureRoute(this->moveControl, this->input, this->navigation);
this->currentModusPtr = new CaptureRoute(this->driveModiParams, this->navigation);
}
break;
case Modi::Autopilot: {
this->currentModusPtr = new Autopilot(this->moveControl, this->input, this->navigation);
this->currentModusPtr = new Autopilot(this->driveModiParams, this->navigation);
}
break;
case Modi::ConsolControl:
this->currentModusPtr = nullptr;
break;
case Modi::TestMode: {
this->currentModusPtr = new TestMode(this->moveControl, this->navigation);
this->currentModusPtr = new TestMode(driveModiParams, this->navigation);
}
break;
@@ -88,3 +73,13 @@ void DriveManager::changeModus(Modi modus) {
this->addChildComponent(this->currentModusPtr);
}
void DriveManager::init(bool wifi) {
this->navigation = new Navigation();
if (wifi)
this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD);
this->addChildComponent(this->driveModiParams.moveControl);
this->addChildComponent(this->navigation);
this->activateOnlyChilds();
}
src/driveModi/driveModi.cpp
+16 -3
View File
@@ -1,9 +1,17 @@
#include "driveModi.h"
DriveModi::DriveModi(MoveControl* moveControl) {
DriveModi::DriveModi(MoveControl* moveControl, ControlPadInput* input, SensorData* sensorData) {
this->moveControl = moveControl;
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
this->loopDelay = 40;
this->input = input;
this->sensorData = sensorData;
this->init();
}
DriveModi::DriveModi(DriveModiParams params){
this->moveControl = params.moveControl;
this->input = params.input;
this->sensorData = params.sensorData;
this->init();
}
DriveModi::~DriveModi() {
@@ -11,3 +19,8 @@ DriveModi::~DriveModi() {
this->moveControl->setRotationSpeed(0);
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
}
void DriveModi::init() {
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
this->loopDelay = 40;
}
src/driveModi/driveModi.h
+16 -1
View File
@@ -14,6 +14,14 @@
#include "component.h"
#include "moveControl.h"
#include "controlPadInput.h"
#include "sensorData.h"
struct DriveModiParams {
MoveControl* moveControl;
ControlPadInput* input;
SensorData* sensorData;
};
/**
* @brief Baseclass to build DriveModi
@@ -23,9 +31,12 @@
*/
class DriveModi : public Component {
public:
DriveModi(MoveControl* moveControl);
DriveModi(MoveControl* moveControl, ControlPadInput* input, SensorData* sensorData);
DriveModi(DriveModiParams);
virtual ~DriveModi();
SensorData getSensorData() const { return this->sensorData; }
/**
* @brief Set the max speed
*
@@ -48,8 +59,12 @@ class DriveModi : public Component {
protected:
MoveControl *moveControl;
ControlPadInput* input;
SensorData* sensorData;
double maxForwardSpeed = 1;
double maxRotationSpeed = 7;
private:
void init();
};
#endif // DRIVEMODI_H
src/main.cpp
+10 -1
View File
@@ -31,6 +31,8 @@
#include "network.h"
#include "debugMqtt.h"
#include "battery.h"
#include "sensors.h"
#include "sensorData.h"
#include "debugTimes.h"
#include "controlPad.h"
@@ -55,6 +57,8 @@ OutputBuf* outputBuf;
DebugMqtt* debugMqtt = nullptr;
Battery* mainBattery;
SPIClass* spiPort;
Sensors* sensors;
SensorData* sensorData;
ControlPad* controlPad;
bool wifiIsActive;
@@ -115,7 +119,11 @@ void setup() {
std::cout << "Build timestamp: " << BUILD_TIMESTAMP << std::endl;
std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl;
driveManager = new DriveManager(&moveController, spiPort, controlPad->getControlPadDataPtr(), wifiIsActive);
sensors = new Sensors();
sensors->enableGnss(spiPort, PinNumbers::gnssSpiCs);
sensors->enableCompass();
sensorData = sensors->getSensorData();
driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr(), wifiIsActive);
outputBuf->activateMqtt(true);
@@ -142,6 +150,7 @@ void loop() {
#endif //MQTT
wifiTime.stopConsol("WiFi-Time", 10);
}
sensors->loop();
driveManager->loop();
controlPad->loop();
main_m->update();