From b6764e54da9fdb231ab2d7d2c95f9b806f8fa513 Mon Sep 17 00:00:00 2001 From: Alexander Klein Date: Sat, 2 Sep 2023 18:30:33 +0200 Subject: [PATCH] - broke nearly everthing - create Sensor Manager --- doc/TODO allgemein.txt | 1 + lib/Navigation/navigation.cpp | 111 +---------------------------- lib/Navigation/navigation.h | 52 ++------------ lib/Sensors/senors.cpp | 129 ++++++++++++++++++++++++++++++++++ lib/Sensors/sensorData.h | 0 lib/Sensors/sensors.h | 77 ++++++++++++++++++++ 6 files changed, 212 insertions(+), 158 deletions(-) create mode 100644 lib/Sensors/senors.cpp create mode 100644 lib/Sensors/sensorData.h create mode 100644 lib/Sensors/sensors.h diff --git a/doc/TODO allgemein.txt b/doc/TODO allgemein.txt index 154a254..443a7b0 100644 --- a/doc/TODO allgemein.txt +++ b/doc/TODO allgemein.txt @@ -34,6 +34,7 @@ Code: 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 Latex: Genaue Beschreibung von PulseCounter HardwareUnit wenn möglich diff --git a/lib/Navigation/navigation.cpp b/lib/Navigation/navigation.cpp index 4f16f11..5c505a4 100644 --- a/lib/Navigation/navigation.cpp +++ b/lib/Navigation/navigation.cpp @@ -17,58 +17,21 @@ uint32_t Navigation::ubxUpdateTimeStatic = 0; UBX_NAV_PVT_data_t* Navigation::ubxDataStatic = nullptr; Navigation::Navigation(Route* route) { - this->gps = new SFE_UBLOX_GNSS(); - if (this->gps->begin() == false) { - std::cout << "u-blox GNSS not detected at default I2C address. Please check wiring. Freezing." << std::endl; - while (1); - } - this->init(route); -} - -Navigation::Navigation(SPIClass* spiPort, uint8_t csPin, Route* route) { - this->gps = new SFE_UBLOX_GNSS(); - if (this->gps->begin(*spiPort, csPin, 4000000) == false) { - std::cout << "u-blox GNSS not detected on SPI bus. Please check wiring. Freezing." << std::endl; - while (1); - } this->init(route); } void Navigation::init(Route* route) { - uint8_t versionHigh = this->gps->getProtocolVersionHigh(); - uint8_t versionLow = this->gps->getProtocolVersionLow(); - std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl; - - this->gps->setSPIOutput(COM_TYPE_UBX); - this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10); - this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA); - this->gps->setAutoPVTcallbackPtr(&(Navigation::savePVTdata)); - // Navigation::setOutputStatusPrintPVTdata(true); - this->gps->setNavigationFrequency(1); - this->gps->setAutoPVT(true); + if (route) this->route = route; else this->route = new Route(); - this->compass = 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); - caliCompass.loadData(); - caliCompass.useData(); - Component::loopDelay = Navigation::loopDelay; } Navigation::~Navigation() { - delete this->gps; - delete this->compass; delete this->route; if (this->ntripClient) delete this->ntripClient; @@ -84,21 +47,9 @@ void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String } void Navigation::run() { - this->compass->read(); - this->realAzimuth = this->compass->getAzimuth(); - this->updateMagneticDeclination(); } -void Navigation::runAsChild() { - this->gps->checkUblox(); - this->gps->checkCallbacks(); - - if (Navigation::newData) { - this->updateCurrentLocation(); - Navigation::newData = false; - } -} void Navigation::newRoute() { if (this->route) @@ -276,10 +227,6 @@ bool Navigation::setTargetPoint(Point target) { return false; } -void Navigation::setOutputStatusPrintPVTdata(bool status) { - Navigation::outputStatusPrintPVTdata = status; -} - int16_t Navigation::fixDegree(int16_t degree) { while (degree < -180 || degree > 180) { if (degree > 180) @@ -289,59 +236,3 @@ int16_t Navigation::fixDegree(int16_t degree) { } return degree; } - -void Navigation::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { - if (!Navigation::outputStatusPrintPVTdata) - return; - - double latitude = (double) ubxDataStruct->lat / 10000000.0; - double longitude = (double) ubxDataStruct->lon / 10000000.0; - double altitude = (double) ubxDataStruct->hMSL / 1000.0; - - uint8_t fixType = ubxDataStruct->fixType; - char fixTypeString[32]; - if (fixType == 0) - strcpy(fixTypeString, "None"); - else if (fixType == 1) - strcpy(fixTypeString, "Dead Reckoning"); - else if (fixType == 2) - strcpy(fixTypeString, "2D"); - else if (fixType == 3) - strcpy(fixTypeString, "3D"); - else if (fixType == 3) - strcpy(fixTypeString, "GNSS + Dead Reckoning"); - else if (fixType == 5) - strcpy(fixTypeString, "Time Only"); - else - strcpy(fixTypeString, "UNKNOWN"); - - uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln; - char carrSolnString[16]; - if (carrSoln == 0) - strcpy(carrSolnString, "None"); - else if (carrSoln == 1) - strcpy(carrSolnString, "Floating"); - else if (carrSoln == 2) - strcpy(carrSolnString, "Fixed"); - else - strcpy(carrSolnString, "UNKNOWN"); - - uint32_t hAcc = ubxDataStruct->hAcc; - - std::cout << "Lat: " << latitude - << " Lng: " << longitude - << " Alt: " << altitude << std::endl; - - std::cout << "Fix: " << fixTypeString - << " Carrier Solution: " << carrSolnString - << " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl; -} - -void Navigation::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { - Navigation::printPVTdata(ubxDataStruct); - - Navigation::newData = true; - Navigation::ubxDataStatic = ubxDataStruct; - Navigation::ubxUpdateTimeStatic = millis(); -} - diff --git a/lib/Navigation/navigation.h b/lib/Navigation/navigation.h index adb4a6d..006e7da 100644 --- a/lib/Navigation/navigation.h +++ b/lib/Navigation/navigation.h @@ -12,15 +12,12 @@ #ifndef NAVIGATION_H #define NAVIGATION_H -#include -#include #include #include -#include #include "route.h" #include "NTRIPClient.h" -#include "calibrateCompass.h" + #include "component.h" /** @@ -52,15 +49,6 @@ class Navigation : public Component { Complete }; - enum CalcAzimuthState { - Invalid, - Bad, - Ok, - Good, - Super - }; - - /** * @brief Construct a new Navigation object and using I2C * @@ -68,13 +56,6 @@ class Navigation : public Component { */ Navigation(Route* route = nullptr); - /** - * @brief Construct a new Navigation object and using SPI - * - * @param route with which to navigate - */ - Navigation(SPIClass* spiPort, uint8_t csPin, Route* route = nullptr); - /** * @brief Destroy the Navigation object * @@ -138,9 +119,6 @@ class Navigation : public Component { * @return false no point added to route */ Status addCurrentPosToRoute(); - - // TODO: can be deleted? - // bool setPointBeforeTurn(); /** * @brief Get the Ubx Data object @@ -187,22 +165,11 @@ class Navigation : public Component { Point::Accuracy getMinAccuracy() const { return this->minAccuracy; } void setMinAccuracy(Point::Accuracy accuracy) { this->minAccuracy = accuracy; } - /** - * @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); // map input in range from -180 to 180 degree static int16_t fixDegree(int16_t degree); private: void run() override; - void runAsChild() override; - void updateCurrentLocation(); void updateMagneticDeclination(); void init(Route* route); bool nextPoint(); @@ -213,11 +180,11 @@ class Navigation : public Component { static constexpr uint8_t maxDisBetweenPoints = 10; static constexpr float minDisBetweenPoints = 0.3; - SFE_UBLOX_GNSS* gps; + UBX_NAV_PVT_data_t* ubxData = nullptr; Route* route = nullptr; NTRIPClient* ntripClient = nullptr; - QMC5883LCompass* compass; + Point lastPointRouteInsert; Point lastPointCalcCorrection; @@ -225,7 +192,7 @@ class Navigation : public Component { Point targetPoint; Point currentPosition; Point::Accuracy minAccuracy = Point::Accuracy::twoDigOfCM; - CalcAzimuthState calcAzimuthState = CalcAzimuthState::Invalid; + bool navigationStarted = false; bool navigationFinished = false; @@ -239,7 +206,6 @@ class Navigation : public Component { char* user; char* password; - int16_t realAzimuth = INT16_MAX; int16_t calcAzimuth = INT16_MAX; uint8_t timeToWait = 200; @@ -248,16 +214,6 @@ class Navigation : public Component { uint32_t ubxUpdateTime = 0; double minDistanceToReachPoint = 0.5; - - 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 // NAVIGATION_H diff --git a/lib/Sensors/senors.cpp b/lib/Sensors/senors.cpp new file mode 100644 index 0000000..2e0a738 --- /dev/null +++ b/lib/Sensors/senors.cpp @@ -0,0 +1,129 @@ +/** + * @file senors.cpp + * @author Alexander Klein (alex@kleiax.de) + * @brief + * @version 0.1 + * @date 2023-09-02 + * + * @copyright Copyright (c) 2023 + * + */ + +#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; +} + +void Sensors::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; + while (1); + } + this->initGnss(); +} + +void Sensors::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; + while (1); + } + this->initGnss(); +} + +void Sensors::enableCompass() { + this->compass = 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); + caliCompass.loadData(); + caliCompass.useData(); +} + +void Sensors::setOutputStatusPrintPVTdata(bool status) { + Sensors::outputStatusPrintPVTdata = status; +} + +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) + return; + + double latitude = (double) ubxDataStruct->lat / 10000000.0; + double longitude = (double) ubxDataStruct->lon / 10000000.0; + double altitude = (double) ubxDataStruct->hMSL / 1000.0; + + uint8_t fixType = ubxDataStruct->fixType; + char fixTypeString[32]; + if (fixType == 0) + strcpy(fixTypeString, "None"); + else if (fixType == 1) + strcpy(fixTypeString, "Dead Reckoning"); + else if (fixType == 2) + strcpy(fixTypeString, "2D"); + else if (fixType == 3) + strcpy(fixTypeString, "3D"); + else if (fixType == 3) + strcpy(fixTypeString, "GNSS + Dead Reckoning"); + else if (fixType == 5) + strcpy(fixTypeString, "Time Only"); + else + strcpy(fixTypeString, "UNKNOWN"); + + uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln; + char carrSolnString[16]; + if (carrSoln == 0) + strcpy(carrSolnString, "None"); + else if (carrSoln == 1) + strcpy(carrSolnString, "Floating"); + else if (carrSoln == 2) + strcpy(carrSolnString, "Fixed"); + else + strcpy(carrSolnString, "UNKNOWN"); + + uint32_t hAcc = ubxDataStruct->hAcc; + + std::cout << "Lat: " << latitude + << " Lng: " << longitude + << " Alt: " << altitude << std::endl; + + std::cout << "Fix: " << fixTypeString + << " Carrier Solution: " << carrSolnString + << " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl; +} + +void Sensors::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) { + Sensors::printPVTdata(ubxDataStruct); + + Sensors::newData = true; + Sensors::ubxDataStatic = ubxDataStruct; + Sensors::ubxUpdateTimeStatic = millis(); +} diff --git a/lib/Sensors/sensorData.h b/lib/Sensors/sensorData.h new file mode 100644 index 0000000..e69de29 diff --git a/lib/Sensors/sensors.h b/lib/Sensors/sensors.h new file mode 100644 index 0000000..cb6d9b1 --- /dev/null +++ b/lib/Sensors/sensors.h @@ -0,0 +1,77 @@ +/** + * @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 +#include +//Sensors +#include +#include +// Gyroskop + +#include "component.h" +#include "sensorData.h" +#include "calibrateCompass.h" + +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(); + + /** + * @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 initGnss(); + + QMC5883LCompass* compass = nullptr; + SFE_UBLOX_GNSS* gnss = nullptr; + + 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