mission/devices/GPSHyperionHandler.cpp

@@ -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
+    readGpsDataFromGpsd();
-    // 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
 }
 
 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;
+    }
+}

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_ */

mission/devices/devicedefinitions/GPSDefinitions.h

@@ -15,15 +15,17 @@ enum GpsPoolIds: lp_id_t {
     LATITUDE = 0,
     LONGITUDE = 1,
     ALTITUDE = 2,
-    FIX_MODE = 3,
+    SPEED = 3,
-    SATS_IN_USE = 4,
+    FIX_MODE = 4,
-    UNIX_SECONDS = 5,
+    SATS_IN_USE = 5,
-    YEAR = 6,
+    SATS_IN_VIEW = 6,
-    MONTH = 7,
+    UNIX_SECONDS = 7,
-    DAY = 8,
+    YEAR = 8,
-    HOURS = 9,
+    MONTH = 9,
-    MINUTES = 10,
+    DAY = 10,
-    SECONDS = 11
+    HOURS = 11,
+    MINUTES = 12,
+    SECONDS = 13
 };
 
 enum GpsFixModes: uint8_t {
@@ -47,8 +49,10 @@ public:
     lp_var_t<double> longitude = lp_var_t<double>(sid.objectId,
             GpsHyperion::LONGITUDE, this);
     lp_var_t<double> altitude = lp_var_t<double>(sid.objectId, GpsHyperion::ALTITUDE, this);
+    lp_var_t<double> speed = lp_var_t<double>(sid.objectId, GpsHyperion::SPEED, this);
     lp_var_t<uint8_t> fixMode = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::FIX_MODE, this);
     lp_var_t<uint8_t> satInUse = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::SATS_IN_USE, this);
+    lp_var_t<uint8_t> satInView = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::SATS_IN_VIEW, this);
     lp_var_t<uint16_t> year = lp_var_t<uint16_t>(sid.objectId, GpsHyperion::YEAR, this);
     lp_var_t<uint8_t> month = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::MONTH, this);
     lp_var_t<uint8_t> day = lp_var_t<uint8_t>(sid.objectId, GpsHyperion::DAY, this);

tmtc

@@ -1 +1 @@
-Subproject commit 8ed7a4e90f392f4f60e63659fa0115a2fe6765e1
+Subproject commit c86cd1874f605a89277e1b8fcf4496f9302c941e