diff --git a/mission/devices/GPSHyperionHandler.cpp b/mission/devices/GPSHyperionHandler.cpp index 3a2253e6..d42421a0 100644 --- a/mission/devices/GPSHyperionHandler.cpp +++ b/mission/devices/GPSHyperionHandler.cpp @@ -4,6 +4,10 @@ #include "fsfw/datapool/PoolReadGuard.h" #include "fsfw/timemanager/Clock.h" +#include +#include +#include + #if FSFW_DEV_HYPERION_GPS_CREATE_NMEA_CSV == 1 #include #include @@ -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(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; + } +} diff --git a/mission/devices/GPSHyperionHandler.h b/mission/devices/GPSHyperionHandler.h index 240fe480..96d6089e 100644 --- a/mission/devices/GPSHyperionHandler.h +++ b/mission/devices/GPSHyperionHandler.h @@ -5,7 +5,6 @@ #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "fsfw/controller/ExtendedControllerBase.h" #include "devicedefinitions/GPSDefinitions.h" -#include "lwgps/lwgps.h" /** * @brief Device handler for the Hyperion HT-GPS200 device @@ -43,6 +42,8 @@ protected: private: GpsPrimaryDataset gpsSet; bool debugHyperionGps = false; + + void readGpsDataFromGpsd(); }; #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/GPSDefinitions.h b/mission/devices/devicedefinitions/GPSDefinitions.h index 90fdb123..aea442a2 100644 --- a/mission/devices/devicedefinitions/GPSDefinitions.h +++ b/mission/devices/devicedefinitions/GPSDefinitions.h @@ -15,15 +15,17 @@ enum GpsPoolIds: lp_id_t { LATITUDE = 0, LONGITUDE = 1, ALTITUDE = 2, - FIX_MODE = 3, - SATS_IN_USE = 4, - UNIX_SECONDS = 5, - YEAR = 6, - MONTH = 7, - DAY = 8, - HOURS = 9, - MINUTES = 10, - SECONDS = 11 + SPEED = 3, + FIX_MODE = 4, + SATS_IN_USE = 5, + SATS_IN_VIEW = 6, + UNIX_SECONDS = 7, + YEAR = 8, + MONTH = 9, + DAY = 10, + HOURS = 11, + MINUTES = 12, + SECONDS = 13 }; enum GpsFixModes: uint8_t { @@ -47,8 +49,10 @@ public: lp_var_t longitude = lp_var_t(sid.objectId, GpsHyperion::LONGITUDE, this); lp_var_t altitude = lp_var_t(sid.objectId, GpsHyperion::ALTITUDE, this); + lp_var_t speed = lp_var_t(sid.objectId, GpsHyperion::SPEED, this); lp_var_t fixMode = lp_var_t(sid.objectId, GpsHyperion::FIX_MODE, this); lp_var_t satInUse = lp_var_t(sid.objectId, GpsHyperion::SATS_IN_USE, this); + lp_var_t satInView = lp_var_t(sid.objectId, GpsHyperion::SATS_IN_VIEW, this); lp_var_t year = lp_var_t(sid.objectId, GpsHyperion::YEAR, this); lp_var_t month = lp_var_t(sid.objectId, GpsHyperion::MONTH, this); lp_var_t day = lp_var_t(sid.objectId, GpsHyperion::DAY, this); diff --git a/tmtc b/tmtc index 8ed7a4e9..c86cd187 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 8ed7a4e90f392f4f60e63659fa0115a2fe6765e1 +Subproject commit c86cd1874f605a89277e1b8fcf4496f9302c941e