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

- broke nearly everthing

Browse Source 
- create Sensor Manager
This commit is contained in:
kleiax
2023-09-02 18:30:33 +02:00
parent daeccf7288
commit b6764e54da
6 changed files with 212 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Navigation/navigation.cpp
+1 -110
View File
@@ -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();
}
lib/Navigation/navigation.h
+4 -48
View File
@@ -12,15 +12,12 @@
#ifndef NAVIGATION_H
#define NAVIGATION_H
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <QMC5883LCompass.h>
#include <Arduino.h>
#include <iostream>
#include <SPI.h>
#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
lib/Sensors/senors.cpp
+129
View File
@@ -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();
}
lib/Sensors/sensorData.h
View File
lib/Sensors/sensors.h
+77
View File
@@ -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 <SPI.h>
#include <iostream>
//Sensors
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <QMC5883LCompass.h>
// 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