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

try to work ntrip client

Browse Source
This commit is contained in:
kleiax
2022-10-24 22:08:19 +02:00
parent ee5bc9129a
commit e01127e3e3
12 changed files with 108 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/moveControl.h
+4
View File
@@ -19,6 +19,7 @@
#include "motorControl.h"
#include "speedometer.h"
#include "moveControlConfig.h"
#include "debugTimes.h"
/**
* @brief used to set driving status
@@ -168,7 +169,10 @@ class MoveControl {
double right_pid_out;
uint8_t delay = 30;
uint8_t overTimeCounter = 0;
uint8_t overTimeMax = 10;
int8_t rawPowerLeft = 0;
int8_t rawPowerRight = 0;
uint32_t lastMillis = 0;
};
#endif // MOVE_CONTROL_H
include/networkConfig.h
+3 -2
View File
@@ -30,9 +30,8 @@
*/
#define WLAN_CONNECT_LOOP_TIME 500
#define WLAN_DNS_SERVER "8.8.8.8"
#define HOTSPOT
#define NTRIP_NRW
#define NTRIP_NS
// NTRIP config NRW
#ifdef NTRIP_NRW
@@ -52,6 +51,8 @@
#define NTRIP_PASSWORD "ALdx-1-3e49"
#endif // NTRIP_NS
#define RHEDE
//Network config ESP32
#ifdef ESP32BROKER
#define WLAN_SSID "ESP32-Broker"
lib/Navigation/navigation.cpp
+2
View File
@@ -46,6 +46,8 @@ Navigation::~Navigation() {
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(true);
this->isNtripInit = true;
}
lib/Ntrip/NTRIPClient.cpp
+45 -21
View File
@@ -13,11 +13,11 @@
NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, const char* mountPoint, const char* user, const char* password) {
this->gps = gps;
this->host = host;
strcpy(this->host, host);
this->port = port;
this->mountPoint = mountPoint;
this->user = user;
this->password = password;
strcpy(this->mountPoint, mountPoint);
strcpy(this->user, user);
strcpy(this->password, password);
this->ntripClient = new WiFiClient;
}
@@ -35,17 +35,27 @@ void NTRIPClient::loop() {
this->gps->checkCallbacks();
if (millis() - this->lastGPGGAPushTime > this->pushGPGGATime) {
NMEA_GGA_data_t *data = nullptr;
this->lastGPGGAPushTime = millis();
// std::cout << "Try to push GPGGA data." << std::endl;
NMEA_GGA_data_t *data = new NMEA_GGA_data_t;
DebugTimes requestGpsData;
uint8_t res = this->gps->getLatestNMEAGPGGA(data);
requestGpsData.stopConsol("RequestGpsData", 5);
if (res == 2)
this->pushGPGGA(data);
delete data;
}
switch (this->state) {
case NTRIPClientStates::openConnection:
if (millis() - this->lastNtripConnectTime > this->tryReconnectTime)
if (millis() - this->lastNtripConnectTime < this->tryReconnectTime)
break;
if (!this->activated) {
this->state = NTRIPClientStates::closeConnection;
break;
}
std::cout << "Connecting to the NTRIP caster..." << std::endl;
if (this->beginClient()) {
std::cout << "Connected to the NTRIP caster!" << std::endl;
@@ -53,12 +63,12 @@ void NTRIPClient::loop() {
} else {
uint8_t seconds = this->tryReconnectTime / 1000;
std::cout << "Could not connect to the caster. Trying again in "
<< seconds << " seconds." << std::endl;
<< (int) seconds << " seconds." << std::endl;
this->lastNtripConnectTime = millis();
}
break;
case pushData:
case NTRIPClientStates::pushData:
if (!processConnection() || !this->activated)
this->state = NTRIPClientStates::closeConnection;
break;
@@ -72,6 +82,14 @@ void NTRIPClient::loop() {
case NTRIPClientStates::wait:
if (this->activated)
this->state = NTRIPClientStates::openConnection;
if (this->activated) {
std::cout << "state is openConnection" << std::endl;
}
break;
default:
std::cout << "Wrong state in NTRIPClient.cpp..." << std::endl;
this->state = NTRIPClientStates::closeConnection;
break;
}
}
@@ -79,7 +97,8 @@ void NTRIPClient::loop() {
void NTRIPClient::gpsConfiguration() {
this->gps->setI2COutput(COM_TYPE_UBX | COM_TYPE_NMEA);
this->gps->setPortInput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3);
this->gps->setDGNSSConfiguration(SFE_UBLOX_DGNSS_MODE_FIXED); // Set the differential mode - ambiguities are fixed whenever possible
// Set the differential mode - ambiguities are fixed whenever possible
// this->gps->setDGNSSConfiguration(SFE_UBLOX_DGNSS_MODE_FIXED);
this->gps->setNavigationFrequency(1);
this->gps->setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);
this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_I2C, 10);
@@ -107,7 +126,7 @@ bool NTRIPClient::beginClient() {
this->mountPoint);
// Credentials
uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 1;
uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 2;
char* userCredentials = new char[userCredentialsLength];
snprintf(userCredentials, userCredentialsLength, "%s:%s", this->user, this->password);
@@ -162,7 +181,7 @@ bool NTRIPClient::beginClient() {
if (httpStatusCode == 0) {
if (strstr(response, "200") != nullptr)
httpStatusCode = 200;
if (strstr(response, "400") != nullptr)
if (strstr(response, "401") != nullptr)
httpStatusCode = 401;
}
}
@@ -171,8 +190,12 @@ bool NTRIPClient::beginClient() {
// std::cout << "Caster response: " << response << std::endl;
if (httpStatusCode != 200) {
std::cout << "Failed to connect to " << this->host << std::endl;
if (httpStatusCode == 401)
std::cout << "Failed to connect to " << this->host << " - HTTP Code: " << (int) httpStatusCode
<< " Length of Response: " << responseIndex << std::endl;
if (httpStatusCode == 0)
std::cout << "Response: " << response << std::endl;
else if (httpStatusCode == 401)
std::cout << "Statuscode 401 - Unauthorized" << std::endl;
return false;
}
@@ -243,6 +266,7 @@ void NTRIPClient::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
else if (fixType == 3)
strcpy(fixTypeString, "GNSS + Dead Reckoning");
else if (fixType == 5)
strcpy(fixTypeString, "Time Only");
else
strcpy(fixTypeString, "UNKNOWN");
@@ -250,23 +274,23 @@ void NTRIPClient::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
char carrSolnString[16];
if (carrSoln == 0)
strcpy(fixTypeString, "None");
strcpy(carrSolnString, "None");
else if (carrSoln == 1)
strcpy(fixTypeString, "Floating");
strcpy(carrSolnString, "Floating");
else if (carrSoln == 2)
strcpy(fixTypeString, "Fixed");
strcpy(carrSolnString, "Fixed");
else
strcpy(fixTypeString, "UNKNOWN");
strcpy(carrSolnString, "UNKNOWN");
uint32_t hAcc = ubxDataStruct->hAcc;
std::cout << "Lat: " << latitude
<< "Lng: " << longitude
<< "Alt: " << altitude << std::endl;
<< " Lng: " << longitude
<< " Alt: " << altitude << std::endl;
std::cout << "Fix: " << fixTypeString
<< "Carrier Solution: " << carrSolnString
<< "Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl;
<< " Carrier Solution: " << carrSolnString
<< " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl;
}
bool NTRIPClient::isConnected() {
lib/Ntrip/NTRIPClient.h
+7 -5
View File
@@ -7,6 +7,8 @@
#include <iostream>
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include "debugTimes.h"
enum NTRIPClientStates {
openConnection,
pushData,
@@ -38,7 +40,7 @@ class NTRIPClient {
SFE_UBLOX_GNSS* gps;
WiFiClient* ntripClient;
NTRIPClientStates state;
NTRIPClientStates state = NTRIPClientStates::closeConnection;
bool transmitLocation = true;
bool activated = true;
@@ -49,10 +51,10 @@ class NTRIPClient {
uint32_t lastGPGGAPushTime = 0;
uint32_t lastLoopTime = 0;
const char* host;
const char* mountPoint;
const char* user;
const char* password;
char host[128];
char mountPoint[128];
char user[128];
char password[128];
const uint8_t delayTime = 20;
const uint16_t timeOut = 5000;
const uint16_t tryReconnectTime = 5000;
lib/Times/debugTimes.cpp
+12 -4
View File
@@ -12,6 +12,13 @@
DebugTimes::DebugTimes() {
this->startTime = millis();
static bool firstInstance = true;
if (firstInstance) {
firstInstance = false;
if (!DebugTimes::print)
std::cout << std::endl << "Warning: DebugTimes is muuted, no times are be shown." << std::endl << std::endl;
}
}
void DebugTimes::restart() {
@@ -22,8 +29,9 @@ uint16_t DebugTimes::stop() {
return millis() - this->startTime;
}
void DebugTimes::stopConsol(const char* name, uint16_t minTime) {
uint16_t DebugTimes::stopConsol(const char* name, uint16_t minTime) {
uint64_t time = millis() - this->startTime;
if (time > minTime)
Serial.printf("%s needs %llu ms\n", name, time);
}
if (time > minTime && DebugTimes::print)
std::cout << name << " needs " << time << " ms" << std::endl;
return time;
}
lib/Times/debugTimes.h
+7 -2
View File
@@ -14,6 +14,7 @@
#include <list>
#include <cstdint>
#include <iostream>
#include <Arduino.h>
@@ -53,11 +54,15 @@ class DebugTimes {
*
* @param name Functionname to print
* @param minTime A minimum time before printing
*
* @return uint16_t elapsed milliseconds
*/
void stopConsol(const char* name, uint16_t minTime = 0);
uint16_t stopConsol(const char* name, uint16_t minTime = 0);
private:
uint64_t startTime;
static const bool print = false;
};
#endif //DEBUG_TIMES_H
#endif //DEBUG_TIMES_H
src/SpecialMenus/GPS/menuGPS.cpp
+2 -2
View File
@@ -83,7 +83,7 @@ void MenuGPS::printPage(uint8_t page) const {
}
if (fixType)
std::cout << "Sats: " << (int) siv
<< "HDOP: " << (int) hdop << std::endl;
<< " HDOP: " << (int) hdop << std::endl;
else
std::cout << "Data isn't valid or no GPS signal" << std::endl;
}
@@ -107,7 +107,7 @@ void MenuGPS::printPage(uint8_t page) const {
}
if (fixType)
std::cout << "Lat: " << lat
<< "Lng: " << lng << std::endl;
<< " Lng: " << lng << std::endl;
else
std::cout << "Data isn't valid or NO GPS signal" << std::endl;
}
src/driveModi/driveManager.cpp
+6
View File
@@ -29,8 +29,14 @@ DriveManager::~DriveManager() {
}
void DriveManager::loop() {
DebugTimes moveControlTime;
this->moveControl->loop();
moveControlTime.stopConsol("MoveControlTime", 5);
DebugTimes navigationTime;
this->navigation->loop();
navigationTime.stopConsol("NavigationTime", 5);
if (currentModusPtr)
this->currentModusPtr->loop();
}
src/driveModi/driveManager.h
+1
View File
@@ -23,6 +23,7 @@
#include "driveModi/driveModi.h"
#include "navigation.h"
#include "networkConfig.h"
#include "debugTimes.h"
// All Drive Modi
#include "driveModi/Modi/ManualControl/manualControl.h"
src/main.cpp
+8 -2
View File
@@ -29,6 +29,7 @@
#include "network.h"
#include "debugMqtt.h"
#include "battery.h"
#include "debugTimes.h"
#include "menu.h"
#include "menuAction.h"
@@ -125,12 +126,17 @@ void setup() {
void loop() {
if (wifiIsActive) {
DebugTimes wifiTime;
Network::checkWiFi();
Network::checkMQTT();
wifiTime.stopConsol("WiFi-Time", 5);
}
driveManager->loop();
DebugTimes mainBatteryTime;
mainBattery->loop();
mainBatteryTime.stopConsol("MainBatteryTime", 5);
// static uint32_t lastMillis = 0;
// if (millis() - lastMillis > 2000) {
@@ -204,7 +210,7 @@ void i2cScanner() {
std::cout << "I2C device found at address 0x";
if (address<16)
std::cout << "0";
std::cout << std::hex << (int) address << std::endl;
std::cout << std::hex << (int) address << std::dec << std::endl;
// std::cout << (int) address << std::endl;
nDevices++;
}
@@ -212,7 +218,7 @@ void i2cScanner() {
std::cout << "Unknow error at address 0x";
if (address<16)
std::cout << "0";
std::cout << std::hex << (int) address << std::endl;
std::cout << std::hex << (int) address << std::dec << std::endl;
}
}
if (nDevices == 0)
src/moveControl.cpp
+11 -6
View File
@@ -57,17 +57,22 @@ void MoveControl::loop() {
|| right_motor_time > 50
|| left_speed_time > 50
|| right_speed_time > 50) {
char buf[128];
sprintf(buf, "left M: %d, right M %d, left S %d, right S %d in moveControl::loop\n", left_motor_time, right_motor_time, left_speed_time, right_speed_time);
std::cout << buf;
this->overTimeCounter++;
if (this->overTimeCounter >= this->overTimeMax) {
char buf[128];
sprintf(buf, "left M: %d, right M %d, left S %d, right S %d in moveControl::loop\n",
left_motor_time, right_motor_time, left_speed_time, right_speed_time);
std::cout << buf;
this->overTimeCounter = 0;
}
}
static uint64_t lastMillis = 0;
if (millis() - lastMillis < delay)
if (millis() - this->lastMillis < this->delay)
return;
this->runMoveControl();
lastMillis = millis();
this->lastMillis = millis();
}
void MoveControl::runMoveControl() {