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

new ntrip client

Browse Source
This commit is contained in:
kleiax
2022-09-21 12:50:01 +02:00
parent af6cb98898
commit a1a976be57
15 changed files with 658 additions and 251 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/moveControl.h
+7 -1
View File
@@ -27,7 +27,8 @@
*
*/
enum DrivingStatus {stop,
drive};
drive,
raw};
/**
* @brief This class manages the motors and the encoders
@@ -96,6 +97,9 @@ class MoveControl {
*/
void setRotationSpeed(double speed);
void setRawPowerLeft(int16_t power);
void setRawPowerRight(int16_t power);
/**
* @brief Set the pid tunings
*
@@ -164,5 +168,7 @@ class MoveControl {
double right_pid_out;
uint8_t delay = 30;
int8_t rawPowerLeft = 0;
int8_t rawPowerRight = 0;
};
#endif // MOVE_CONTROL_H
include/networkConfig.h
+9 -12
View File
@@ -30,15 +30,15 @@
*/
#define WLAN_CONNECT_LOOP_TIME 500
#define WLAN_DNS_SERVER "8.8.8.8"
#define DORO
#define HOTSPOT
#define NTRIP_NS
#define NTRIP_NRW
// NTRIP config NRW
#ifdef NTRIP_NRW
#define NTRIP_HOST "80.158.61.104"
#define NTRIP_HOST "www.sapos-nw-ntrip.de"
#define NTRIP_PORT 2101
#define NTRIP_MOUNT_POINT "EPS_NW"
#define NTRIP_MOUNT_POINT "VRS_3_3G_NW"
#define NTRIP_USER "nw-916771"
#define NTRIP_PASSWORD "Schalke#246"
#endif // NTRIP_NRW
@@ -67,15 +67,12 @@
#ifdef HOTSPOT
#define WLAN_SSID "Kleiax Handy"
#define WLAN_PASSWORD "12345677"
#define WLAN_IP "192.168.11.4"
#define WLAN_IP "192.168.43.4"
#define WLAN_SUBNETMASK "255.255.128.0"
#define WLAN_GATEWAY "192.168.0.1"
#define MQTT
#define MQTT_SERVER "192.168.1.7"
#define MQTT_PORT 1883
#define MQTT_AUTH
#define MQTT_USER "kleiax"
#define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#"
#define WLAN_GATEWAY "192.168.43.1"
// Only for defines reasons
#define MQTT_SERVER "172.22.64.216"
#define MQTT_PORT 1883
#endif //HOTSPOT
//Network config Rhede
lib/Menu/menuIntInput.cpp
+144 -17
View File
@@ -16,18 +16,143 @@ MenuIntInput::MenuIntInput(int16_t values[], char *names, uint8_t length, MenuIn
this->names = names;
this->length = length;
this->wrapper = wrapper;
this->min = new int16_t[this->length];
this->max = new int16_t[this->length];
this->originalValues = new int16_t[this->length];
this->stepsPerInput = new uint8_t[this->length];
for (uint8_t i = 0; i < this->length; i++) {
this->min[i] = INT16_MIN;
this->max[i] = INT16_MAX;
this->originalValues[i] = this->values[i];
this->stepsPerInput[i] = 1;
}
}
MenuIntInput::MenuIntInput(uint8_t length, MenuIntInputWrapper *wrapper) {
this->length = length;
this->wrapper = wrapper;
this->values = new int16_t[this->length];
this->names = (char*) new char[this->length][MAX_NAME_LENGTH];
this->min = new int16_t[this->length];
this->max = new int16_t[this->length];
this->originalValues = new int16_t[this->length];
this->stepsPerInput = new uint8_t[this->length];
for (uint8_t i = 0; i < this->length; i++) {
this->min[i] = INT16_MIN;
this->max[i] = INT16_MAX;
this->values[i] = 0;
this->originalValues[i] = 0;
this->stepsPerInput[i] = 1;
strcpy(this->names + MAX_NAME_LENGTH * i, "Default");
}
}
MenuIntInput::~MenuIntInput() {
delete this->min;
delete this->max;
delete this->originalValues;
delete this->stepsPerInput;
delete this->values;
delete this->names;
}
void MenuIntInput::setEntry(uint8_t valueNumber, const char* name, int16_t startValue) {
if (this->length < valueNumber)
return;
if (strlen(name) > MAX_NAME_LENGTH)
return;
strcpy(this->names + MAX_NAME_LENGTH * valueNumber, name);
this->values[valueNumber] = startValue;
this->originalValues[valueNumber] = startValue;
}
void MenuIntInput::setMin(uint8_t valueNumber, int16_t min) {
if (this->length < valueNumber)
return;
this->min[valueNumber] = min;
}
void MenuIntInput::setMax(uint8_t valueNumber, int16_t max) {
if (this->length < valueNumber)
return;
this->max[valueNumber] = max;
}
void MenuIntInput::setMinMax(uint8_t valueNumber, int16_t min, int16_t max) {
this->setMin(valueNumber, min);
this->setMax(valueNumber, max);
}
void MenuIntInput::setMin(int16_t min) {
for (uint8_t i = 0; i < this->length; i++)
this->min[i] = min;
}
void MenuIntInput::setMax(int16_t max) {
for (uint8_t i = 0; i < this->length; i++)
this->max[i] = max;
}
void MenuIntInput::setMinMax(int16_t min, int16_t max) {
this->setMin(min);
this->setMax(max);
}
void MenuIntInput::setMinMaxSteps(uint8_t valueNumber, int16_t min, int16_t max, uint8_t steps) {
this->setMin(valueNumber, min);
this->setMax(valueNumber, max);
this->setStepsPerInput(valueNumber, steps);
}
void MenuIntInput::setMinMaxSteps(int16_t min, int16_t max, uint8_t steps) {
this->setMin(min);
this->setMax(max);
this->setStepsPerInput(steps);
}
void MenuIntInput::setStepsPerInput(uint8_t valueNumber, uint8_t steps) {
if (this->length < valueNumber)
return;
this->stepsPerInput[valueNumber] = steps;
}
void MenuIntInput::setStepsPerInput(uint8_t steps) {
for (uint8_t i = 0; i < this->length; i++)
this->stepsPerInput[i] = steps;
}
void MenuIntInput::down() {
if (values[currentPosition] > this->min)
values[currentPosition]--;
if (this->values[this->currentPosition]
- this->stepsPerInput[this->currentPosition]
>= this->min[currentPosition])
{
this->values[this->currentPosition] -= this->stepsPerInput[this->currentPosition];
} else {
this->values[this->currentPosition] = this->max[this->currentPosition];
}
this->printMenu();
}
void MenuIntInput::up() {
if (values[currentPosition] < this->max)
values[currentPosition]++;
if (this->values[this->currentPosition]
+ this->stepsPerInput[this->currentPosition]
<= this->max[this->currentPosition])
{
this->values[this->currentPosition] += this->stepsPerInput[this->currentPosition];
} else {
this->values[this->currentPosition] = this->min[this->currentPosition];
}
this->printMenu();
}
@@ -51,37 +176,42 @@ void MenuIntInput::left() {
}
void MenuIntInput::no() {
if (parentMenu)
for (uint8_t i = 0; i < this->length; i++)
this->values[i] = this->originalValues[i];
this->currentPosition = 0;
if (this->parentMenu)
this->parentMenu->printMenu();
}
void MenuIntInput::yes() {
this->wrapper->action(this->values, this->length);
if (parentMenu)
if (this->parentMenu)
this->parentMenu->printMenu();
}
void MenuIntInput::printMenu() {
// TODO: Name max 12 chars
char bufferName[17];
if (this->currentPosition == 0 && this->length == 1) {
// No arrow
sprintf(bufferName, " %s ", this->names[this->currentPosition]);
sprintf(bufferName, " %s ", this->names + 12 * this->currentPosition);
} else if (this->currentPosition > 0 && this->length - 1 == this->currentPosition) {
// Left arrow only
sprintf(bufferName, "< %s ", this->names[this->currentPosition]);
sprintf(bufferName, "< %s ", this->names + 12 * this->currentPosition);
} else if (this->currentPosition == 0 && this->length > 1) {
// Right arrow only
sprintf(bufferName, " %s >", this->names[this->currentPosition]);
sprintf(bufferName, " %s >", this->names + 12 * this->currentPosition);
} else {
// Both arrow
sprintf(bufferName, "< %s >", this->names[this->currentPosition]);
sprintf(bufferName, "< %s >", this->names + 12 * this->currentPosition);
}
char bufferValue[17];
sprintf(bufferValue, " -> %5.d", this->values[this->currentPosition]);
if (this->values[this->currentPosition])
sprintf(bufferValue, " -> %5.d", this->values[this->currentPosition]);
else
sprintf(bufferValue, " -> 0");
if (this->lcd) {
lcd->clear();
@@ -92,6 +222,3 @@ void MenuIntInput::printMenu() {
}
std::cout << bufferName << " : " << bufferValue << std::endl;
}
lib/Menu/menuIntInput.h
+20 -4
View File
@@ -24,9 +24,22 @@ class MenuIntInputWrapper {
class MenuIntInput : public MenuControl {
public:
MenuIntInput(int16_t values[], char *names, uint8_t length, MenuIntInputWrapper* wrapper);
MenuIntInput(uint8_t length, MenuIntInputWrapper *wrapper);
~MenuIntInput();
void setMin(int16_t min = 0) { this->min = min; }
void setMax(int16_t max = 100) { this->max = max; }
void setEntry(uint8_t valueNumber, const char* name, int16_t startValue = 0);
void setMin(uint8_t valueNumber, int16_t min);
void setMin(int16_t min);
void setMax(uint8_t valueNumber, int16_t max);
void setMax(int16_t max);
void setMinMax(uint8_t valueNumber, int16_t min, int16_t max);
void setMinMax(int16_t min, int16_t max);
void setMinMaxSteps(uint8_t valueNumber, int16_t min, int16_t max, uint8_t steps);
void setMinMaxSteps(int16_t min, int16_t max, uint8_t steps);
void setStepsPerInput(uint8_t valueNumber, uint8_t steps);
void setStepsPerInput(uint8_t steps);
/**
* @brief Decrement selected value
@@ -73,9 +86,12 @@ class MenuIntInput : public MenuControl {
uint8_t currentPosition = 0;
uint8_t length;
uint8_t *stepsPerInput;
uint8_t countSameActions = 0;
int16_t *values;
int16_t min = INT16_MIN;
int16_t max = INT16_MAX;
int16_t *originalValues;
int16_t *min;
int16_t *max;
char *names;
};
lib/Navigation/navigation.cpp
+7 -5
View File
@@ -95,11 +95,13 @@ void Navigation::loop() {
if (millis() - this->lastMillis < this->timeToWait)
return;
uint8_t rtcmData[512 * 4];
uint16_t rtcmCount = this->ntripClient->available();
if (rtcmCount) {
this->ntripClient->readBytes(rtcmData, rtcmCount);
this->gps->pushRawData(rtcmData, rtcmCount);
if (this->isNtripInit) {
uint8_t rtcmData[512 * 4];
uint16_t rtcmCount = this->ntripClient->available();
if (rtcmCount) {
this->ntripClient->readBytes(rtcmData, rtcmCount);
this->gps->pushRawData(rtcmData, rtcmCount);
}
}
}
lib/Ntrip/NTRIPClient.cpp
+249 -79
View File
@@ -1,94 +1,264 @@
#include"NTRIPClient.h"
bool NTRIPClient::reqSrcTbl(char* host,uint16_t port){
if(!connect(host,port)){
Serial.print("Cannot connect to ");
Serial.println(host);
return false;
} /*p = String("GET ") + String("/") + String(" HTTP/1.0\r\n");
p = p + String("User-Agent: NTRIP Enbeded\r\n");*/
print(
"GET / HTTP/1.0\r\n"
"User-Agent: NTRIPClient for Arduino v1.0\r\n"
);
unsigned long timeout = millis();
while (available() == 0) {
if (millis() - timeout > 5000) {
Serial.println("Client Timeout !");
stop();
return false;
}
delay(10);
}
char buffer[50];
readLine(buffer,sizeof(buffer));
if(strncmp((char*)buffer,"SOURCETABLE 200 OK",17)) {
Serial.print((char*)buffer);
return false;
}
return true;
/**
* @file NTRIPClient.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2022-09-18
*
* @copyright Copyright (c) 2022
*
*/
#include "NTRIPClient.h"
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;
this->port = port;
this->mountPoint = mountPoint;
this->user = user;
this->password = password;
this->ntripClient = new WiFiClient;
}
bool NTRIPClient::reqRaw(char* host,uint16_t port,char* mntpnt,char* user,char* psw) {
if(!connect(host,port))return false;
String p="GET /";
String auth="";
Serial.println("Request NTRIP");
p = p + mntpnt + String(" HTTP/1.0\r\n"
"User-Agent: NTRIPClient for Arduino v1.0\r\n"
);
if (strlen(user)==0) {
p = p + String(
"Accept: */*\r\n"
"Connection: close\r\n"
);
}
else {
auth = base64::encode(String(user) + String(":") + psw);
#ifdef Debug
Serial.println(String(user) + String(":") + psw);
#endif
NTRIPClient::~NTRIPClient() {
delete this->ntripClient;
}
p = p + String("Authorization: Basic ");
p = p + auth;
p = p + String("\r\n");
void NTRIPClient::loop() {
if (millis() - this->lastLoopTime < this->delayTime)
return;
this->lastLoopTime = millis();
this->gps->checkUblox();
this->gps->checkCallbacks();
switch (this->state) {
case NTRIPClientStates::openConnection:
if (millis() - this->lastNtripConnectTime > this->tryReconnectTime)
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::pushData;
} else {
uint8_t seconds = this->tryReconnectTime / 1000;
std::cout << "Could not connect to the caster. Trying again in "
<< seconds << " seconds." << std::endl;
this->lastNtripConnectTime = millis();
}
break;
case pushData:
if (!processConnection() || !this->activated)
this->state = NTRIPClientStates::closeConnection;
break;
case NTRIPClientStates::closeConnection:
std::cout << "Closing the connection to the NTRIP caster..." << std::endl;
this->closeConnection();
state = NTRIPClientStates::wait;
break;
case NTRIPClientStates::wait:
if (this->activated)
this->state = NTRIPClientStates::openConnection;
break;
}
p = p + String("\r\n");
print(p);
#ifdef Debug
Serial.println(p);
#endif
unsigned long timeout = millis();
while (available() == 0) {
if (millis() - timeout > 20000) {
Serial.println("Client Timeout !");
}
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->(SFE_UBLOX_DGNSS_MODE_FIXED); // Set the differential mode - ambiguities are fixed whenever possible
this->gps->setNavigationFrequency(1);
this->gps->setMainTalkerID(SFE_UBLOX_MAIN_TALKER_ID_GP);
this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_I2C, 10);
this->gps->setNMEAGPGGAcallbackPtr(&pushGPGGA); // Set up the callback for GPGGA
this->gps->setAutoPVTcallbackPtr(&(NTRIPClient::printPVTdata));
}
bool NTRIPClient::beginClient() {
std::cout << "Opening socket to " << this->host << std::endl;
char serverRequest[this->bufferSize];
char credentials[this->bufferSize];
if (!this->ntripClient->connect(this->host, this->port)) {
std::cout << "Connection to caster failed" << std::endl;
return false;
} else {
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)
snprintf(serverRequest,
this->bufferSize,
"GET /%s HTTP/1.0\r\nUser-Agent: NTRIP SparkFun u-blox Client v1.0\r\n",
this->mountPoint);
// Credentials
uint8_t userCredentialsLength = strlen(this->user) + strlen(this->password) + 1;
char* userCredentials = new char[userCredentialsLength];
snprintf(userCredentials, userCredentialsLength, "%s:%s", this->user, this->password);
std::cout << "Sending credentials: " << userCredentials << std::endl;
//Encode
base64 b;
String strEncodedCredentials = b.encode(userCredentials);
delete userCredentials;
char encodedCredentials[strEncodedCredentials.length() + 1];
strEncodedCredentials.toCharArray(encodedCredentials, sizeof(encodedCredentials));
snprintf(credentials, sizeof(credentials), "Authorization: Basic %s\r\n", encodedCredentials);
}
// Add the encoded credentials to the server request
strncat(serverRequest, credentials, this->bufferSize);
strncat(serverRequest, "\r\n", this->bufferSize);
std::cout << "serverRequest size: "
<< strlen(serverRequest)
<< " of "
<< this->bufferSize
<< " 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
uint32_t lastMillis = millis();
while (!ntripClient->available()) {
if (millis() - lastMillis > this->timeOut) {
std::cout << "Caster timed out!" << std::endl;
this->ntripClient->stop();
return false;
}
delay(10);
delay(10)
}
char buffer[50];
readLine(buffer,sizeof(buffer));
if(strncmp((char*)buffer,"ICY 200 OK",10))
{
Serial.print((char*)buffer);
return false;
//Check reply
uint16_t httpStatusCode = 0;
char response[this->bufferSize];
uint16_t responseIndex = 0;
while (this->ntripClient->available()) {
if (responseIndex == sizeof(response))
break;
response[responseIndex++] = ntripClient->read();
if (httpStatusCode == 0) {
if (strstr(response, "200") != nullptr)
httpStatusCode = 200;
if (strstr(response, "400") != nullptr)
httpStatusCode = 401;
}
}
response[responseIndex] = '\0';
// 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 << "Statuscode 401 - Unauthorized" << std::endl;
return false;
}
std::cout << "Connected to: " << this->host << std::endl;
this->lastReceivedRtcmTime = millis();
return true;
}
bool NTRIPClient::reqRaw(char* host,uint16_t port,char* mntpnt) {
return reqRaw(host,port,mntpnt,"","");
void NTRIPClient::closeConnection() {
if (this->ntripClient->connected())
this->ntripClient->stop();
this->activated = false;
std::cout << "NtripClient disconnected from: " << this->host << std::endl;
}
int NTRIPClient::readLine(char* _buffer,int size) {
int len = 0;
while(available()) {
_buffer[len] = read();
len++;
if(_buffer[len-1] == '\n' || len >= size) break;
}
_buffer[len]='\0';
bool NTRIPClient::processConnection() {
if (this->ntripClient->connected()) {
uint8_t rtcmData[this->bufferSize * 4];
uint16_t rtcmCount = 0;
return len;
while (this->ntripClient->available()) {
rtcmData[rtcmCount++] = ntripClient->read();
if (rtcmCount == sizeof(rtcmData))
break;
}
if (rtcmCount > 0) {
this->lastReceivedRtcmTime = millis();
this->gps->pushRawData(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::pushGPGGA(NMEA_GGA_data_t *nmeaData) {
if (this->ntripClient->connected() && this->transmitLocation) {
std::cout << "Pushing GGA to server: " << (const char *)nmeaData->nmea) << std::endl;
this->ntripClient->print((const char *)nmeaData->nmea);
}
}
void NTRIPClient::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
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(fixTypeString, "None");
else if (carrSoln == 1)
strcpy(fixTypeString, "Floating");
else if (carrSoln == 2)
strcpy(fixTypeString, "Fixed");
else
strcpy(fixTypeString, "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;
}
lib/Ntrip/NTRIPClient.h
+47 -8
View File
@@ -3,16 +3,55 @@
#include <WiFiClient.h>
#include <Arduino.h>
#include<base64.h>
#include <base64.h>
#include <iostream>
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
class NTRIPClient : public WiFiClient{
public :
bool reqSrcTbl(char* host,uint16_t port); //request MountPoints List serviced the NTRIP Caster
bool reqRaw(char* host,uint16_t port,char* mntpnt,char* user,char* psw); //request RAW data from Caster
bool reqRaw(char* host,uint16_t port,char* mntpnt); //non user
int readLine(char* buffer,int size);
enum NTRIPClientStates {
openConnection,
pushData,
closeConnection,
wait
};
class NTRIPClient {
public:
NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, const char* mountPoint, const char* user, const char* password);
~NTRIPClient();
void loop();
void gpsConfiguration();
void pushGPGGA(NMEA_GGA_data_t *nmeaData);
void setTransmitLocation(bool b) { this->transmitLocation = b; }
void setActivated(bool b) { this->activated = b; }
static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
private:
bool beginClient();
void closeConnection();
bool processConnection();
SFE_UBLOX_GNSS* gps;
WiFiClient* ntripClient;
NTRIPClientStates state;
bool transmitLocation = true;
bool activated = true;
uint16_t port;
uint32_t lastReceivedRtcmTime = 0;
uint32_t lastNtripConnectTime = 0;
uint32_t lastLoopTime = 0;
const char* host;
const char* mountPoint;
const char* user;
const char* password;
const uint8_t delayTime = 20;
const uint16_t timeOut = 5000;
const uint16_t tryReconnectTime = 5000;
const uint16_t bufferSize = 512;
};
#endif
platformio.ini
+1 -1
View File
@@ -16,7 +16,6 @@ framework = arduino
monitor_speed = 115200
upload_speed = 921600
monitor_port = COM3
;build_flags = -DCORE_DEBUG_LEVEL=5
lib_deps =
madhephaestus/ESP32Encoder@^0.4.0
jvpernis/PS3 Controller Host@^1.1.0
@@ -24,6 +23,7 @@ lib_deps =
knolleary/PubSubClient@^2.8
br3ttb/PID@^1.2.1
marcoschwartz/LiquidCrystal_I2C@^1.1.4
marian-craciunescu/ESP32Ping@^1.7
upload_port = COM3
[platformio]
src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp
+46 -37
View File
@@ -13,13 +13,11 @@
MenuTestMode::MenuTestMode(DriveManager* driveManager)
: MenuDriveMode(driveManager) {
// this->testMode = testMode;
this->values[0] = 1;
this->values[1] = 2;
this->values[2] = 3;
// this->values[0] = 0;
// this->values[1] = 0;
strcpy(this->names[0], "Alpha");
strcpy(this->names[1], "Beta");
strcpy(this->names[2], "Gamma");
// strcpy(this->names[0], "Centimeter");
// strcpy(this->names[1], "Degree");
}
MenuTestMode::~MenuTestMode() {
@@ -68,6 +66,7 @@ void MenuTestMode::no() {
}
void MenuTestMode::init() {
this->driveManager->changeModus(Modi::TestMode);
this->testMode = (TestMode*) this->driveManager->getDriveModiPtr();
auto dummy = []() {
@@ -77,40 +76,49 @@ void MenuTestMode::init() {
// Create menu
this->mainMenu = new Menu;
Menu* engineMenu = new Menu;
Menu* engineLeftMenu = new Menu;
Menu* engineRightMenu = new Menu;
Menu* drivingMenu = new Menu;
Menu* drivingForwardMenu = new Menu;
Menu* drivingBackwardMenu = new Menu;
Menu* lightMenu = new Menu;
MenuIntInput* testInputMenu = new MenuIntInput(values, (char *)names, length, new MenuTestModeWrapper(this->testMode));
MenuIntInput* drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::drive));
MenuIntInput* engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::leftEngine));
MenuIntInput* engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::rightEngine));
MenuIntInput* engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::bothEngine));
// Set menus on LCD
this->mainMenu->setLcd(this->lcd);
engineMenu->setLcd(this->lcd);
engineLeftMenu->setLcd(this->lcd);
engineRightMenu->setLcd(this->lcd);
engineBothMenu->setLcd(this->lcd);
drivingMenu->setLcd(this->lcd);
drivingForwardMenu->setLcd(this->lcd);
drivingBackwardMenu->setLcd(this->lcd);
lightMenu->setLcd(this->lcd);
testInputMenu->setLcd(this->lcd);
// Other menu config
drivingMenu->setEntry(0, "Centimeter", 100);
drivingMenu->setMinMaxSteps(0, -1000, 1000, 25);
drivingMenu->setEntry(1, "Degree");
drivingMenu->setMinMaxSteps(1, -360, 360, 15);
engineLeftMenu->setEntry(0, "Percentage");
engineLeftMenu->setMinMaxSteps(0, -100, 100, 5);
engineLeftMenu->setEntry(1, "Seconds", 10);
engineLeftMenu->setMinMaxSteps(1, 0, 120, 1);
engineRightMenu->setEntry(0, "Percentage");
engineRightMenu->setMinMaxSteps(0, -100, 100, 5);
engineRightMenu->setEntry(1, "Seconds", 10);
engineRightMenu->setMinMaxSteps(1, 0, 120, 1);
engineBothMenu->setEntry(0, "Percentage");
engineBothMenu->setMinMaxSteps(0, -100, 100, 5);
engineBothMenu->setEntry(1, "Seconds", 10);
engineBothMenu->setMinMaxSteps(1, 0, 120, 1);
// Entrys for the menus
MenuAction* engineAction = new MenuAction("Engine", engineMenu);
MenuAction* engineLeftAction = new MenuAction("Left", engineLeftMenu);
MenuAction* engineRightAction = new MenuAction("Right", engineRightMenu);
MenuAction* engineBothAction = new MenuAction("Both", engineBothMenu);
MenuAction* drivingAction = new MenuAction("Driving", drivingMenu);
MenuAction* drivingForwardAction = new MenuAction("Forward", drivingForwardMenu);
MenuAction* drivingForwardLeftAction = new MenuAction("Left", testInputMenu);
MenuAction* drivingForwardStraightAction = new MenuAction("Straight", dummy);
MenuAction* drivingForwardRightAction = new MenuAction("Right", dummy);
MenuAction* drivingBackwardAction = new MenuAction("Backward", drivingBackwardMenu);
MenuAction* drivingBackwardLeftAction = new MenuAction("Left", dummy);
MenuAction* drivingBackwardStraightAction = new MenuAction("Straight", dummy);
MenuAction* drivingBackwardRightAction = new MenuAction("Right", dummy);
MenuAction* lightAction = new MenuAction("Light", lightMenu);
MenuAction* lightFlashAction = new MenuAction("Flash", dummy);
@@ -127,15 +135,7 @@ void MenuTestMode::init() {
engineMenu->addEntry(engineLeftAction);
engineMenu->addEntry(engineRightAction);
drivingMenu->addEntry(drivingForwardAction);
drivingMenu->addEntry(drivingBackwardAction);
drivingForwardMenu->addEntry(drivingForwardLeftAction);
drivingForwardMenu->addEntry(drivingForwardStraightAction);
drivingForwardMenu->addEntry(drivingForwardRightAction);
drivingBackwardMenu->addEntry(drivingBackwardLeftAction);
drivingBackwardMenu->addEntry(drivingBackwardStraightAction);
drivingBackwardMenu->addEntry(drivingBackwardRightAction);
engineMenu->addEntry(engineBothAction);
lightMenu->addEntry(lightFlashAction);
lightMenu->addEntry(lightFadeAction);
@@ -145,13 +145,22 @@ void MenuTestMode::init() {
lightMenu->addEntry(lightOffAction);
}
MenuTestModeWrapper::MenuTestModeWrapper(TestMode* testMode) {
MenuTestModeWrapper::MenuTestModeWrapper(TestMode* testMode, TestModeFunctionSingle testModeFunction) {
this->testMode = testMode;
this->testModeFunctionSingle = testModeFunction;
}
MenuTestModeWrapper::MenuTestModeWrapper(TestMode* testMode, TestModeFunctionDouble testModeFunction) {
this->testMode = testMode;
this->testModeFunctionDouble = testModeFunction;
}
void MenuTestModeWrapper::action(int16_t* values, uint8_t length) {
if (length > 2)
std::cout << "Dummy in Action" << values[0] << values[1] << values[2] << std::endl;
else
std::cout << "Dummy in Action kene dre values" << std::endl;
// This function calls a member function with a pointer
// Very helpful site:
// https://isocpp.org/wiki/faq/pointers-to-members#fnptr-vs-memfnptr-types
if (length == 2 && this->testModeFunctionDouble)
(this->testMode->*this->testModeFunctionDouble)(values[0], values[1]);
else if (length == 1 && this->testModeFunctionSingle)
(this->testMode->*this->testModeFunctionSingle)(values[0]);
}
src/SpecialMenus/driveModi/TestMode/menuTestMode.h
+6 -5
View File
@@ -37,20 +37,21 @@ class MenuTestMode : public MenuDriveMode {
Menu* mainMenu;
bool isInit = false;
uint8_t length = 3;
int16_t values[3];
char names[3][MAX_NAME_LENGTH];
};
typedef bool (TestMode::*TestModeFunctionSingle)(int16_t);
typedef bool (TestMode::*TestModeFunctionDouble)(int16_t, int16_t);
class MenuTestModeWrapper : public MenuIntInputWrapper {
public:
MenuTestModeWrapper(TestMode* testMode);
MenuTestModeWrapper(TestMode* testMode, TestModeFunctionSingle testModeFunction);
MenuTestModeWrapper(TestMode* testMode, TestModeFunctionDouble testModeFunction);
void action(int16_t* values, uint8_t length) override;
private:
TestMode* testMode;
TestModeFunctionSingle testModeFunctionSingle = nullptr;
TestModeFunctionDouble testModeFunctionDouble = nullptr;
};
#endif // MENU_TEST_MODE
src/driveModi/Modi/TestMode/testMode.cpp
+85 -69
View File
@@ -12,12 +12,9 @@
TestMode::TestMode(MoveControl *moveControl) {
this->moveControl = moveControl;
this->moveControl->setDrivingStatus(DrivingStatus::drive);
}
TestMode::~TestMode() {
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
this->moveControl->setDrivingStatus(DrivingStatus::stop);
}
@@ -27,98 +24,117 @@ void TestMode::loop() {
if (millis() - this->actionStart > this->maneuverTime) {
this->busy = false;
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
this->moveControl->setDrivingStatus(DrivingStatus::stop);
}
}
void TestMode::setSpeed(double speed) {
this->speed = speed;
void TestMode::setSpeed(int16_t speed) {
this->speed = (double) speed / 100.0;
}
void TestMode::setRotationSpeed(double speed) {
this->rotationSpeed = speed;
void TestMode::setRotationSpeed(int16_t speed) {
this->rotationSpeed = (double) speed / 100.0;
}
bool TestMode::driveForward(uint16_t cm) {
bool TestMode::drive(int16_t cm, int16_t degree) {
if (this->busy)
return false;
this->moveControl->setRotationSpeed(0);
this->moveControl->setSpeed(this->speed);
this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
if (true) {
std::cout << "It works... cm: " << cm << " degree: " << degree << std::endl;
return false;
}
this->actionStart = millis();
this->moveControl->setDrivingStatus(DrivingStatus::drive);
if (cm == 0) {
//Only left or right
this->moveControl->setSpeed(0);
this->busy = true;
return true;
}
if (degree < 0)
this->moveControl->setRotationSpeed(-this->rotationSpeed);
else if (degree > 0)
this->moveControl->setRotationSpeed(this->rotationSpeed);
//TODO: calc this shit
this->maneuverTime = 10;
this->busy = true;
return true;
bool TestMode::driveBackward(uint16_t cm) {
if (this->busy)
return false;
} else if (degree == 0) {
//Only forward or backward
this->moveControl->setRotationSpeed(0);
this->moveControl->setRotationSpeed(0);
this->moveControl->setSpeed(-this->speed);
if (cm < 0)
this->moveControl->setSpeed(-this->speed);
else if (cm > 0)
this->moveControl->setSpeed(this->speed);
this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
this->busy = true;
return true;
} else {
//forward or backward and left or right
}
this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
this->actionStart = millis();
this->busy = true;
return true;
}
bool TestMode::turnLeft(uint16_t degree) {
if (this->busy)
return false;
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(this->rotationSpeed);
//TODO: calc this shit
this->maneuverTime = 10;
this->actionStart = millis();
this->busy = true;
return true;
}
bool TestMode::turnRight(uint16_t degree) {
if (this->busy)
return false;
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(this->rotationSpeed);
//TODO: calc this shit
this->maneuverTime = 10;
this->actionStart = millis();
this->busy = true;
return true;
}
bool TestMode::driveForwardLeft(uint16_t cm, uint16_t degree) {
if (this->busy)
return false;
this->moveControl->setDrivingStatus(DrivingStatus::stop);
return false;
}
bool TestMode::driveForwardRight(uint16_t cm, uint16_t degree) {
bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
if (this->busy)
return false;
return false;
if (powerPercentage >= 100 || powerPercentage <= -100)
return false;
if (true) {
std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
return false;
}
this->actionStart = millis();
this->moveControl->setDrivingStatus(DrivingStatus::raw);
this->moveControl->setRawPowerLeft(powerPercentage);
this->maneuverTime = seconds * 1000;
this->busy = true;
}
bool TestMode::driveBackwardLeft(uint16_t cm, uint16_t degree) {
bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
if (this->busy)
return false;
return false;
if (powerPercentage >= 100 || powerPercentage <= -100)
return false;
if (true) {
std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
return false;
}
this->actionStart = millis();
this->moveControl->setDrivingStatus(DrivingStatus::raw);
this->moveControl->setRawPowerRight(powerPercentage);
this->maneuverTime = seconds * 1000;
this->busy = true;
}
bool TestMode::driveBackwardRight(uint16_t cm, uint16_t degree) {
bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
if (this->busy)
return false;
return false;
}
if (powerPercentage >= 100 || powerPercentage <= -100)
return false;
if (true) {
std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
return false;
}
this->actionStart = millis();
this->moveControl->setDrivingStatus(DrivingStatus::raw);
this->moveControl->setRawPowerLeft(powerPercentage);
this->moveControl->setRawPowerRight(powerPercentage);
this->maneuverTime = seconds * 1000;
this->busy = true;
}
src/driveModi/Modi/TestMode/testMode.h
+6 -11
View File
@@ -21,19 +21,14 @@ class TestMode : public DriveModi {
void loop() override;
void setSpeed(double speed);
void setRotationSpeed(double speed);
void setSpeed(int16_t speed);
void setRotationSpeed(int16_t speed);
bool driveForward(uint16_t cm);
bool driveBackward(uint16_t cm);
bool drive(int16_t cm = 0, int16_t degree = 0);
bool turnLeft(uint16_t degree);
bool turnRight(uint16_t degree);
bool driveForwardLeft(uint16_t cm, uint16_t degree);
bool driveForwardRight(uint16_t cm, uint16_t degree);
bool driveBackwardLeft(uint16_t cm, uint16_t degree);
bool driveBackwardRight(uint16_t cm, uint16_t degree);
bool leftEngine(int16_t powerPercentage, int16_t seconds);
bool rightEngine(int16_t powerPercentage, int16_t seconds);
bool bothEngine(int16_t powerPercentage, int16_t seconds);
private:
MoveControl *moveControl;
src/driveModi/driveManager.cpp
+2
View File
@@ -20,6 +20,8 @@ DriveManager::DriveManager(MoveControl *moveControl, bool wifi) {
this->navigation = new Navigation();
if (wifi)
this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD);
// if (wifi)
// std::cout << "Wifi is true" << std::endl;
}
DriveManager::~DriveManager() {
src/main.cpp
+10 -2
View File
@@ -19,6 +19,7 @@
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <BluetoothSerial.h>
#include <ESP32Ping.h>
#include "config.h"
@@ -74,8 +75,16 @@ void setup() {
Network::setIps();
Network::setupMQTT();
wifiIsActive = Network::connectWifi();
driveManager = new DriveManager(&moveController, wifiIsActive);
if (wifiIsActive) {
driveManager = new DriveManager(&moveController, true);
const char* remoteHost = "www.google.com";
std::cout << "Pinging host: " << remoteHost << std::endl;
if (Ping.ping(remoteHost))
std::cout << "Ping successful..." << std::endl;
else
std::cout << "Ping failed. :/" << std::endl;
if (Network::checkMQTT()) {
DebugMqtt::init(Network::getMqttClient(), Loglevel::debug);
std::cout << "Activate additional output via MQTT..." << std::endl;
@@ -85,7 +94,6 @@ void setup() {
outputBuf = new OutputBuf();
wifiIndicator = '-';
} else {
driveManager = new DriveManager(&moveController);
outputBuf = new OutputBuf();
}
std::cout.rdbuf(outputBuf);
src/moveControl.cpp
+19
View File
@@ -81,6 +81,10 @@ void MoveControl::runMoveControl() {
}
void MoveControl::setDrivingStatus(DrivingStatus status) {
this->setSpeed(0);
this->setRotationSpeed(0);
this->setRawPowerLeft(0);
this->setRawPowerRight(0);
this->driving_status = status;
// switch (this->driving_status) {
// case DrivingStatus::stop :
@@ -111,6 +115,16 @@ void MoveControl::setRotationSpeed(double speed) {
this->rotation_speed = speed;
}
void MoveControl::setRawPowerLeft(int16_t power) {
if (power <= 100 && power >= 100)
this->rawPowerLeft = power;
}
void MoveControl::setRawPowerRight(int16_t power) {
if (power <= 100 && power >= 100)
this->rawPowerRight = power;
}
void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
PID* selectedPID = nullptr;
if (side == 0)
@@ -165,6 +179,11 @@ void MoveControl::regulateMotors() {
this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
break;
case DrivingStatus::raw :
this->left_motor->setTargetPower(this->rawPowerLeft);
this->right_motor->setTargetPower(this->rawPowerRight);
break;
default:
Serial.println("Wrong drivingState in MoveControl::regulateMotors");
break;