clangtidy corrections part 2
This commit is contained in:
+138
-71
@@ -1,30 +1,32 @@
|
||||
/**
|
||||
* @file senors.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;
|
||||
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;
|
||||
}
|
||||
|
||||
SensorData::~SensorData() {
|
||||
if (this->ntripClient)
|
||||
delete this->ntripClient;
|
||||
SensorData::~SensorData()
|
||||
{
|
||||
delete this->ntripClient;
|
||||
}
|
||||
|
||||
void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, String user, String password) {
|
||||
void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, String user, String password)
|
||||
{
|
||||
this->ntripClient = new NTRIPClient(this->gnss, host.c_str(), port, mountPoint.c_str(), user.c_str(), password.c_str());
|
||||
this->ntripClient->gpsConfiguration();
|
||||
this->ntripClient->loop();
|
||||
@@ -33,120 +35,176 @@ void SensorData::enableNtrip(String host, uint16_t port, String mountPoint, Stri
|
||||
this->addChildComponent(this->ntripClient);
|
||||
}
|
||||
|
||||
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::enableGyroskop()
|
||||
{
|
||||
this->gyroskop = new MPU6050();
|
||||
this->gyroskop->initialize();
|
||||
if (!this->gyroskop->testConnection()) {
|
||||
if (!this->gyroskop->testConnection())
|
||||
{
|
||||
std::cout << "SensorData::enableGyroskop: Gyroskop is not conntected. Freeze!" << std::endl;
|
||||
while (true);
|
||||
while (true)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t deviceStatus = this->gyroskop->dmpInitialize();
|
||||
const uint8_t deviceStatus = this->gyroskop->dmpInitialize();
|
||||
|
||||
// TODO: !!! MagicNumer 6x
|
||||
this->gyroskop->setXGyroOffset(220);
|
||||
this->gyroskop->setYGyroOffset(76);
|
||||
this->gyroskop->setZGyroOffset(-85);
|
||||
this->gyroskop->setZAccelOffset(1788);
|
||||
|
||||
if (deviceStatus == 0) {
|
||||
|
||||
if (deviceStatus == 0)
|
||||
{
|
||||
this->gyroskop->CalibrateAccel(6);
|
||||
this->gyroskop->CalibrateGyro(6);
|
||||
this->gyroskop->PrintActiveOffsets();
|
||||
this->gyroskop->setDMPEnabled(true);
|
||||
} else {
|
||||
}
|
||||
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::enableGyroskop: DMP Initialization failed (code" << (int) deviceStatus <<"). Freeze!" << std::endl;
|
||||
while (true);
|
||||
std::cout << "SensorData::enableGyroskop: 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;
|
||||
}
|
||||
|
||||
NTRIPClientStates SensorData::getNtripState() const {
|
||||
if (this->ntripClient)
|
||||
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) {
|
||||
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
|
||||
@@ -157,45 +215,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::ubxDataStatic = ubxDataStruct;
|
||||
SensorData::ubxUpdateTimeStatic = millis();
|
||||
}
|
||||
|
||||
void SensorData::setOutputStatusPrintPVTdata(bool status) {
|
||||
void SensorData::setOutputStatusPrintPVTdata(bool status)
|
||||
{
|
||||
SensorData::outputStatusPrintPVTdata = status;
|
||||
}
|
||||
|
||||
void SensorData::run() {
|
||||
if (this->realCompass) {
|
||||
void SensorData::run()
|
||||
{
|
||||
if (static_cast<bool>(this->realCompass))
|
||||
{
|
||||
this->realCompass->read();
|
||||
this->realAzimuth = this->realCompass->getAzimuth();
|
||||
}
|
||||
|
||||
if (this->gyroskop
|
||||
&& this->gyroskop->dmpGetCurrentFIFOPacket(this->gyroBuffer))
|
||||
if (static_cast<bool>(this->gyroskop) && this->gyroskop->dmpGetCurrentFIFOPacket(static_cast<uint8_t *>(this->gyroBuffer)))
|
||||
{
|
||||
this->gyroskop->dmpGetQuaternion(&this->quaternion, this->gyroBuffer);
|
||||
this->gyroskop->dmpGetQuaternion(&this->quaternion, static_cast<uint8_t *>(this->gyroBuffer));
|
||||
this->gyroskop->dmpGetGravity(&this->gravity, &this->quaternion);
|
||||
this->gyroskop->dmpGetYawPitchRoll(this->yawPitchRoll, &this->quaternion, &this->gravity);
|
||||
this->gyroskop->dmpGetYawPitchRoll(static_cast<float *>(this->yawPitchRoll), &this->quaternion, &this->gravity);
|
||||
}
|
||||
}
|
||||
|
||||
void SensorData::runAsChild() {
|
||||
if (this->gnss) {
|
||||
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);
|
||||
@@ -206,11 +272,12 @@ void SensorData::initGnss() {
|
||||
this->gnss->setAutoPVT(true);
|
||||
}
|
||||
|
||||
void SensorData::updateUbxData() {
|
||||
void SensorData::updateUbxData()
|
||||
{
|
||||
this->gnssData = SensorData::ubxDataStatic;
|
||||
this->lastUbxUpdate = SensorData::ubxUpdateTimeStatic;
|
||||
|
||||
Point::Coordinates coords;
|
||||
Point::Coordinates coords{0, 0};
|
||||
coords.lat = this->gnssData->lat / 10000000.0;
|
||||
coords.lon = this->gnssData->lon / 10000000.0;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user