From cce0d5448dc14f7873d2ea97413fa6fd39377c53 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 3 May 2022 00:55:01 +0200 Subject: [PATCH] support both SHM and socket read --- bsp_q7s/boardtest/Q7STestTask.cpp | 104 +++++++++++++++++-- bsp_q7s/boardtest/Q7STestTask.h | 11 +- linux/devices/GPSHyperionLinuxController.cpp | 85 +++++++++------ linux/devices/GPSHyperionLinuxController.h | 4 + 4 files changed, 161 insertions(+), 43 deletions(-) diff --git a/bsp_q7s/boardtest/Q7STestTask.cpp b/bsp_q7s/boardtest/Q7STestTask.cpp index c3eb37ca..847c3fa5 100644 --- a/bsp_q7s/boardtest/Q7STestTask.cpp +++ b/bsp_q7s/boardtest/Q7STestTask.cpp @@ -23,8 +23,9 @@ Q7STestTask::Q7STestTask(object_id_t objectId) : TestTask(objectId) { doTestSdCard = false; doTestScratchApi = false; - doTestGps = false; - doTestXadc = true; + doTestGpsShm = true; + doTestGpsSocket = false; + doTestXadc = false; } ReturnValue_t Q7STestTask::performOneShotAction() { @@ -36,15 +37,20 @@ ReturnValue_t Q7STestTask::performOneShotAction() { } // testJsonLibDirect(); // testDummyParams(); - // testProtHandler(); + if (doTestProtHandler) { + testProtHandler(); + } FsOpCodes opCode = FsOpCodes::APPEND_TO_FILE; testFileSystemHandlerDirect(opCode); return TestTask::performOneShotAction(); } ReturnValue_t Q7STestTask::performPeriodicAction() { - if (doTestGps) { - testGpsDaemon(); + if (doTestGpsShm) { + testGpsDaemonShm(); + } + if (doTestGpsSocket) { + testGpsDaemonSocket(); } if (doTestXadc) { xadcTest(); @@ -238,8 +244,8 @@ void Q7STestTask::testProtHandler() { } } -void Q7STestTask::testGpsDaemon() { - gpsmm gpsmm(GPSD_SHARED_MEMORY, 0); +void Q7STestTask::testGpsDaemonShm() { + gpsmm gpsmm(GPSD_SHARED_MEMORY, ""); gps_data_t* gps; gps = gpsmm.read(); if (gps == nullptr) { @@ -266,6 +272,90 @@ void Q7STestTask::testGpsDaemon() { sif::info << "Speed(m/s): " << gps->fix.speed << std::endl; } +void Q7STestTask::testGpsDaemonSocket() { + 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()) { + if (gpsNotOpenSwitch) { + // Opening failed +#if FSFW_VERBOSE_LEVEL >= 1 + sif::warning << "Q7STestTask::testGpsDaemonSocket: Opening GPSMM failed | " + << "Error " << errno << " | " << gps_errstr(errno) << std::endl; +#endif + + gpsNotOpenSwitch = false; + } + return; + } + for (;;) { + struct gps_data_t* gps; + + if (!gpsmm.waiting(50000000)) continue; + + if ((gps = gpsmm.read()) == NULL) { + std::cerr << "Read error.\n"; + } else { + sif::info << "-- Q7STestTask: GPS socket read test --" << std::endl; +#if LIBGPS_VERSION_MINOR <= 17 + time_t timeRaw = gps->fix.time; +#else + time_t timeRaw = gps->fix.time.tv_sec; +#endif + std::tm* time = gmtime(&timeRaw); + sif::info << "Time: " << std::put_time(time, "%c %Z") << std::endl; + sif::info << "Visible satellites: " << gps->satellites_visible << std::endl; + sif::info << "Satellites used: " << gps->satellites_used << std::endl; + sif::info << "Fix (0:Not Seen|1:No Fix|2:2D|3:3D): " << gps->fix.mode << std::endl; + sif::info << "Latitude: " << gps->fix.latitude << std::endl; + sif::info << "Longitude: " << gps->fix.longitude << std::endl; + } + } + // // Stopwatch watch; + // gps_data_t *gps = nullptr; + // gpsmm.stream(WATCH_ENABLE | WATCH_JSON); + // if(not gpsmm.waiting(50000000)) { + // return; + // } + // gps = gpsmm.read(); + // if (gps == nullptr) { + // if (gpsReadFailedSwitch) { + // gpsReadFailedSwitch = false; + // sif::warning << "Q7STestTask::testGpsDaemonSocket: Reading GPS data failed" + // << std::endl; + // } + // return; + // } + // if (MODE_SET != (MODE_SET & gps->set)) { + // if (noModeSetCntr >= 0) { + // noModeSetCntr++; + // } + // if (noModeSetCntr == 10) { + // // TODO: Trigger event here + // sif::warning << "Q7STestTask::testGpsDaemonSocket: No mode could be " + // "read for 10 consecutive reads" + // << std::endl; + // noModeSetCntr = -1; + // } + // return; + // } else { + // noModeSetCntr = 0; + // } + // sif::info << "-- Q7STestTask: GPS socket read test --" << std::endl; + //#if LIBGPS_VERSION_MINOR <= 17 + // time_t timeRaw = gps->fix.time; + //#else + // time_t timeRaw = gps->fix.time.tv_sec; + //#endif + // std::tm* time = gmtime(&timeRaw); + // sif::info << "Time: " << std::put_time(time, "%c %Z") << std::endl; + // sif::info << "Visible satellites: " << gps->satellites_visible << std::endl; + // sif::info << "Satellites used: " << gps->satellites_used << std::endl; + // sif::info << "Fix (0:Not Seen|1:No Fix|2:2D|3:3D): " << gps->fix.mode << std::endl; + // sif::info << "Latitude: " << gps->fix.latitude << std::endl; + // sif::info << "Longitude: " << gps->fix.longitude << std::endl; +} + void Q7STestTask::testFileSystemHandlerDirect(FsOpCodes opCode) { auto fsHandler = ObjectManager::instance()->get(objects::FILE_SYSTEM_HANDLER); if (fsHandler == nullptr) { diff --git a/bsp_q7s/boardtest/Q7STestTask.h b/bsp_q7s/boardtest/Q7STestTask.h index ebef1fad..95ee3997 100644 --- a/bsp_q7s/boardtest/Q7STestTask.h +++ b/bsp_q7s/boardtest/Q7STestTask.h @@ -14,14 +14,21 @@ class Q7STestTask : public TestTask { private: bool doTestSdCard = false; bool doTestScratchApi = false; - bool doTestGps = false; + bool doTestGpsShm = false; + bool doTestGpsSocket = false; + bool doTestProtHandler = false; bool doTestXadc = false; + bool gpsNotOpenSwitch = false; + bool gpsReadFailedSwitch = false; + int32_t noModeSetCntr = 0; + CoreController* coreController = nullptr; ReturnValue_t performOneShotAction() override; ReturnValue_t performPeriodicAction() override; - void testGpsDaemon(); + void testGpsDaemonShm(); + void testGpsDaemonSocket(); void testSdCard(); void fileTests(); diff --git a/linux/devices/GPSHyperionLinuxController.cpp b/linux/devices/GPSHyperionLinuxController.cpp index 1c740f58..9290ae18 100644 --- a/linux/devices/GPSHyperionLinuxController.cpp +++ b/linux/devices/GPSHyperionLinuxController.cpp @@ -109,56 +109,73 @@ ReturnValue_t GPSHyperionLinuxController::handleCommandMessage(CommandMessage *m #ifdef FSFW_OSAL_LINUX void GPSHyperionLinuxController::readGpsDataFromGpsd() { - 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()) { + 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 failed | " - << "Error " << errno << " | " << gps_errstr(errno) << std::endl; + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type + << " failed | Error " << errno << " | " << gps_errstr(errno) << std::endl; #endif - gpsNotOpenSwitch = false; } - return; - } - // Stopwatch watch; - gps_data_t *gps = nullptr; - gpsmm.stream(WATCH_ENABLE | WATCH_JSON); - if(not gpsmm.waiting(50000000)) { - return; - } - gps = gpsmm.read(); - if (gps == nullptr) { + }; + auto readError = [&]() { if (gpsReadFailedSwitch) { gpsReadFailedSwitch = false; sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed" - << std::endl; + << std::endl; } - return; - } - if (MODE_SET != (MODE_SET & gps->set)) { - if (noModeSetCntr >= 0) { - noModeSetCntr++; + }; + 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"); } - if (noModeSetCntr == 10) { - // TODO: Trigger event here - sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be " - "read for 10 consecutive reads" - << std::endl; - noModeSetCntr = -1; + // Stopwatch watch; + gpsmm.stream(WATCH_ENABLE | WATCH_JSON); + if (not gpsmm.waiting(50000000)) { + return; + } + gps = gpsmm.read(); + if (gps == nullptr) { + readError(); + return; + } + if (MODE_SET != (MODE_SET & gps->set)) { + if (noModeSetCntr >= 0) { + noModeSetCntr++; + } + if (noModeSetCntr == 10) { + // TODO: Trigger event here + sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be " + "read for 10 consecutive reads" + << std::endl; + noModeSetCntr = -1; + } + return; + } else { + noModeSetCntr = 0; } - return; } else { - noModeSetCntr = 0; + gpsmm gpsmm(GPSD_SHARED_MEMORY, ""); + if (not gpsmm.is_open()) { + return openError("SHM"); + } + gps = gpsmm.read(); + if (gps == nullptr) { + readError(); + return; + } + } + if (gps != nullptr) { + handleGpsRead(gps); } - handleGpsRead(gps); } - -ReturnValue_t GPSHyperionLinuxController::handleGpsRead(gps_data_t* gps) { +ReturnValue_t GPSHyperionLinuxController::handleGpsRead(gps_data_t *gps) { PoolReadGuard pg(&gpsSet); if (pg.getReadResult() != HasReturnvaluesIF::RETURN_OK) { #if FSFW_VERBOSE_LEVEL >= 1 diff --git a/linux/devices/GPSHyperionLinuxController.h b/linux/devices/GPSHyperionLinuxController.h index bb9eef09..e7340c7f 100644 --- a/linux/devices/GPSHyperionLinuxController.h +++ b/linux/devices/GPSHyperionLinuxController.h @@ -24,6 +24,8 @@ class GPSHyperionLinuxController : public ExtendedControllerBase { public: static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 60 * 5; + enum ReadModes { SHM = 0, SOCKET = 1 }; + GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId, bool debugHyperionGps = false); virtual ~GPSHyperionLinuxController(); @@ -48,8 +50,10 @@ class GPSHyperionLinuxController : public ExtendedControllerBase { LocalDataPoolManager& poolManager) override; ReturnValue_t handleGpsRead(gps_data_t* gps); + private: GpsPrimaryDataset gpsSet; + ReadModes readMode = ReadModes::SHM; Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000); bool modeCommanded = true; bool timeInit = true;