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

Merge branch 'main' into projektarbeit

Browse Source
This commit is contained in:
kleiax
2023-10-12 20:35:31 +02:00
parent f78804d836 451517e7cd
commit e511afb2a5
80 changed files with 5031 additions and 3385 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/Sensors/sensorData.cpp
+177 -59
View File
@@ -1,107 +1,195 @@
/**
* @file senors.cpp
* @file sensorData.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief
* @version 0.1
* @date 2023-09-02
*
*
* @copyright Copyright (c) 2023
*
*
*/
#include "sensorData.h"
bool SensorData::outputStatusPrintPVTdata = false;
bool SensorData::newData = false;
uint32_t SensorData::ubxUpdateTimeStatic = 0;
UBX_NAV_PVT_data_t* SensorData::ubxDataStatic = nullptr;
UBX_NAV_PVT_data_t *SensorData::ubxDataStatic = nullptr;
SensorData::SensorData() {
this->loopDelay = 50;
SensorData::SensorData()
{
Component::loopDelay = SensorData::loopDelay;
}
void SensorData::enableGnss(SPIClass* spiPort, uint8_t csPin) {
void SensorData::enableGnss(SPIClass *spiPort, uint8_t csPin)
{
this->gnss = new SFE_UBLOX_GNSS();
if (this->gnss->begin(*spiPort, csPin, 4000000) == false) {
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);
while (true)
{
}
}
this->initGnss();
}
void SensorData::enableGnss() {
void SensorData::enableGnss()
{
this->gnss = new SFE_UBLOX_GNSS();
if (this->gnss->begin() == false) {
if (this->gnss->begin() == false)
{
std::cout << "u-blox GNSS not detected at default I2C address. Please check wiring. Freezing." << std::endl;
while (1);
while (true)
{
}
}
this->initGnss();
}
void SensorData::enableRealCompass() {
void SensorData::enableRealCompass()
{
static constexpr byte address = 0x0d;
this->realCompass = new QMC5883LCompass();
// Init Compass
Wire.beginTransmission(0x0d);
Wire.beginTransmission(address);
// TODO: describe Bytes !!!
Wire.write(0x0b);
Wire.write(0x01);
Wire.endTransmission();
this->realCompass->setMode(0x01,0x0C,0x10,0X00);
this->realCompass->setMode(0x01, 0x0C, 0x10, 0X00);
CalibrateCompass caliCompass(this->realCompass);
caliCompass.loadData();
caliCompass.useData();
}
void SensorData::enableCalcCompass() {
void SensorData::enableCalcCompass()
{
// TODO: !!! implementieren
}
void SensorData::enableGyroskop() {
void SensorData::enableGyroscope()
{
this->gyroscope = new MPU6050();
this->gyroscope->initialize();
if (!this->gyroscope->testConnection())
{
std::cout << "SensorData::enableGyroscope: Gyroskop is not conntected. Freeze!" << std::endl;
while (true)
{
}
}
const uint8_t deviceStatus = this->gyroscope->dmpInitialize();
// TODO: !!! MagicNumer 6x
this->gyroscope->setXGyroOffset(220);
this->gyroscope->setYGyroOffset(76);
this->gyroscope->setZGyroOffset(-85);
this->gyroscope->setZAccelOffset(1788);
if (deviceStatus == 0)
{
this->gyroscope->CalibrateAccel(6);
this->gyroscope->CalibrateGyro(6);
this->gyroscope->PrintActiveOffsets();
this->gyroscope->setDMPEnabled(true);
}
else
{
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
std::cout << "SensorData::enableGyroscope: DMP Initialization failed (code" << static_cast<int>(deviceStatus) << "). Freeze!" << std::endl;
while (true)
{
}
}
}
CalcAzimuth::State SensorData::getCalcAzimuthState() const {
if (this->calcCompass)
CalcAzimuth::State SensorData::getCalcAzimuthState() const
{
if (static_cast<bool>(this->calcCompass))
{
return this->calcCompass->getState();
}
return CalcAzimuth::State::Invalid;
}
void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
NTRIPClientStates SensorData::getNtripState() const
{
if (static_cast<bool>(this->ntripClient))
{
return this->ntripClient->getClientState();
}
return NTRIPClientStates::notAvailable;
}
void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct)
{
static constexpr uint8_t stringSize = 32;
if (!SensorData::outputStatusPrintPVTdata)
{
return;
}
double latitude = (double) ubxDataStruct->lat / 10000000.0;
double longitude = (double) ubxDataStruct->lon / 10000000.0;
double altitude = (double) ubxDataStruct->hMSL / 1000.0;
const double latitude = ubxDataStruct->lat / 10000000.0;
const double longitude = ubxDataStruct->lon / 10000000.0;
const double altitude = ubxDataStruct->hMSL / 1000.0;
uint8_t fixType = ubxDataStruct->fixType;
char fixTypeString[32];
const uint8_t fixType = ubxDataStruct->fixType;
char fixTypeString[stringSize];
if (fixType == 0)
strcpy(fixTypeString, "None");
{
strcpy(fixTypeString, static_cast<const char *>("None"));
}
else if (fixType == 1)
strcpy(fixTypeString, "Dead Reckoning");
{
strcpy(fixTypeString, static_cast<const char *>("Dead Reckoning"));
}
else if (fixType == 2)
strcpy(fixTypeString, "2D");
{
strcpy(fixTypeString, static_cast<const char *>("2D"));
}
else if (fixType == 3)
strcpy(fixTypeString, "3D");
{
strcpy(fixTypeString, static_cast<const char *>("3D"));
}
else if (fixType == 3)
strcpy(fixTypeString, "GNSS + Dead Reckoning");
{
strcpy(fixTypeString, static_cast<const char *>("GNSS + Dead Reckoning"));
}
else if (fixType == 5)
strcpy(fixTypeString, "Time Only");
{
strcpy(fixTypeString, static_cast<const char *>("Time Only"));
}
else
strcpy(fixTypeString, "UNKNOWN");
{
strcpy(fixTypeString, static_cast<const char *>("UNKNOWN"));
}
uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
char carrSolnString[16];
const uint8_t carrSoln = ubxDataStruct->flags.bits.carrSoln;
char carrSolnString[stringSize];
if (carrSoln == 0)
strcpy(carrSolnString, "None");
{
strcpy(carrSolnString, static_cast<const char *>("None"));
}
else if (carrSoln == 1)
strcpy(carrSolnString, "Floating");
{
strcpy(carrSolnString, static_cast<const char *>("Floating"));
}
else if (carrSoln == 2)
strcpy(carrSolnString, "Fixed");
{
strcpy(carrSolnString, static_cast<const char *>("Fixed"));
}
else
strcpy(carrSolnString, "UNKNOWN");
{
strcpy(carrSolnString, static_cast<const char *>("UNKNOWN"));
}
uint32_t hAcc = ubxDataStruct->hAcc;
const uint32_t hAcc = ubxDataStruct->hAcc;
std::cout << "Lat: " << latitude
<< " Lng: " << longitude
@@ -112,35 +200,53 @@ void SensorData::printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct) {
<< " Horizontal Accuracy Estimate: " << hAcc << " mm" << std::endl;
}
void SensorData::savePVTdata(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();
}
void SensorData::setOutputStatusPrintPVTdata(bool status) {
void SensorData::setOutputStatusPrintPVTdata(bool status)
{
SensorData::outputStatusPrintPVTdata = status;
}
void SensorData::run() {
this->realCompass->read();
this->realAzimuth = this->realCompass->getAzimuth();
void SensorData::run()
{
if (static_cast<bool>(this->realCompass))
{
this->realCompass->read();
this->realAzimuth = this->realCompass->getAzimuth();
}
if (static_cast<bool>(this->gyroscope) && this->gyroscope->dmpGetCurrentFIFOPacket(static_cast<uint8_t *>(this->gyroBuffer)))
{
this->gyroscope->dmpGetQuaternion(&this->quaternion, static_cast<uint8_t *>(this->gyroBuffer));
this->gyroscope->dmpGetGravity(&this->gravity, &this->quaternion);
this->gyroscope->dmpGetYawPitchRoll(static_cast<float *>(this->yawPitchRoll), &this->quaternion, &this->gravity);
}
}
void SensorData::runAsChild() {
this->gnss->checkUblox();
this->gnss->checkCallbacks();
if (SensorData::newData)
SensorData::newData = false;
void SensorData::runAsChild()
{
if (static_cast<bool>(this->gnss))
{
this->gnss->checkUblox();
this->gnss->checkCallbacks();
if (SensorData::ubxUpdateTimeStatic != this->lastUbxUpdate)
{
this->updateUbxData();
}
}
}
void SensorData::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;
void SensorData::initGnss()
{
const uint8_t versionHigh = this->gnss->getProtocolVersionHigh();
const uint8_t versionLow = this->gnss->getProtocolVersionLow();
std::cout << "u-blox protocol version: " << static_cast<int>(versionHigh) << "." << static_cast<int>(versionLow) << std::endl;
this->gnss->setSPIOutput(COM_TYPE_UBX);
this->gnss->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
@@ -150,3 +256,15 @@ 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{0, 0};
coords.lat = this->gnssData->lat / 10000000.0;
coords.lon = this->gnssData->lon / 10000000.0;
this->currentPosition = Point(coords, this->gnssData->hAcc);
}
lib/Sensors/sensorData.h
+142 -52
View File
@@ -1,18 +1,19 @@
/**
* @file sensorData.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @brief Contains the SensorData class
* @version 0.1
* @date 2023-09-02
*
*
* @copyright Copyright (c) 2023
*
*
*/
#ifndef SENSOR_DATA_H
#define SENSOR_DATA_H
#include <SPI.h>
#include <I2Cdev.h>
#include <iostream>
#include "component.h"
@@ -20,70 +21,159 @@
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include <QMC5883LCompass.h>
// Gyroskop
#include <MPU6050_6Axis_MotionApps20.h>
#include "calcAzimuth.h"
#include "point.h"
class Sensors;
class SensorData : public Component {
public:
SensorData();
void enableGnss(SPIClass* spiPort, uint8_t csPin);
void enableGnss();
void enableRealCompass();
void enableCalcCompass();
void enableGyroskop();
/**
* @brief A class to manage all sensors
*
* Each sensor have separately to be enabled
*/
class SensorData : public Component
{
public:
SensorData();
~SensorData();
// Interface Const kram
int16_t getRealAzimuth() const { return this->realAzimuth; }
int16_t getCalcAzimuth() const { return this->calcAzimuth; }
CalcAzimuth::State getCalcAzimuthState() const;
void enableNtrip(String host, uint16_t port, String mountPoint, String user, String password);
Point getCurrentPos() const { return this->currentPosition; }
const UBX_NAV_PVT_data_t* getGnssData() const { return this->gnssData; };
const void* const getGyroData() const;
/**
* @brief Enable the gnss module over spi
*
* @param spiPort
* @param csPin
*/
void enableGnss(SPIClass *spiPort, uint8_t csPin);
CalcAzimuth* getCalcCompass() const { return this->calcCompass; }
QMC5883LCompass* getRealCompass() const { return this->realCompass; }
/**
* @brief Enable the gnss module over i2c
*/
void enableGnss();
// static
/**
* @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);
void enableRealCompass();
void enableCalcCompass();
void enableGyroscope();
private:
void run() override;
void runAsChild() override;
void initGnss();
// Interface Const
/**
* @brief Get the azimuth measured by the compass module
*
* @return int16_t
*/
int16_t getRealAzimuth() const { return this->realAzimuth; }
QMC5883LCompass* realCompass = nullptr;
CalcAzimuth* calcCompass = nullptr;
SFE_UBLOX_GNSS* gnss = nullptr;
/**
* @brief Get the azimuth calculated by CalcAzimuth
*
* Consider to call getCalcAzimuthState() to check, if the data is valid.
*
* @return int16_t
*/
int16_t getCalcAzimuth() const { return this->calcAzimuth; }
UBX_NAV_PVT_data_t* gnssData;
Point currentPosition;
/**
* @brief Get the CalcAzimuth::State object
*
* Needed to check the quality of calculated azimuth
*
* @return CalcAzimuth::State
*/
CalcAzimuth::State getCalcAzimuthState() const;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
Point getCurrentPos() const { return this->currentPosition; }
const UBX_NAV_PVT_data_t *getGnssData() const { return this->gnssData; };
NTRIPClientStates getNtripState() const;
// static
static void printPVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
static void savePVTdata(UBX_NAV_PVT_data_t *ubxDataStruct);
/**
* @brief Get the data from the gyroscope
*
* The returned float pointer is an array of 3 floats
* - Yaw
* - Pitch
* - Roll
*
* @return const float*
*/
const float *getGyroData() const { return this->yawPitchRoll; }
static UBX_NAV_PVT_data_t* ubxDataStatic;
/**
* @brief Get the CalcCompass object
* @return CalcAzimuth*
*/
CalcAzimuth *getCalcCompass() const { return this->calcCompass; }
static uint32_t ubxUpdateTimeStatic;
/**
* @brief Get the RealCompass object
* @return QMC5883LCompass*
*/
QMC5883LCompass *getRealCompass() const { return this->realCompass; }
static bool outputStatusPrintPVTdata;
static bool newData;
/**
* @brief Get the NTRIPClient object
* @return NTRIPClient*
*/
NTRIPClient *getNtripClient() const { return this->ntripClient; }
/**
* @brief Get the Gyroscope object
* @return MPU6050*
*/
MPU6050 *getGyroscope() const { return this->gyroscope; }
// static
/**
* @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 run() override;
void runAsChild() override;
void initGnss();
void updateUbxData();
QMC5883LCompass *realCompass = nullptr;
CalcAzimuth *calcCompass = nullptr;
SFE_UBLOX_GNSS *gnss = nullptr;
NTRIPClient *ntripClient = nullptr;
MPU6050 *gyroscope = nullptr;
UBX_NAV_PVT_data_t *gnssData = nullptr;
Point currentPosition;
Quaternion quaternion;
VectorFloat gravity;
char *host = nullptr;
char *mountPoint = nullptr;
char *user = nullptr;
char *password = nullptr;
bool isNtripInit = false;
uint8_t gyroBuffer[64];
uint16_t port = 0;
int16_t realAzimuth = INT16_MAX;
int16_t calcAzimuth = INT16_MAX;
uint32_t lastUbxUpdate = 0;
float yawPitchRoll[3]{0, 0, 0};
// static
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 constexpr uint8_t loopDelay = 50;
};
#endif //SENSOR_DATA_H
#endif // SENSOR_DATA_H