134 lines
3.9 KiB
C++
134 lines
3.9 KiB
C++
/**
|
|
* @file senors.cpp
|
|
* @author Alexander Klein (alex@kleiax.de)
|
|
* @brief
|
|
* @version 0.1
|
|
* @date 2023-09-02
|
|
*
|
|
* @copyright Copyright (c) 2023
|
|
*
|
|
*/
|
|
|
|
#include "sensors.h"
|
|
|
|
Sensors::Sensors() {
|
|
this->sensorData = new SensorData(this);
|
|
}
|
|
|
|
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::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::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();
|
|
}
|