From 456dac6afdaca057935ff986a2dae65c4d0bafb5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 3 May 2022 11:33:07 +0200 Subject: [PATCH] SHM and socket readout working now --- CHANGELOG.md | 5 +- linux/devices/GPSHyperionLinuxController.cpp | 141 ++++++++++--------- linux/devices/GPSHyperionLinuxController.h | 6 +- 3 files changed, 78 insertions(+), 74 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index cde62562..78f6f248 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,10 +15,9 @@ list yields a list of all related PRs for each release. ## Added - +- Custom Gomspace FDIR which disabled most of the default FDIR functionality - Custom Syrlinks FDIR which disabled most of the default FDIR functionality - ## Changed - PCDU handler only called once in PST, but can handle multiple messages now @@ -27,6 +26,8 @@ list yields a list of all related PRs for each release. - Add `/usr/local/bin` to PATH. All shell scripts are there now - Rename GPS device to `/dev/gps0` - Add Syrlinks and TMP devices to Software by default +- Update GPS Linux Hyperion Handler to use socket interface. Still allows switching + back to SHM interface, but the SHM interface is a possible cause of SW crashes # [v1.10.1] diff --git a/linux/devices/GPSHyperionLinuxController.cpp b/linux/devices/GPSHyperionLinuxController.cpp index 9290ae18..dee3c9af 100644 --- a/linux/devices/GPSHyperionLinuxController.cpp +++ b/linux/devices/GPSHyperionLinuxController.cpp @@ -24,7 +24,10 @@ GPSHyperionLinuxController::GPSHyperionLinuxController(object_id_t objectId, obj timeUpdateCd.resetTimer(); } -GPSHyperionLinuxController::~GPSHyperionLinuxController() {} +GPSHyperionLinuxController::~GPSHyperionLinuxController() { + gps_stream(&gps, WATCH_DISABLE, nullptr); + gps_close(&gps); +} void GPSHyperionLinuxController::performControlOperation() { #ifdef FSFW_OSAL_LINUX @@ -99,6 +102,27 @@ ReturnValue_t GPSHyperionLinuxController::initialize() { if (result != HasReturnvaluesIF::RETURN_OK) { return result; } + auto openError = [&](const char *type, int error) { + if (gpsNotOpenSwitch) { + // Opening failed +#if FSFW_VERBOSE_LEVEL >= 1 + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type + << " failed | Error " << error << " | " << gps_errstr(error) << std::endl; +#endif + gpsNotOpenSwitch = false; + } + }; + if (readMode == ReadModes::SOCKET) { + int retval = gps_open("localhost", DEFAULT_GPSD_PORT, &gps); + if (retval != 0) { + openError("Socket", retval); + } + } else if (readMode == ReadModes::SHM) { + int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps); + if (retval != 0) { + openError("SHM", retval); + } + } return result; } @@ -109,42 +133,27 @@ ReturnValue_t GPSHyperionLinuxController::handleCommandMessage(CommandMessage *m #ifdef FSFW_OSAL_LINUX void GPSHyperionLinuxController::readGpsDataFromGpsd() { - gps_data_t *gps = nullptr; - auto openError = [&](const char *type) { - if (gpsNotOpenSwitch) { - // Opening failed -#if FSFW_VERBOSE_LEVEL >= 1 - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type - << " failed | Error " << errno << " | " << gps_errstr(errno) << std::endl; -#endif - gpsNotOpenSwitch = false; - } - }; - auto readError = [&]() { + auto readError = [&](int error) { if (gpsReadFailedSwitch) { gpsReadFailedSwitch = false; - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed" - << std::endl; + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | " + "Error " + << error << " | " << gps_errstr(error) << std::endl; } }; + currentClientBuf = gps_data(&gps); if (readMode == ReadModes::SOCKET) { - gpsmm gpsmm("localhost", DEFAULT_GPSD_PORT); - // The data from the device will generally be read all at once. Therefore, we - // can set all field here - if (not gpsmm.is_open()) { - return openError("Socket"); - } - // Stopwatch watch; - gpsmm.stream(WATCH_ENABLE | WATCH_JSON); - if (not gpsmm.waiting(50000000)) { + gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr); + // Exit if no data is seen in 2 seconds (should not happen) + if (not gps_waiting(&gps, 2000000)) { return; } - gps = gpsmm.read(); - if (gps == nullptr) { - readError(); + int result = gps_read(&gps); + if (result == -1) { + readError(result); return; } - if (MODE_SET != (MODE_SET & gps->set)) { + if (MODE_SET != (MODE_SET & gps.set)) { if (noModeSetCntr >= 0) { noModeSetCntr++; } @@ -155,27 +164,19 @@ void GPSHyperionLinuxController::readGpsDataFromGpsd() { << std::endl; noModeSetCntr = -1; } - return; - } else { - noModeSetCntr = 0; } - } else { - gpsmm gpsmm(GPSD_SHARED_MEMORY, ""); - if (not gpsmm.is_open()) { - return openError("SHM"); - } - gps = gpsmm.read(); - if (gps == nullptr) { - readError(); + noModeSetCntr = 0; + } else if (readMode == ReadModes::SHM) { + int result = gps_read(&gps); + if (result == -1) { + readError(result); return; } } - if (gps != nullptr) { - handleGpsRead(gps); - } + handleGpsRead(); } -ReturnValue_t GPSHyperionLinuxController::handleGpsRead(gps_data_t *gps) { +ReturnValue_t GPSHyperionLinuxController::handleGpsRead() { PoolReadGuard pg(&gpsSet); if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { #if FSFW_VERBOSE_LEVEL >= 1 @@ -187,7 +188,7 @@ ReturnValue_t GPSHyperionLinuxController::handleGpsRead(gps_data_t *gps) { bool validFix = false; static_cast(validFix); // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix - int newFixMode = gps->fix.mode; + int newFixMode = gps.fix.mode; if (newFixMode == 2 or newFixMode == 3) { validFix = true; } @@ -195,7 +196,7 @@ ReturnValue_t GPSHyperionLinuxController::handleGpsRead(gps_data_t *gps) { triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFixMode); } gpsSet.fixMode.value = newFixMode; - if (gps->fix.mode == 0 or gps->fix.mode == 1) { + if (gps.fix.mode == 0 or gps.fix.mode == 1) { if (modeCommanded and maxTimeToReachFix.hasTimedOut()) { // We are supposed to be on and functioning, but not fix was found if (mode == MODE_ON or mode == MODE_NORMAL) { @@ -204,51 +205,51 @@ ReturnValue_t GPSHyperionLinuxController::handleGpsRead(gps_data_t *gps) { modeCommanded = false; } gpsSet.setValidity(false, true); - } else if (gps->satellites_used > 0) { + } else if (gps.satellites_used > 0) { gpsSet.setValidity(true, true); } - gpsSet.satInUse.value = gps->satellites_used; - gpsSet.satInView.value = gps->satellites_visible; + gpsSet.satInUse.value = gps.satellites_used; + gpsSet.satInView.value = gps.satellites_visible; - if (std::isfinite(gps->fix.latitude)) { + if (std::isfinite(gps.fix.latitude)) { // Negative latitude -> South direction - gpsSet.latitude.value = gps->fix.latitude; + gpsSet.latitude.value = gps.fix.latitude; } else { gpsSet.latitude.setValid(false); } - if (std::isfinite(gps->fix.longitude)) { + if (std::isfinite(gps.fix.longitude)) { // Negative longitude -> West direction - gpsSet.longitude.value = gps->fix.longitude; + gpsSet.longitude.value = gps.fix.longitude; } else { gpsSet.longitude.setValid(false); } - if (std::isfinite(gps->fix.altitude)) { - gpsSet.altitude.value = gps->fix.altitude; + 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; + if (std::isfinite(gps.fix.speed)) { + gpsSet.speed.value = gps.fix.speed; } else { gpsSet.speed.setValid(false); } #if LIBGPS_VERSION_MINOR <= 17 - gpsSet.unixSeconds.value = gps->fix.time; + gpsSet.unixSeconds.value = gps.fix.time; #else - gpsSet.unixSeconds.value = gps->fix.time.tv_sec; + gpsSet.unixSeconds.value = gps.fix.time.tv_sec; #endif timeval time = {}; time.tv_sec = gpsSet.unixSeconds.value; #if LIBGPS_VERSION_MINOR <= 17 - double fractionalPart = gps->fix.time - std::floor(gps->fix.time); + double fractionalPart = gps.fix.time - std::floor(gps.fix.time); time.tv_usec = fractionalPart * 1000.0 * 1000.0; #else - time.tv_usec = gps->fix.time.tv_nsec / 1000; + time.tv_usec = gps.fix.time.tv_nsec / 1000; #endif std::time_t t = std::time(nullptr); if (time.tv_sec == t) { @@ -289,23 +290,23 @@ ReturnValue_t GPSHyperionLinuxController::handleGpsRead(gps_data_t *gps) { if (debugHyperionGps) { sif::info << "-- Hyperion GPS Data --" << std::endl; #if LIBGPS_VERSION_MINOR <= 17 - time_t timeRaw = gps->fix.time; + time_t timeRaw = gps.fix.time; #else - time_t timeRaw = gps->fix.time.tv_sec; + time_t timeRaw = gps.fix.time.tv_sec; #endif 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 << "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; #if LIBGPS_VERSION_MINOR <= 17 - std::cout << "Altitude(MSL): " << gps->fix.altitude << std::endl; + std::cout << "Altitude(MSL): " << gps.fix.altitude << std::endl; #else - std::cout << "Altitude(MSL): " << gps->fix.altMSL << std::endl; + std::cout << "Altitude(MSL): " << gps.fix.altMSL << std::endl; #endif - std::cout << "Speed(m/s): " << gps->fix.speed << std::endl; + std::cout << "Speed(m/s): " << gps.fix.speed << std::endl; std::time_t t = std::time(nullptr); std::tm tm = *std::gmtime(&t); std::cout << "C Time: " << std::put_time(&tm, "%c") << std::endl; diff --git a/linux/devices/GPSHyperionLinuxController.h b/linux/devices/GPSHyperionLinuxController.h index e7340c7f..94e82023 100644 --- a/linux/devices/GPSHyperionLinuxController.h +++ b/linux/devices/GPSHyperionLinuxController.h @@ -49,11 +49,13 @@ class GPSHyperionLinuxController : public ExtendedControllerBase { ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) override; - ReturnValue_t handleGpsRead(gps_data_t* gps); + ReturnValue_t handleGpsRead(); private: GpsPrimaryDataset gpsSet; - ReadModes readMode = ReadModes::SHM; + gps_data_t gps = {}; + const char* currentClientBuf = nullptr; + ReadModes readMode = ReadModes::SOCKET; Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000); bool modeCommanded = true; bool timeInit = true;