quite some fixes

2023-09-23 22:47:07 +02:00
parent d0c5421ad9
commit e48f680fec
14 changed files with 69 additions and 49 deletions
@@ -14,7 +14,7 @@

 #include <stdint.h>

-#define HOTSPOT
+#define RHEDE

 #ifdef HOTSPOT
 namespace NetworkConfig {
@@ -51,8 +51,9 @@ namespace MqttConfig {
 namespace NtripConfig {
    const char host[] = "rtk2go.com";
    const uint16_t port = 2101;
-    const char mountPoint[] = "GER-Dortmund";
+    // const char mountPoint[] = "GER-Dortmund";
    // const char mountPoint[] = "GER-Papenburg";
+    const char mountPoint[] = "Rtkramerntrip";
    const char user[] = "alklein1@gmx.de";
    const char password[] = "none";
 }
@@ -19,6 +19,8 @@ Network::Network(const char *ssid, const char *passphrase) {
 }

 Network::Network(const char *ssid, const char *passphrase, NetworkAdresses adresses) {
+    this->adresses = adresses;
+
    if (!WiFi.mode(WIFI_AP_STA))
        std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl;

@@ -81,8 +83,8 @@ void Network::printIPs() {
    }
    std::cout << "WiFi is connected to" << std::endl;
    std::cout << "IP address: " << std::endl;
-    std::cout << this->adresses.localIP << std::endl;
-    std::cout << "WiFi MAC Address: " << WiFi.macAddress() << std::endl << std::endl;
+    std::cout << WiFi.localIP().toString().c_str() << std::endl;
+    std::cout << "WiFi MAC Address: " << WiFi.macAddress().c_str() << std::endl << std::endl;
 }

 uint8_t Network::getCurrentChannel() {
@@ -12,7 +12,6 @@
 #include "sensorData.h"

 bool SensorData::outputStatusPrintPVTdata = false;
-bool SensorData::newData = false;
 uint32_t SensorData::ubxUpdateTimeStatic = 0;
 UBX_NAV_PVT_data_t* SensorData::ubxDataStatic = nullptr;

@@ -161,7 +160,6 @@ void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
 void SensorData::savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
    SensorData::printPVTdata(ubxDataStruct);

-    SensorData::newData = true;
    SensorData::ubxDataStatic = ubxDataStruct;
    SensorData::ubxUpdateTimeStatic = millis();
 }
@@ -189,11 +187,8 @@ void SensorData::runAsChild() {
    if (this->gnss) {
        this->gnss->checkUblox();
        this->gnss->checkCallbacks();
-    }
-
-    if (SensorData::newData) {
-        SensorData::newData = false;
-        // TODO: Update data
+        if (SensorData::ubxUpdateTimeStatic != this->lastUbxUpdate)
+            this->updateUbxData();
    }        
 }

@@ -210,3 +205,14 @@ void SensorData::initGnss() {
    this->gnss->setNavigationFrequency(1);
    this->gnss->setAutoPVT(true);
 }
+
+void SensorData::updateUbxData() {
+    this->gnssData = SensorData::ubxDataStatic;
+    this->lastUbxUpdate = SensorData::ubxUpdateTimeStatic;
+
+    Point::Coordinates coords;
+    coords.lat = this->gnssData->lat / 10000000.0;
+    coords.lon = this->gnssData->lon / 10000000.0;
+
+    this->currentPosition = Point(coords, this->gnssData->hAcc);
+}
@@ -70,6 +70,8 @@ class SensorData : public Component {
        void run() override;
        void runAsChild() override;
        void initGnss();
+        void updateUbxData();
+

        QMC5883LCompass* realCompass = nullptr;
        CalcAzimuth* calcCompass = nullptr;
@@ -93,8 +95,9 @@ class SensorData : public Component {
        uint16_t port;
        int16_t realAzimuth = INT16_MAX;
        int16_t calcAzimuth = INT16_MAX;
+        uint32_t lastUbxUpdate = 0;

-        float yawPitchRoll[3];
+        float yawPitchRoll[3] {0, 0, 0};

        // static
        static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
@@ -103,7 +106,6 @@ class SensorData : public Component {
        static UBX_NAV_PVT_data_t* ubxDataStatic;
        static uint32_t ubxUpdateTimeStatic;
        static bool outputStatusPrintPVTdata;
-        static bool newData;
 };

 #endif //SENSOR_DATA_H
@@ -14,6 +14,7 @@
 MenuSensorData::MenuSensorData(SensorData* sensorData) :
    MenuInformationSites(7) {
        this->sensorData = sensorData;
+        this->noEqualLeft = true;
 }

 void MenuSensorData::printPage() const {
@@ -81,20 +82,20 @@ void MenuSensorData::printPage() const {
            lineTwo = "Ntrip: ";
            uint8_t carrSoln = gpsData->flags.bits.carrSoln;
            if (carrSoln == 0)
-                lineOne = "None";
+                lineOne.concat("None");
            else if (carrSoln == 1)
-                lineOne = "Floating";
+                lineOne.concat("Floating");
            else if (carrSoln == 2)
-                lineOne = "Fixed";
+                lineOne.concat("Fixed");
            else
                lineOne = "UNKNOWN";
            NTRIPClientStates status = this->sensorData->getNtripState();
            if (status ==  NTRIPClientStates::pushData)
-                lineTwo = "enabled";
+                lineTwo.concat("enabled");
            else if (status == NTRIPClientStates::notAvailable)
-                lineTwo = " N/A";
+                lineTwo.concat(" N/A");
            else
-                lineTwo = "disabled";
+                lineTwo.concat("disabled");
            }
            break;

@@ -105,8 +106,8 @@ void MenuSensorData::printPage() const {
                lineOne.concat(gpsData->hAcc);
                lineTwo.concat(gpsData->numSV);
            } else {
-                lineOne = "-/-";
-                lineTwo = "-/-";
+                lineOne.concat("-/-");
+                lineTwo.concat("-/-");
            }
            break;

@@ -24,7 +24,3 @@ void MenuDriveMode::left() {
        this->leaved = true;
    }
 }
-
-void MenuDriveMode::no() {
-    this->left();
-}
@@ -32,11 +32,6 @@ class MenuDriveMode : public MenuInformationSites {
         */
        void left() override;

-        /**
-         * @brief Calls left.
-         */
-        void no() override;
-
    protected:
        void configureOnLeave() {}

@@ -18,20 +18,20 @@ DirectionChangeSignal::DirectionChangeSignal(Autopilot* pilot) {
 }

 DirectionChangeSignal::~DirectionChangeSignal() {
-    pilot->getSensorData()->getCalcCompass()->disableCalcAzimuth();
+    CalcAzimuth* calcAzimuth = pilot->getSensorData()->getCalcCompass();
+    if (calcAzimuth)
+        calcAzimuth->disableCalcAzimuth();
 }

 void DirectionChangeSignal::action() {
-    pilot->getSensorData()->getCalcCompass()->drivingDirectionChange(pilot->getSensorData()->getCurrentPos());
+    CalcAzimuth* calcAzimuth = pilot->getSensorData()->getCalcCompass();
+    if (calcAzimuth)
+        calcAzimuth->drivingDirectionChange(pilot->getSensorData()->getCurrentPos());
 }


 Autopilot::Autopilot() {
-        this->setInputMode(ManualControl::InputMode::Digital);
-        this->directionChangeSignal = new DirectionChangeSignal(this);
-        this->setDirectionChangeCallback(this->directionChangeSignal);
-        this->navigation = new Navigation(this->sensorData);
-        this->init();
+        
 }

 Autopilot::~Autopilot() {
@@ -247,3 +247,11 @@ void Autopilot::restartLoop() {
    this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
    this->updateDisplay = true;
 }
+
+void Autopilot::afterActivate() {
+    this->setInputMode(ManualControl::InputMode::Digital);
+    this->directionChangeSignal = new DirectionChangeSignal(this);
+    this->setDirectionChangeCallback(this->directionChangeSignal);
+    this->navigation = new Navigation(this->sensorData);
+    this->init();
+}
@@ -102,6 +102,7 @@ class Autopilot : public ManualControl {
        void askNavigationForOrder();
        void selfDriving();
        void restartLoop();
+        void afterActivate() override;

        Navigation* navigation;
        CourseCorrection courseCorrection;
@@ -10,16 +10,14 @@
 */

 #include "driveModi/Modi/CaptureRoute/captureRoute.h"
-
-CaptureRoute::CaptureRoute() {
-        this->navigation = new Navigation(this->sensorData);
-        this->navigation->getRoute()->clear();
-        this->sensorData->getNtripClient()->setAutoReconnect(true);
-        this->routeInfo = navigation->getRouteInfo();
-}
+#include "captureRoute.h"

 CaptureRoute::~CaptureRoute() {
-    // this->navigation->getNTRIPClient()->setActivated(false);
+    if (this->navigation)
+        delete this->navigation;
+
+    if (this->sensorData->getNtripClient())
+        this->sensorData->getNtripClient()->setActivated(false);
 }

 void CaptureRoute::run() {
@@ -34,6 +32,13 @@ void CaptureRoute::run() {
    }
 }

+void CaptureRoute::afterActivate() {
+    this->navigation = new Navigation(this->sensorData);
+        this->navigation->getRoute()->clear();
+        this->sensorData->getNtripClient()->setAutoReconnect(true);
+        this->routeInfo = navigation->getRouteInfo();
+}
+
 double CaptureRoute::getDistanceToLastPoint() const {
    if (!this->lastSavedPoint.isInit())
        return 0;
@@ -27,8 +27,6 @@
 */
 class CaptureRoute : public ManualControl {
    public:
-        CaptureRoute();
-
        /**
         * @brief Destroy the Capture Route object
         * 
@@ -68,8 +66,9 @@ class CaptureRoute : public ManualControl {
        bool shouldUpdate();
    private:
        void run() override;
+        void afterActivate() override;

-        Navigation* navigation;
+        Navigation* navigation = nullptr;
        RouteInfo routeInfo;
        Point lastSavedPoint;
        Navigation::Status status = Navigation::Status::Complete;
@@ -27,4 +27,5 @@ void DriveModi::activate(DriveModiParams params){
 void DriveModi::init() {
    this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
    this->loopDelay = 40;
+    this->afterActivate();
 }
@@ -60,6 +60,8 @@ class DriveModi : public Component {
        double getMaxRotation() const { return this->maxRotationSpeed; }

    protected:
+        virtual void afterActivate() {}
+
        MoveControl *moveControl;
        const ControlPadInput* input;
        const SensorData* sensorData;
@@ -114,6 +114,7 @@ void setup() {
    sensorData = new SensorData();
    sensorData->enableGnss(spiPort, PinNumbers::gnssSpiCs);
    sensorData->enableRealCompass();
+    sensorData->enableNtrip(NtripConfig::host, NtripConfig::port, NtripConfig::mountPoint, NtripConfig::user, NtripConfig::password);
    // sensorData->enableGyroskop();
    driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr());