401 lines
12 KiB
C++
401 lines
12 KiB
C++
/**
|
|
* @file NTRIPClient.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief Contains the implementation of the class NTRIPClient.
|
|
* @version 0.1
|
|
* @date 2022-09-18
|
|
*
|
|
* @copyright Copyright (c) 2022
|
|
*
|
|
*/
|
|
|
|
#include "ntripClient.h"
|
|
|
|
NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS *gnss, const char *host, uint16_t port, const char *mountPoint, const char *user, const char *password)
|
|
: gnss{gnss}, port{port}, host{host}, mountPoint{mountPoint}, user{user}, password{password}, ntripClient{new WiFiClient}, state{NTRIPClientStates::closingConnection}
|
|
{
|
|
Component::loopDelay = NTRIPClient::loopDelay;
|
|
}
|
|
|
|
NTRIPClient::~NTRIPClient()
|
|
{
|
|
delete this->ntripClient;
|
|
}
|
|
|
|
void NTRIPClient::run()
|
|
{
|
|
switch (this->state)
|
|
{
|
|
case NTRIPClientStates::openingConnection:
|
|
if (!this->activated)
|
|
{
|
|
this->state = NTRIPClientStates::closingConnection;
|
|
break;
|
|
}
|
|
|
|
std::cout << "Connecting to the NTRIP caster..." << std::endl;
|
|
if (this->beginClient())
|
|
{
|
|
std::cout << "Connected to the NTRIP caster!" << std::endl;
|
|
this->state = NTRIPClientStates::pushingData;
|
|
}
|
|
else
|
|
{
|
|
std::cout << "Failed!" << std::endl;
|
|
this->state = NTRIPClientStates::waiting;
|
|
this->activated = false;
|
|
}
|
|
break;
|
|
|
|
case NTRIPClientStates::pushingData:
|
|
if (!processConnection() || !this->activated)
|
|
{
|
|
this->state = NTRIPClientStates::closingConnection;
|
|
}
|
|
break;
|
|
|
|
case NTRIPClientStates::closingConnection:
|
|
std::cout << "Closing the connection to the NTRIP caster..." << std::endl;
|
|
this->closeConnection();
|
|
state = NTRIPClientStates::waiting;
|
|
break;
|
|
|
|
case NTRIPClientStates::waiting:
|
|
if (this->activated)
|
|
{
|
|
this->state = NTRIPClientStates::openingConnection;
|
|
}
|
|
else
|
|
{
|
|
this->checkAutoReconnect();
|
|
}
|
|
break;
|
|
|
|
case NTRIPClientStates::notAvailable:
|
|
break;
|
|
|
|
default:
|
|
std::cout << "Wrong state in NTRIPClient.cpp..." << std::endl;
|
|
this->state = NTRIPClientStates::closingConnection;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void NTRIPClient::runAsChild()
|
|
{
|
|
this->pushGPGGA();
|
|
}
|
|
|
|
void NTRIPClient::gnssConfiguration()
|
|
{
|
|
this->gnss->setSPIOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
|
|
this->gnss->setPortInput(COM_PORT_SPI, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3);
|
|
// Set the differential mode - ambiguities are fixed whenever possible
|
|
this->gnss->setDGNSSConfiguration(SFE_UBLOX_DGNSS_MODE_FIXED);
|
|
this->gnss->setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);
|
|
this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
|
|
}
|
|
|
|
bool NTRIPClient::setActivated(bool state)
|
|
{
|
|
if (state && this->state != NTRIPClientStates::notAvailable)
|
|
{
|
|
this->activated = true;
|
|
}
|
|
else if (state)
|
|
{
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
this->activated = false;
|
|
this->autoReconnect = false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void NTRIPClient::setAutoReconnect(bool state)
|
|
{
|
|
if (state)
|
|
{
|
|
this->reconnectAttemps = 0;
|
|
this->autoReconnect = true;
|
|
return;
|
|
}
|
|
|
|
this->autoReconnect = false;
|
|
}
|
|
|
|
bool NTRIPClient::beginClient()
|
|
{
|
|
static constexpr uint16_t httpError = 401;
|
|
static constexpr uint16_t httpCheck = 200;
|
|
|
|
std::cout << "Opening socket to " << this->host << std::endl;
|
|
|
|
char serverRequest[this->bufferSize];
|
|
char credentials[this->bufferSize];
|
|
|
|
if (!static_cast<bool>(this->ntripClient->connect(static_cast<const char *>(this->host), this->port)))
|
|
{
|
|
std::cout << "Connection to caster failed" << std::endl;
|
|
return false;
|
|
}
|
|
std::cout << "Connected to " << this->host << " : " << this->port << std::endl;
|
|
std::cout << "Requesting NTRIP Data from mount point " << this->mountPoint << std::endl;
|
|
|
|
// Generate the server request (GET)
|
|
if (this->useNtripRev1)
|
|
{
|
|
snprintf(static_cast<char *>(serverRequest),
|
|
this->bufferSize,
|
|
static_cast<const char *>("GET /%s HTTP/1.0\r\n\
|
|
Host: %s:%u\r\n\
|
|
User-Agent: NTRIP KleiaxNtripClient/0.1\r\n\
|
|
Accept: */*\r\n\
|
|
Connection: close\r\n"),
|
|
this->mountPoint,
|
|
this->host,
|
|
this->port);
|
|
}
|
|
else
|
|
{
|
|
snprintf(static_cast<char *>(serverRequest),
|
|
this->bufferSize,
|
|
static_cast<const char *>("GET /%s HTTP/1.1\r\n\
|
|
Host: %s:%u\r\n\
|
|
Ntrip-Version: Ntrip/2.0\r\n\
|
|
User-Agent: NTRIP KleiaxNtripClient/0.1\r\n\
|
|
Accept: */*\r\n\
|
|
Connection: close\r\n"),
|
|
this->mountPoint,
|
|
this->host,
|
|
this->port);
|
|
}
|
|
|
|
// Add own Position if activated
|
|
if (this->transmitLocation && !this->useNtripRev1)
|
|
{
|
|
auto *data = new NMEA_GGA_data_t;
|
|
const uint8_t res = this->gnss->getLatestNMEAGPGGA(data);
|
|
if (res > 0) // valid data
|
|
{
|
|
char positionUpdate[this->bufferSizePushGPGGA];
|
|
snprintf(static_cast<char *>(positionUpdate),
|
|
this->bufferSizePushGPGGA,
|
|
static_cast<const char *>("Ntrip-GGA: %s\r\n"),
|
|
data->nmea);
|
|
strncat(static_cast<char *>(serverRequest), static_cast<const char *>(positionUpdate), this->bufferSize);
|
|
}
|
|
delete data;
|
|
|
|
this->lastGPGGAPushTime = millis();
|
|
}
|
|
|
|
if (this->user && this->password)
|
|
{
|
|
// Credentials
|
|
const uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 2;
|
|
auto *userCredentials = new char[userCredentialsLength];
|
|
snprintf(static_cast<char *>(userCredentials), userCredentialsLength, static_cast<const char *>("%s:%s"), this->user, this->password);
|
|
|
|
std::cout << "Sending credentials: " << userCredentials << std::endl;
|
|
|
|
// Encode
|
|
const base64 base;
|
|
const String strEncodedCredentials = base64::encode(userCredentials);
|
|
delete userCredentials;
|
|
char encodedCredentials[strEncodedCredentials.length() + 1];
|
|
strEncodedCredentials.toCharArray(static_cast<char *>(encodedCredentials), sizeof(encodedCredentials));
|
|
|
|
snprintf(credentials, sizeof(credentials), static_cast<const char *>("Authorization: Basic %s\r\n"), static_cast<const char *>(encodedCredentials));
|
|
|
|
// Add the encoded credentials to the server request
|
|
strncat(static_cast<char *>(serverRequest), static_cast<const char *>(credentials), this->bufferSize);
|
|
}
|
|
|
|
strncat(static_cast<char *>(serverRequest), static_cast<const char *>("\r\n"), this->bufferSize);
|
|
|
|
std::cout << static_cast<const char *>("serverRequest size: ")
|
|
<< strlen(serverRequest)
|
|
<< static_cast<const char *>(" of ")
|
|
<< this->bufferSize
|
|
<< static_cast<const char *>(" bytes available")
|
|
<< std::endl;
|
|
|
|
// Send the server request
|
|
std::cout << "Sending server request: " << serverRequest << std::endl;
|
|
this->ntripClient->write(serverRequest, strlen(serverRequest));
|
|
|
|
// Wait up to 5 seconds for response
|
|
const uint32_t lastMillis = millis();
|
|
while (static_cast<bool>(!ntripClient->available()))
|
|
{
|
|
if (millis() - lastMillis > this->timeOut)
|
|
{
|
|
std::cout << "Caster timed out!" << std::endl;
|
|
this->ntripClient->stop();
|
|
return false;
|
|
}
|
|
delay(10);
|
|
}
|
|
|
|
// Check reply
|
|
uint16_t httpStatusCode = 0;
|
|
char response[this->bufferSize];
|
|
uint16_t responseIndex = 0;
|
|
while (static_cast<bool>(this->ntripClient->available()))
|
|
{
|
|
if (responseIndex == sizeof(response))
|
|
{
|
|
break;
|
|
}
|
|
|
|
response[responseIndex++] = ntripClient->read();
|
|
|
|
if (httpStatusCode == 0)
|
|
{
|
|
if (strstr(response, static_cast<const char *>("200")) != nullptr)
|
|
{
|
|
httpStatusCode = httpCheck;
|
|
}
|
|
if (strstr(response, static_cast<const char *>("401")) != nullptr)
|
|
{
|
|
httpStatusCode = httpError;
|
|
}
|
|
}
|
|
}
|
|
response[responseIndex] = '\0';
|
|
|
|
// std::cout << "Caster response: " << response << std::endl;
|
|
|
|
if (httpStatusCode != httpCheck)
|
|
{
|
|
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 == httpError)
|
|
{
|
|
std::cout << "Statuscode 401 - Unauthorized" << std::endl;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
std::cout << "Connected to: " << this->host << std::endl;
|
|
this->lastReceivedRtcmTime = millis();
|
|
return true;
|
|
}
|
|
|
|
void NTRIPClient::closeConnection()
|
|
{
|
|
if (static_cast<bool>(this->ntripClient->connected()))
|
|
{
|
|
this->ntripClient->stop();
|
|
}
|
|
this->activated = false;
|
|
std::cout << "NtripClient disconnected from: " << this->host << std::endl;
|
|
}
|
|
|
|
bool NTRIPClient::processConnection()
|
|
{
|
|
if (static_cast<bool>(this->ntripClient->connected()))
|
|
{
|
|
uint8_t rtcmData[this->bufferSize * 8];
|
|
uint16_t rtcmCount = 0;
|
|
|
|
while (static_cast<bool>(this->ntripClient->available()))
|
|
{
|
|
rtcmData[rtcmCount++] = ntripClient->read();
|
|
if (rtcmCount == sizeof(rtcmData))
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (rtcmCount > 0)
|
|
{
|
|
this->lastReceivedRtcmTime = millis();
|
|
this->gnss->pushRawData(static_cast<uint8_t *>(rtcmData), rtcmCount);
|
|
// std::cout << "Pushed " << rtcmCount << " RTCM bytes to ZED." << std::endl;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
std::cout << "Connection to " << this->host << " dropped!" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
if (millis() - this->lastReceivedRtcmTime > this->timeOut)
|
|
{
|
|
std::cout << "RTCM timeout!" << std::endl;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void NTRIPClient::checkAutoReconnect()
|
|
{
|
|
if (!this->autoReconnect)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (millis() - this->lastReconnectTime < this->reconnectDelayTime)
|
|
{
|
|
return;
|
|
}
|
|
this->lastReconnectTime = millis();
|
|
|
|
if (this->reconnectAttemps >= this->maxReconnectAttemps)
|
|
{
|
|
this->autoReconnect = false;
|
|
return;
|
|
}
|
|
|
|
this->activated = true;
|
|
this->reconnectAttemps++;
|
|
}
|
|
|
|
void NTRIPClient::pushGPGGA()
|
|
{
|
|
if (!this->transmitLocation && !this->activated)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (millis() - this->lastGPGGAPushTime < this->pushGPGGATime)
|
|
{
|
|
return;
|
|
}
|
|
this->lastGPGGAPushTime = millis();
|
|
|
|
if (!this->ntripClient->connected())
|
|
{
|
|
std::cout << "Failed to pushing GGA to server: " << std::endl;
|
|
}
|
|
|
|
auto *data = new NMEA_GGA_data_t;
|
|
const uint8_t res = this->gnss->getLatestNMEAGPGGA(data);
|
|
if (res == 2) // 2 means fresh data
|
|
{
|
|
char positionUpdate[this->bufferSizePushGPGGA];
|
|
snprintf(static_cast<char *>(positionUpdate),
|
|
this->bufferSizePushGPGGA,
|
|
static_cast<const char *>("%s\r\n"),
|
|
data->nmea);
|
|
this->ntripClient->write(positionUpdate, strlen(positionUpdate));
|
|
std::cout << "Position update: " << positionUpdate << std::endl;
|
|
}
|
|
delete data;
|
|
}
|
|
|
|
bool NTRIPClient::isConnected()
|
|
{
|
|
return this->state == NTRIPClientStates::pushingData;
|
|
}
|