eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

finished GPS handler refactoring
Some checks failed
EIVE/eive-obsw/pipeline/pr-develop There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Müller
2022-01-17 11:31:50 +01:00
parent 858fd07c4a
commit 37ae508af5
4 changed files with 106 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

151
mission/devices/GPSHyperionHandler.cpp
View File

@ -4,6 +4,10 @@
#include "fsfw/datapool/PoolReadGuard.h"
#include "fsfw/timemanager/Clock.h"
#include <gps.h>
#include <libgpsmm.h>
#include <cmath>
#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
#include <filesystem>
#include <fstream>
@ -18,67 +22,7 @@ GPSHyperionHandler::GPSHyperionHandler(object_id_t objectId, object_id_t parentI
GPSHyperionHandler::~GPSHyperionHandler() {}
void GPSHyperionHandler::performControlOperation() {
// The data from the device will generally be read all at once. Therefore, we
// can set all field here
// PoolReadGuard pg(&gpsSet);
// if(pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
//#if FSFW_VERBOSE_LEVEL >= 1
// sif::warning << "GPSHyperionHandler::scanForReply: Reading dataset failed"
// << std::endl;
//#endif
// }
// // Print messages
// if(gpsData.is_valid) {
// // Set all entries valid now, set invalid on case basis if values are sanitized
// gpsSet.setValidity(true, true);
// }
// // Negative latitude -> South direction
// gpsSet.latitude.value = gpsData.latitude;
// // Negative longitude -> West direction
// gpsSet.longitude.value = gpsData.longitude;
// if(gpsData.altitude > 600000.0 or gpsData.altitude < 400000.0) {
// gpsSet.altitude.setValid(false);
// }
// else {
// gpsSet.altitude.setValid(true);
// gpsSet.altitude.value = gpsData.altitude;
// }
// gpsSet.fixMode.value = gpsData.fix_mode;
// gpsSet.satInUse.value = gpsData.sats_in_use;
// Clock::TimeOfDay_t timeStruct = {};
// timeStruct.day = gpsData.date;
// timeStruct.hour = gpsData.hours;
// timeStruct.minute = gpsData.minutes;
// timeStruct.month = gpsData.month;
// timeStruct.second = gpsData.seconds;
// // Convert two-digit year to full year (AD)
// timeStruct.year = gpsData.year + 2000;
// timeval timeval = {};
// Clock::convertTimeOfDayToTimeval(&timeStruct, &timeval);
// gpsSet.year = timeStruct.year;
// gpsSet.month = gpsData.month;
// gpsSet.day = gpsData.date;
// gpsSet.hours = gpsData.hours;
// gpsSet.minutes = gpsData.minutes;
// gpsSet.seconds = gpsData.seconds;
// gpsSet.unixSeconds = timeval.tv_sec;
// if(debugHyperionGps) {
// sif::info << "GPS Data" << std::endl;
// printf("Valid status: %d\n", gpsData.is_valid);
// printf("Latitude: %f degrees\n", gpsData.latitude);
// printf("Longitude: %f degrees\n", gpsData.longitude);
// printf("Altitude: %f meters\n", gpsData.altitude);
// }
//#if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1
// std::string filename = "/mnt/sd0/gps_log.txt";
// std::ofstream gpsFile;
// if(not std::filesystem::exists(filename)) {
// gpsFile.open(filename, std::ofstream::out);
// }
// gpsFile.open(filename, std::ofstream::out | std::ofstream::app);
// gpsFile.write("\n", 1);
// gpsFile.write(reinterpret_cast<const char*>(start), len);
//#endif
readGpsDataFromGpsd();
}
LocalPoolDataSetBase* GPSHyperionHandler::getDataSetHandle(sid_t sid) {
@ -143,3 +87,88 @@ ReturnValue_t GPSHyperionHandler::initialize() {
ReturnValue_t GPSHyperionHandler::handleCommandMessage(CommandMessage *message) {
return ExtendedControllerBase::handleCommandMessage(message);
}
void GPSHyperionHandler::readGpsDataFromGpsd() {
// The data from the device will generally be read all at once. Therefore, we
// can set all field here
gpsmm gpsmm(GPSD_SHARED_MEMORY, 0);
gps_data_t* gps;
gps = gpsmm.read();
if(gps == nullptr) {
sif::warning << "Q7STestTask: Reading GPS data failed" << std::endl;
}
PoolReadGuard pg(&gpsSet);
if(pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_VERBOSE_LEVEL >= 1
sif::warning << "GPSHyperionHandler::scanForReply: Reading dataset failed"
<< std::endl;
#endif
}
// Print messages
if((gps->set & MODE_SET) != MODE_SET) {
// Could not even set mode
gpsSet.setValidity(false, true);
return;
}
if(gps->satellites_used > 0) {
gpsSet.setValidity(true, true);
}
gpsSet.satInUse.value = gps->satellites_used;
gpsSet.satInView.value = gps->satellites_visible;
// 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
gpsSet.fixMode = gps->fix.mode;
if(std::isfinite(gps->fix.latitude)) {
// Negative latitude -> South direction
gpsSet.latitude.value = gps->fix.latitude;
} else {
gpsSet.latitude.setValid(false);
}
if(std::isfinite(gps->fix.longitude)) {
// Negative longitude -> West direction
gpsSet.longitude.value = gps->fix.longitude;
} else {
gpsSet.longitude.setValid(false);
}
if(std::isfinite(gps->fix.altitude)) {
gpsSet.altitude.value = gps->fix.altitude;
} else {
gpsSet.altitude.setValid(false);
}
if(std::isfinite(gps->fix.speed)) {
gpsSet.speed.value = gps->fix.speed;
} else {
gpsSet.speed.setValid(false);
}
gpsSet.unixSeconds.value = gps->fix.time.tv_sec;
timeval time = {};
time.tv_sec = gpsSet.unixSeconds.value;
time.tv_usec = gps->fix.time.tv_nsec / 1000;
Clock::TimeOfDay_t timeOfDay = {};
Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
gpsSet.year = timeOfDay.year;
gpsSet.month = timeOfDay.month;
gpsSet.day = timeOfDay.day;
gpsSet.hours = timeOfDay.hour;
gpsSet.minutes = timeOfDay.minute;
gpsSet.seconds = timeOfDay.second;
if(debugHyperionGps) {
sif::info << "-- Hyperion GPS Data --" << std::endl;
time_t timeRaw = gps->fix.time.tv_sec;
std::tm* time = gmtime(&timeRaw);
std::cout << "Time: " << std::put_time(time, "%c %Z") << std::endl;
std::cout << "Visible satellites: " << gps->satellites_visible << std::endl;
std::cout << "Satellites used: " << gps->satellites_used << std::endl;
std::cout << "Fix (0:Not Seen|1:No Fix|2:2D|3:3D): " << gps->fix.mode << std::endl;
std::cout << "Latitude: " << gps->fix.latitude << std::endl;
std::cout << "Longitude: " << gps->fix.longitude << std::endl;
std::cout << "Altitude(MSL): " << gps->fix.altMSL << std::endl;
std::cout << "Speed(m/s): " << gps->fix.speed << std::endl;
}
}