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

compilation works again

Browse Source
This commit is contained in:
kleiax
2023-09-04 13:30:34 +02:00
parent 5f650982a7
commit 6a76470f45
23 changed files with 285 additions and 321 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/CalcAzimuth/calcAzimuth.cpp
+15 -15
View File
@@ -21,7 +21,7 @@ void CalcAzimuth::drivingDirectionChange(Point point) {
if (point.isInit() && point.isValid()) {
this->directionChangeMode = true;
this->lastChangePoint = point;
this->state = CalcAzimuthState::Invalid;
this->state = State::Invalid;
}
}
@@ -30,9 +30,9 @@ void CalcAzimuth::updateCurrentPosition(Point point) {
this->positionChanged = true;
}
void CalcAzimuth::CalcAzimuth::run() {
void CalcAzimuth::run() {
if (!this->positionChanged)
return
return;
this->positionChanged = false;
this->updateAzimuth();
@@ -42,47 +42,47 @@ void CalcAzimuth::updateAzimuth() {
if (!this->directionChangeMode
|| this->lastChangePoint.distanceTo(this->currentPosition) < 1.0)
{
this->state = CalcAzimuthState::Invalid;
this->state = State::Invalid;
this->calcAzimuth = 999;
return;
}
this->calcAzimuth = this->lastChangePoint.courseTo(this->currentPosition);
// Map point accuracy to CalcAzimuthState
// Map point accuracy to State
if (this->lastChangePoint.getAccuracy() == Point::Accuracy::oneDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::oneDigOfCM)
{
this->state = CalcAzimuthState::Good;
this->state = State::Good;
}
else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::twoDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::twoDigOfCM)
{
this->state = CalcAzimuthState::Ok;
this->state = State::Ok;
}
else if (this->lastChangePoint.getAccuracy() == Point::Accuracy::threeDigOfCM
|| this->currentPosition.getAccuracy() == Point::Accuracy::threeDigOfCM)
{
this->state = CalcAzimuthState::Bad;
this->state = State::Bad;
}
else
{
this->state = CalcAzimuthState::Invalid;
this->state = State::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;
case State::Bad :
this->state = State::Ok;
break;
case CalcAzimuthState::Ok :
this->state = CalcAzimuthState::Good;
case State::Ok :
this->state = State::Good;
break;
case CalcAzimuthState::Good :
this->state = CalcAzimuthState::Super;
case State::Good :
this->state = State::Super;
break;
default:
lib/CalcAzimuth/calcAzimuth.h
+3 -3
View File
@@ -17,7 +17,7 @@
class CalcAzimuth : public Component {
public:
enum CalcAzimuthState {
enum State {
Invalid,
Bad,
Ok,
@@ -32,13 +32,13 @@ class CalcAzimuth : public Component {
void disableCalcAzimuth() { this->directionChangeMode = false; }
int16_t getAzimuth() const { return this->calcAzimuth; }
CalcAzimuthState getState() const { return this->state; }
State getState() const { return this->state; }
private:
void run() override;
void updateAzimuth();
CalcAzimuthState state = CalcAzimuthState::Invalid;
State state = State::Invalid;
Point lastChangePoint;
Point currentPosition;
lib/ControllPad/controlPad.h
+1 -1
View File
@@ -24,7 +24,7 @@ class ControlPad : public Component {
void setMenuControl(MenuControl* menuControl) { this->menuControl = menuControl; }
const ControlPadInput * getControlPadDataPtr() const { return &this->controlInput; }
const ControlPadInput* getControlPadDataPtr() const { return &this->controlInput; }
bool isControlPadConnected() const { return this->connected; }
lib/Navigation/navigation.cpp
+17 -37
View File
@@ -11,18 +11,12 @@
#include "navigation.h"
bool Navigation::outputStatusPrintPVTdata = false;
bool Navigation::newData = false;
uint32_t Navigation::ubxUpdateTimeStatic = 0;
UBX_NAV_PVT_data_t* Navigation::ubxDataStatic = nullptr;
Navigation::Navigation(Route* route) {
Navigation::Navigation(const SensorData* sensorData, Route* route) {
this->sensorData = sensorData;
this->init(route);
}
void Navigation::init(Route* route) {
if (route)
this->route = route;
else
@@ -33,23 +27,9 @@ void Navigation::init(Route* route) {
Navigation::~Navigation() {
delete this->route;
if (this->ntripClient)
delete this->ntripClient;
}
void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String user, String password) {
this->ntripClient = new NTRIPClient(this->gps, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str());
this->ntripClient->gpsConfiguration();
this->ntripClient->loop();
this->ntripClient->setActivated(false);
this->isNtripInit = true;
this->addChildComponent(this->ntripClient);
}
void Navigation::run() {
this->updateMagneticDeclination();
}
void Navigation::run() {}
void Navigation::newRoute() {
if (this->route)
@@ -119,31 +99,31 @@ Navigation::Status Navigation::addCurrentPosToRoute() {
return Status::Unchanged;
}
void Navigation::updateCurrentLocation() {
if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime)
return;
// void Navigation::updateCurrentLocation() {
// if (Navigation::ubxUpdateTimeStatic == this->ubxUpdateTime)
// return;
this->ubxData = Navigation::ubxDataStatic;
this->ubxUpdateTime = Navigation::ubxUpdateTimeStatic;
// this->ubxData = Navigation::ubxDataStatic;
// this->ubxUpdateTime = Navigation::ubxUpdateTimeStatic;
Point::Coordinates coords;
coords.lat = this->ubxData->lat / 10000000.0;
coords.lon = this->ubxData->lon / 10000000.0;
// Point::Coordinates coords;
// coords.lat = this->ubxData->lat / 10000000.0;
// coords.lon = this->ubxData->lon / 10000000.0;
this->currentPosition = Point(coords, this->ubxData->hAcc);
}
// this->currentPosition = Point(coords, this->ubxData->hAcc);
// }
int16_t Navigation::calculateCourseCorrection(Point& point) {
int16_t targetCourse = point.courseTo(this->targetPoint);
int16_t correctionCourse;
if (this->calcAzimuthState == CalcAzimuthState::Good
|| this->calcAzimuthState == CalcAzimuthState::Super)
if (this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Good
|| this->sensorData->getCalcAzimuthState() == CalcAzimuth::State::Super)
{
correctionCourse = targetCourse - this->calcAzimuth;
correctionCourse = targetCourse - this->sensorData->getCalcAzimuth();
this->lastUsedCalcAzimuth = true;
} else {
correctionCourse = targetCourse - this->realAzimuth;
correctionCourse = targetCourse - this->sensorData->getRealAzimuth();
this->lastUsedCalcAzimuth = false;
}
lib/Navigation/navigation.h
+4 -42
View File
@@ -16,8 +16,7 @@
#include <iostream>
#include "route.h"
#include "NTRIPClient.h"
#include "sensorData.h"
#include "component.h"
/**
@@ -54,7 +53,7 @@ class Navigation : public Component {
*
* @param route with which to navigate
*/
Navigation(Route* route = nullptr);
Navigation(const SensorData* sensorData, Route* route = nullptr);
/**
* @brief Destroy the Navigation object
@@ -62,17 +61,6 @@ class Navigation : public Component {
*/
~Navigation();
/**
* @brief
*
* @param host
* @param port
* @param mountPoint
* @param user
* @param password
*/
void initNtrip(String host, uint16_t port, String mountPoint, String user, String password);
/**
* @brief creates a new empty route
*
@@ -118,22 +106,6 @@ class Navigation : public Component {
* @return false no point added to route
*/
Status addCurrentPosToRoute();
/**
* @brief Get the Ubx Data object
*
* This struct includes the most Data from the GNSS-Module.
*
* @return UBX_NAV_PVT_data_t*
*/
UBX_NAV_PVT_data_t* getUbxData() { return this->ubxData; }
/**
* @brief Returns the NTRIPClient object
*
* @return NTRIPClient*
*/
NTRIPClient* getNTRIPClient() { return this->ntripClient; }
/**
* @brief Get the Route Info object
@@ -163,12 +135,9 @@ class Navigation : public Component {
static constexpr uint8_t loopDelay = 20;
static constexpr uint8_t maxDisBetweenPoints = 10;
static constexpr float minDisBetweenPoints = 0.3;
UBX_NAV_PVT_data_t* ubxData = nullptr;
Route* route = nullptr;
NTRIPClient* ntripClient = nullptr;
const SensorData* sensorData;
Route* route = nullptr;
Point lastPointRouteInsert;
Point lastPointCalcCorrection;
@@ -177,7 +146,6 @@ class Navigation : public Component {
Point currentPosition;
Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM;
bool navigationStarted = false;
bool navigationFinished = false;
bool isNtripInit = false;
@@ -185,13 +153,7 @@ class Navigation : public Component {
bool directionChangeMode = false;
bool lastUsedCalcAzimuth = false;
char* host;
char* mountPoint;
char* user;
char* password;
uint8_t timeToWait = 200;
uint16_t port;
uint32_t lastMillis = 0;
uint32_t ubxUpdateTime = 0;
lib/Point/point.cpp
+2
View File
@@ -9,6 +9,8 @@
*
*/
#include "point.h"
Point::Point(double lat, double lon, uint32_t horizontalAccuracy, uint32_t creationTime) {
this->coordinates.lat = lat;
this->coordinates.lon = lon;
lib/Sensors/senors.cpp → lib/Sensors/sensorData.cpp
+74 -41
View File
@@ -9,13 +9,32 @@
*
*/
#include "sensors.h"
#include "sensorData.h"
Sensors::Sensors() {
this->sensorData = new SensorData(this);
bool SensorData::outputStatusPrintPVTdata = false;
bool SensorData::newData = false;
uint32_t SensorData::ubxUpdateTimeStatic = 0;
UBX_NAV_PVT_data_t* SensorData::ubxDataStatic = nullptr;
SensorData::SensorData() {
this->loopDelay = 50;
}
void Sensors::enableGnss(SPIClass* spiPort, uint8_t csPin) {
SensorData::~SensorData() {
if (this->ntripClient)
delete this->ntripClient;
}
void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, String user, String password) {
this->ntripClient = new NTRIPClient(this->gnss, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str());
this->ntripClient->gpsConfiguration();
this->ntripClient->loop();
this->ntripClient->setActivated(false);
this->isNtripInit = true;
this->addChildComponent(this->ntripClient);
}
void SensorData::enableGnss(SPIClass* spiPort, uint8_t csPin) {
this->gnss = new SFE_UBLOX_GNSS();
if (this->gnss->begin(*spiPort, csPin, 4000000) == false) {
std::cout << "u-blox GNSS not detected on SPI bus. Please check wiring. Freezing." << std::endl;
@@ -24,7 +43,7 @@ void Sensors::enableGnss(SPIClass* spiPort, uint8_t csPin) {
this->initGnss();
}
void Sensors::enableGnss() {
void SensorData::enableGnss() {
this->gnss = new SFE_UBLOX_GNSS();
if (this->gnss->begin() == false) {
std::cout << "u-blox GNSS not detected at default I2C address. Please check wiring. Freezing." << std::endl;
@@ -33,52 +52,35 @@ void Sensors::enableGnss() {
this->initGnss();
}
void Sensors::enableCompass() {
this->compass = new QMC5883LCompass();
void SensorData::enableRealCompass() {
this->realCompass = new QMC5883LCompass();
// Init Compass
Wire.beginTransmission(0x0d);
Wire.write(0x0b);
Wire.write(0x01);
Wire.endTransmission();
this->compass->setMode(0x01,0x0C,0x10,0X00);
CalibrateCompass caliCompass(this->compass);
this->realCompass->setMode(0x01,0x0C,0x10,0X00);
CalibrateCompass caliCompass(this->realCompass);
caliCompass.loadData();
caliCompass.useData();
}
void Sensors::setOutputStatusPrintPVTdata(bool status) {
Sensors::outputStatusPrintPVTdata = status;
void SensorData::enableCalcCompass() {
}
void Sensors::run() {
this->compass->read();
this->realAzimuth = this->compass->getAzimuth();
void SensorData::enableGyroskop() {
}
void Sensors::runAsChild() {
this->gnss->checkUblox();
this->gnss->checkCallbacks();
if (Sensors::newData)
Sensors::newData = false;
CalcAzimuth::State SensorData::getCalcAzimuthState() const {
if (this->calcCompass)
return this->calcCompass->getState();
return CalcAzimuth::State::Invalid;
}
void Sensors::initGnss() {
uint8_t versionHigh = this->gnss->getProtocolVersionHigh();
uint8_t versionLow = this->gnss->getProtocolVersionLow();
std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;
this->gnss->setSPIOutput(COM_TYPE_UBX);
this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
this->gnss->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
this->gnss->setAutoPVTcallbackPtr(&(Sensors::savePVTdata));
// Sensors::setOutputStatusPrintPVTdata(true);
this->gnss->setNavigationFrequency(1);
this->gnss->setAutoPVT(true);
}
void Sensors::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
if (!Sensors::outputStatusPrintPVTdata)
void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
if (!SensorData::outputStatusPrintPVTdata)
return;
double latitude = (double) ubxDataStruct->lat / 10000000.0;
@@ -124,10 +126,41 @@ void Sensors::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
<< " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl;
}
void Sensors::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
Sensors::printPVTdata(ubxDataStruct);
void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
SensorData::printPVTdata(ubxDataStruct);
Sensors::newData = true;
Sensors::ubxDataStatic = ubxDataStruct;
Sensors::ubxUpdateTimeStatic = millis();
SensorData::newData = true;
SensorData::ubxDataStatic = ubxDataStruct;
SensorData::ubxUpdateTimeStatic = millis();
}
void SensorData::setOutputStatusPrintPVTdata(bool status) {
SensorData::outputStatusPrintPVTdata = status;
}
void SensorData::run() {
this->realCompass->read();
this->realAzimuth = this->realCompass->getAzimuth();
}
void SensorData::runAsChild() {
this->gnss->checkUblox();
this->gnss->checkCallbacks();
if (SensorData::newData)
SensorData::newData = false;
}
void SensorData::initGnss() {
uint8_t versionHigh = this->gnss->getProtocolVersionHigh();
uint8_t versionLow = this->gnss->getProtocolVersionLow();
std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;
this->gnss->setSPIOutput(COM_TYPE_UBX);
this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
this->gnss->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
this->gnss->setAutoPVTcallbackPtr(&(SensorData::savePVTdata));
// SensorData::setOutputStatusPrintPVTdata(true);
this->gnss->setNavigationFrequency(1);
this->gnss->setAutoPVT(true);
}
lib/Sensors/sensorData.h
+75 -13
View File
@@ -12,29 +12,91 @@
#ifndef SENSOR_DATA_H
#define SENSOR_DATA_H
#include "sensors.h"
#include <SPI.h>
#include <iostream>
#include "component.h"
#include "calibrateCompass.h"
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <QMC5883LCompass.h>
// Gyroskop
#include "calcAzimuth.h"
#include "ntripClient.h"
class SensorData {
#include "point.h"
class Sensors;
class SensorData : public Component {
public:
SensorData(Sensors *senors);
SensorData();
~SensorData();
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;
void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password);
void enableGnss(SPIClass* spiPort, uint8_t csPin);
void enableGnss();
void enableRealCompass();
void enableCalcCompass();
void enableGyroskop();
// Interface Const kram
int16_t getRealAzimuth() const { return this->realAzimuth; }
int16_t getCalcAzimuth() const { return this->calcAzimuth; }
CalcAzimuth::State getCalcAzimuthState() const;
Point getCurrentPos() const { return this->currentPosition; }
const UBX_NAV_PVT_data_t* getGnssData() const { return this->gnssData; };
const void* const getGyroData() const;
CalcAzimuth* getCalcCompass() const { return this->calcCompass; }
QMC5883LCompass* getRealCompass() const { return this->realCompass; }
NTRIPClient* getNtripClient() const { return this->ntripClient; }
// static
/**
* @brief Set the output status for PVTdata.
*
* If this is true, a lot of information from the gnss module will be printed in
* the interval of navigation frequency.
*
* @param status
*/
static void setOutputStatusPrintPVTdata(bool status);
private:
Sensors* sensors;
void run() override;
void runAsChild() override;
void initGnss();
QMC5883LCompass* realCompass = nullptr;
CalcAzimuth* calcCompass = nullptr;
SFE_UBLOX_GNSS* gnss = nullptr;
NTRIPClient* ntripClient = nullptr;
UBX_NAV_PVT_data_t* gnssData;
Point currentPosition;
char* host;
char* mountPoint;
char* user;
char* password;
bool isNtripInit = false;
uint16_t port;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
// static
static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static UBX_NAV_PVT_data_t* ubxDataStatic;
static uint32_t ubxUpdateTimeStatic;
static bool outputStatusPrintPVTdata;
static bool newData;
};
SensorData::SensorData(Sensors* senors) {
this->sensors = sensors;
}
#endif //SENSOR_DATA_H
lib/Sensors/sensors.h
-71
View File
@@ -1,71 +0,0 @@
/**
* @file sensors.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-09-02
*
* @copyright Copyright (c) 2023
*
*/
#ifndef SENSORS_H
#define SENSORS_H
#include <SPI.h>
#include <iostream>
//Sensors
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <QMC5883LCompass.h>
// Gyroskop
#include "component.h"
#include "sensorData.h"
#include "calibrateCompass.h"
class Sensors : public Component {
public:
Sensors();
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.
*
* If this is true, a lot of information from the gnss module will be printed in
* the interval of navigation frequency.
*
* @param status
*/
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;
int16_t realAzimuth = INT16_MAX;
static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static UBX_NAV_PVT_data_t* ubxDataStatic;
static uint32_t ubxUpdateTimeStatic;
static bool outputStatusPrintPVTdata;
static bool newData;
};
#endif // SENSORS_H
src/SpecialMenus/GPS/menuGPS.cpp
+19 -19
View File
@@ -11,14 +11,14 @@
#include "menuGPS.h"
MenuGPS::MenuGPS(DriveManager* driveManager) :
MenuGPS::MenuGPS(SensorData* sensorData) :
MenuInformationSites(10) {
this->navigation = driveManager->getNavigation();
this->ntripClient = this->navigation->getNTRIPClient();
this->sensorData = sensorData;
// this->ntripClient = this->navigation->getNTRIPClient();
}
void MenuGPS::printPage() const {
UBX_NAV_PVT_data_t* gpsData = this->navigation->getUbxData();
const UBX_NAV_PVT_data_t* gpsData = this->sensorData->getGnssData();
uint8_t fixType = 0;
if (gpsData)
fixType = gpsData->fixType;
@@ -66,15 +66,15 @@ void MenuGPS::printPage() const {
break;
case 3: {
NTRIPClientStates status = this->ntripClient->getClientState();
lineOne = "NTRIP Client is";
// NTRIPClientStates status = this->ntripClient->getClientState();
// lineOne = "NTRIP Client is";
if (status == NTRIPClientStates::pushData)
lineTwo = "enabled";
else if (status == NTRIPClientStates::notAvailable)
lineTwo = "not available";
else
lineTwo = "disabled";
// if (status == NTRIPClientStates::pushData)
// lineTwo = "enabled";
// else if (status == NTRIPClientStates::notAvailable)
// lineTwo = "not available";
// else
// lineTwo = "disabled";
}
break;
@@ -134,7 +134,7 @@ void MenuGPS::printPage() const {
case 8:
lineOne = "Azimuth: ";
lineTwo = "";
lineTwo.concat(this->navigation->getAzimuth());
lineTwo.concat(this->sensorData->getRealAzimuth());
break;
default:
@@ -148,12 +148,12 @@ void MenuGPS::printPage() const {
void MenuGPS::runCommand() const {
switch (this->getCurrentPage()) {
case 3: {
// std::cout << "MenuGPS::runCommand " << this->ntripClient->getClientState() << std::endl;
if (this->ntripClient->getClientState() == NTRIPClientStates::pushData)
this->ntripClient->setActivated(false);
else if (this->ntripClient->getClientState() != NTRIPClientStates::notAvailable)
this->ntripClient->setActivated(true);
break;
// // std::cout << "MenuGPS::runCommand " << this->ntripClient->getClientState() << std::endl;
// if (this->ntripClient->getClientState() == NTRIPClientStates::pushData)
// this->ntripClient->setActivated(false);
// else if (this->ntripClient->getClientState() != NTRIPClientStates::notAvailable)
// this->ntripClient->setActivated(true);
}
break;
}
}
src/SpecialMenus/GPS/menuGPS.h
+4 -5
View File
@@ -15,8 +15,7 @@
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include "menuInformationSites.h"
#include "driveModi/driveManager.h"
#include "navigation.h"
#include "sensorData.h"
#include "ntripClient.h"
/**
@@ -30,14 +29,14 @@ class MenuGPS : public MenuInformationSites {
*
* @param gps
*/
MenuGPS(DriveManager* driveManager);
MenuGPS(SensorData* sensorData);
private:
void printPage() const override;
void runCommand() const override;
NTRIPClient* ntripClient;
Navigation* navigation;
// NTRIPClient* ntripClient;
SensorData* sensorData;
};
#endif // MENU_GPS_H
src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+12 -12
View File
@@ -13,7 +13,7 @@
void MenuAutopilot::printPage() const {
CourseCorrection correction = this->autopilot->getCourseCorrection();
UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
const UBX_NAV_PVT_data_t* gpsData = this->autopilot->getSensorData()->getGnssData();
bool navigationStarted = this->autopilot->getState() >= Autopilot::State::NavigationStarted;
String distanceString = "";
@@ -104,7 +104,7 @@ void MenuAutopilot::printPage() const {
break;
case 3: {
NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
NTRIPClientStates status = this->autopilot->getSensorData()->getNtripClient()->getClientState();
uint8_t carrSoln = gpsData->flags.bits.carrSoln;
lineOne = "GNSS: ";
@@ -132,30 +132,30 @@ void MenuAutopilot::printPage() const {
case 4:
if (this->autopilot->getSensorData().getCalcAzimuth() == CalcAzimuth::CalcAzimuthState::Good
||this->autopilot->getSensorData().getCalcAzimuth() == CalcAzimuth::CalcAzimuthState::Super)
if (this->autopilot->getSensorData()->getCalcAzimuth() == CalcAzimuth::State::Good
||this->autopilot->getSensorData()->getCalcAzimuth() == CalcAzimuth::State::Super)
{
lineOne = "Calc Azi: ";
lineTwo = "State: ";
lineOne.concat(this->autopilot->getSensorData().getCalcAzimuth());
switch (this->autopilot->getSensorData().getCalcAzimuthState()) {
case CalcAzimuth::CalcAzimuthState::Bad :
lineOne.concat(this->autopilot->getSensorData()->getCalcAzimuth());
switch (this->autopilot->getSensorData()->getCalcAzimuthState()) {
case CalcAzimuth::State::Bad :
lineTwo.concat("Bad");
break;
case CalcAzimuth::CalcAzimuthState::Good :
case CalcAzimuth::State::Good :
lineTwo.concat("Good");
break;
case CalcAzimuth::CalcAzimuthState::Invalid :
case CalcAzimuth::State::Invalid :
lineTwo.concat("Invalid");
break;
case CalcAzimuth::CalcAzimuthState::Ok :
case CalcAzimuth::State::Ok :
lineTwo.concat("Ok");
break;
case CalcAzimuth::CalcAzimuthState::Super :
case CalcAzimuth::State::Super :
lineTwo.concat("Super");
break;
@@ -166,7 +166,7 @@ void MenuAutopilot::printPage() const {
} else {
lineOne = "Real Azi: ";
lineTwo = "";
lineOne.concat(this->driveManager->getNavigation()->getAzimuth());
lineOne.concat(this->autopilot->getSensorData()->getRealAzimuth());
}
break;
src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.cpp
+8 -9
View File
@@ -10,13 +10,11 @@
*/
#include "menuCalibrateCompass.h"
MenuCalibrateCompass::MenuCalibrateCompass(DriveManager* driveManager) : MenuDriveMode(driveManager) {
this->caliCompass = new CalibrateCompass(this->driveManager->getNavigation()->getCompass());
}
MenuCalibrateCompass::MenuCalibrateCompass(DriveManager* driveManager) : MenuDriveMode(driveManager) {}
MenuCalibrateCompass::~MenuCalibrateCompass() {
delete this->caliCompass;
this->manualControl->setCalibrateCompass();
this->caliCompassMode->setCalibrateCompass();
}
void MenuCalibrateCompass::printPage() const {
@@ -31,7 +29,7 @@ void MenuCalibrateCompass::printPage() const {
case 1:
lineOne = "Azimuth:";
lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
lineTwo.concat(this->caliCompassMode->getSensorData()->getRealAzimuth());
break;
case 2:
@@ -112,8 +110,9 @@ void MenuCalibrateCompass::printPage() const {
void MenuCalibrateCompass::init() {
this->firstPrint = false;
this->driveManager->changeModus(Modi::ManualControl);
this->manualControl = (ManualControl*) this->driveManager->getDriveModiPtr();
this->driveManager->changeModus(Modi::CalibrateCompass);
this->caliCompassMode = (CalibrateCompassM*) this->driveManager->getDriveModiPtr();
this->caliCompass = new CalibrateCompass(this->caliCompassMode->getSensorData()->getRealCompass());
this->setCountPages(11);
this->updateDelay = 500;
}
@@ -123,12 +122,12 @@ void MenuCalibrateCompass::runCommand() const {
case 2:
switch (this->caliCompass->getState()) {
case CalibrateCompass::State::Ready :
this->manualControl->setCalibrateCompass(this->caliCompass);
this->caliCompassMode->setCalibrateCompass(this->caliCompass);
this->caliCompass->start();
break;
case CalibrateCompass::State::Finished:
this->manualControl->setCalibrateCompass();
this->caliCompassMode->setCalibrateCompass();
this->caliCompass->useData();
break;
src/SpecialMenus/driveModi/CalibrateCompass/menuCalibrateCompass.h
+1 -1
View File
@@ -13,7 +13,7 @@
#define MENU_CALIBRATE_COMPASS_H
#include "SpecialMenus/driveModi/menuDriveMode.h"
#include ""
#include "driveModi/Modi/CalibrateCompass/calibrateCompassM.h"
#include "calibrateCompass.h"
/**
src/SpecialMenus/driveModi/CaptureRoute/menuCaptureRoute.cpp
+3 -3
View File
@@ -12,7 +12,7 @@
void MenuCaptureRoute::printPage() const {
RouteInfo routeInfo = this->captureRoute->getRouteInfo();
UBX_NAV_PVT_data_t* gpsData = this->driveManager->getNavigation()->getUbxData();
const UBX_NAV_PVT_data_t* gpsData = this->captureRoute->getSensorData()->getGnssData();
String lineOne = "";
String lineTwo = "";
@@ -56,7 +56,7 @@ void MenuCaptureRoute::printPage() const {
break;
case 4: {
NTRIPClientStates status = this->driveManager->getNavigation()->getNTRIPClient()->getClientState();
NTRIPClientStates status = this->captureRoute->getSensorData()->getNtripClient()->getClientState();
lineOne = "NTRIP Client is";
if (status == NTRIPClientStates::pushData)
@@ -85,7 +85,7 @@ void MenuCaptureRoute::printPage() const {
case 6:
lineOne = "hAccuracy: ";
lineTwo = "Azimuth: ";
lineTwo.concat(this->driveManager->getNavigation()->getAzimuth());
lineTwo.concat(this->captureRoute->getSensorData()->getRealAzimuth());
if (gpsData->fixType)
lineOne.concat(gpsData->hAcc);
else
src/driveModi/Modi/Autopilot/autopilot.cpp
+12 -12
View File
@@ -12,24 +12,24 @@
#include "driveModi/Modi/Autopilot/autopilot.h"
#include "autopilot.h"
DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) {
this->navigation = navigation;
DirectionChangeSignal::DirectionChangeSignal(Autopilot* pilot) {
this->pilot = pilot;
this->action();
}
DirectionChangeSignal::~DirectionChangeSignal() {
navigation->disableCalcAzimuth();
pilot->getSensorData()->getCalcCompass()->disableCalcAzimuth();
}
void DirectionChangeSignal::action() {
this->navigation->drivingDirectionChange();
pilot->getSensorData()->getCalcCompass()->drivingDirectionChange(pilot->getSensorData()->getCurrentPos());
}
Autopilot::Autopilot(DriveModiParams params, Navigation* navigation)
: ManualControl(params) {
this->setInputMode(ManualControl::InputMode::Digital);
this->directionChangeSignal = new DirectionChangeSignal(navigation);
this->directionChangeSignal = new DirectionChangeSignal(this);
this->setDirectionChangeCallback(this->directionChangeSignal);
this->navigation = navigation;
this->init();
@@ -119,7 +119,7 @@ void Autopilot::init() {
else
this->state = State::NoRoute;
this->routeInfo = this->navigation->getRouteInfo();
this->navigation->getNTRIPClient()->setAutoReconnect(true);
this->sensorData->getNtripClient()->setAutoReconnect(true);
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
@@ -143,7 +143,7 @@ void Autopilot::beginRotate() {
if (this->state != State::SelfDrivingRotate) {
this->lastState = this->state;
this->state = State::SelfDrivingRotate;
this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction;
this->rotationAimAzimuth = this->getSensorData()->getRealAzimuth() + this->courseCorrection.correction;
this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth);
this->moveControl->setSpeed(0);
@@ -155,17 +155,17 @@ void Autopilot::beginRotate() {
}
void Autopilot::rotate() {
if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) {
if (abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) {
this->endRotate();
return;
}
if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3)
if ((abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3)
&&
((this->courseCorrection.correction > 0
&& this->rotationAimAzimuth < this->navigation->getAzimuth())
&& this->rotationAimAzimuth < this->getSensorData()->getRealAzimuth())
|| (this->courseCorrection.correction < 0
&& this->rotationAimAzimuth > this->navigation->getAzimuth())))
&& this->rotationAimAzimuth > this->getSensorData()->getRealAzimuth())))
{
this->endRotate();
std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl;
@@ -177,7 +177,7 @@ void Autopilot::endRotate() {
return;
this->state = this->lastState;
this->navigation->drivingDirectionChange();
this->sensorData->getCalcCompass()->drivingDirectionChange(this->sensorData->getCurrentPos());
this->moveControl->setRotationSpeed(0);
this->moveControl->emergencyStop();
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
src/driveModi/Modi/Autopilot/autopilot.h
+11 -9
View File
@@ -16,15 +16,7 @@
#include "navigation.h"
#include "driveModi/Modi/ManualControl/manualControl.h"
class DirectionChangeSignal : public DirectionChangeWrapper {
public:
DirectionChangeSignal(Navigation* navigation);
~DirectionChangeSignal();
void action() override;
private:
Navigation* navigation;
};
class DirectionChangeSignal;
/**
* @brief This class use the navigate class to drive automaticaly
@@ -130,4 +122,14 @@ class Autopilot : public ManualControl {
double minRemainingDistance = 0.25;
};
class DirectionChangeSignal : public DirectionChangeWrapper {
public:
DirectionChangeSignal(Autopilot* pilot);
~DirectionChangeSignal();
void action() override;
private:
Autopilot* pilot;
};
#endif // AUTOPILOT_H
src/driveModi/Modi/CaptureRoute/captureRoute.cpp
+1 -1
View File
@@ -15,7 +15,7 @@ CaptureRoute::CaptureRoute(DriveModiParams params, Navigation* navigation)
: ManualControl(params) {
this->navigation = navigation;
this->navigation->getRoute()->clear();
this->navigation->getNTRIPClient()->setAutoReconnect(true);
this->sensorData->getNtripClient()->setAutoReconnect(true);
this->routeInfo = navigation->getRouteInfo();
}
src/driveModi/Modi/TestMode/testMode.cpp
+2 -2
View File
@@ -21,7 +21,7 @@ TestMode::~TestMode() {
void TestMode::run() {
if (this->maneuver == Maneuver::Turn) {
uint16_t delta = abs(this->azimuth - this->navigation->getAzimuth());
uint16_t delta = abs(this->azimuth - this->getSensorData()->getRealAzimuth());
if (delta > this->degree)
this->abort = true;
}
@@ -50,7 +50,7 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
else if (degree > 0)
this->moveControl->setRotationSpeed(this->maxRotationSpeed);
this->azimuth = this->navigation->getAzimuth();
this->azimuth = this->getSensorData()->getRealAzimuth();
this->degree = degree;
this->maneuverTime = 5 * 1000;
src/driveModi/driveManager.cpp
+6 -8
View File
@@ -11,18 +11,18 @@
#include "driveModi/driveManager.h"
DriveManager::DriveManager(MoveControl *moveControl, SensorData* sensorData, ControlPadInput *input, bool wifi) {
DriveManager::DriveManager(MoveControl *moveControl, const SensorData* sensorData, const ControlPadInput *input) {
this->driveModiParams.input = input;
this->driveModiParams.moveControl = moveControl;
this->driveModiParams.sensorData = sensorData;
this->init(wifi);
this->init();
}
DriveManager::DriveManager(DriveModiParams params, bool wifi){
DriveManager::DriveManager(DriveModiParams params){
this->driveModiParams = params;
this->init(wifi);
this->init();
}
DriveManager::~DriveManager() {
@@ -73,10 +73,8 @@ 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);
void DriveManager::init() {
this->navigation = new Navigation(this->driveModiParams.sensorData);
this->addChildComponent(this->driveModiParams.moveControl);
this->addChildComponent(this->navigation);
src/driveModi/driveManager.h
+4 -3
View File
@@ -38,6 +38,7 @@
enum class Modi {
Off,
ManualControl,
CalibrateCompass,
CaptureRoute,
Autopilot,
TestMode
@@ -52,8 +53,8 @@ class DriveManager : public Component {
*
* @param moveControl
*/
DriveManager(MoveControl *moveControl, SensorData* sensorData, ControlPadInput *input, bool wifi = false);
DriveManager(DriveModiParams params, bool wifi = false);
DriveManager(MoveControl *moveControl, const SensorData* sensorData, const ControlPadInput *input);
DriveManager(DriveModiParams params);
/**
* @brief Destroy the Drive Manager object
@@ -99,7 +100,7 @@ class DriveManager : public Component {
private:
void run() override {};
void init(bool wifi);
void init();
Modi currentModus = Modi::Off;
DriveModi *currentModusPtr = nullptr;
src/driveModi/driveModi.h
+5 -5
View File
@@ -19,8 +19,8 @@
struct DriveModiParams {
MoveControl* moveControl;
ControlPadInput* input;
SensorData* sensorData;
const ControlPadInput* input;
const SensorData* sensorData;
};
/**
@@ -35,7 +35,7 @@ class DriveModi : public Component {
DriveModi(DriveModiParams);
virtual ~DriveModi();
SensorData getSensorData() const { return this->sensorData; }
const SensorData* getSensorData() const { return this->sensorData; }
/**
* @brief Set the max speed
@@ -59,8 +59,8 @@ class DriveModi : public Component {
protected:
MoveControl *moveControl;
ControlPadInput* input;
SensorData* sensorData;
const ControlPadInput* input;
const SensorData* sensorData;
double maxForwardSpeed = 1;
double maxRotationSpeed = 7;
src/main.cpp
+6 -9
View File
@@ -31,7 +31,6 @@
#include "network.h"
#include "debugMqtt.h"
#include "battery.h"
#include "sensors.h"
#include "sensorData.h"
#include "debugTimes.h"
#include "controlPad.h"
@@ -57,7 +56,6 @@ OutputBuf* outputBuf;
DebugMqtt* debugMqtt = nullptr;
Battery* mainBattery;
SPIClass* spiPort;
Sensors* sensors;
SensorData* sensorData;
ControlPad* controlPad;
@@ -119,11 +117,10 @@ void setup() {
std::cout << "Build timestamp: " << BUILD_TIMESTAMP << std::endl;
std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl;
sensors = new Sensors();
sensors->enableGnss(spiPort, PinNumbers::gnssSpiCs);
sensors->enableCompass();
sensorData = sensors->getSensorData();
driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr(), wifiIsActive);
sensorData = new SensorData();
sensorData->enableGnss(spiPort, PinNumbers::gnssSpiCs);
sensorData->enableRealCompass();
driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr());
outputBuf->activateMqtt(true);
@@ -150,7 +147,7 @@ void loop() {
#endif //MQTT
wifiTime.stopConsol("WiFi-Time", 10);
}
sensors->loop();
sensorData->loop();
driveManager->loop();
controlPad->loop();
main_m->update();
@@ -225,7 +222,7 @@ void makeMenu() {
MenuAutopilot* auto_m = new MenuAutopilot(driveManager);
MenuTestMode* testM_m = new MenuTestMode(driveManager);
MenuCalibrateCompass* comp_m = new MenuCalibrateCompass(driveManager);
MenuGPS* gps_m = new MenuGPS(driveManager);
MenuGPS* gps_m = new MenuGPS(sensorData);
MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery);
MenuRoute* rout_m = new MenuRoute(driveManager->getNavigation()->getRoute());