Merge branch 'main' into projektarbeit
This commit is contained in:
+177
-59
@@ -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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user