Merge pull request 'bugfix for GPS handler' (#368) from gps_bugfix into develop

Reviewed-on: #368
2023-02-06 18:25:52 +01:00 · 2023-02-06 18:25:52 +01:00 · d4e8db49d2
commit d4e8db49d2
parent 970e018c27 86babcf07b
8 changed files with 120 additions and 99 deletions
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@ -17,6 +17,13 @@ change warranting a new major release:

 # [unreleased]

+## Fixed
+
+- `GpsHyperionLinuxController`: Fix `gpsd` polling by continuously calling `gps_read` in one cycle
+  until it does not have any data left anymore. Also, the data is now polled in a permanent loop,
+  where controller handling is done on 0.2 second timeouts.
+  PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/368
+
 # [v1.24.0] 2023-02-03

 - eive-tmtc v2.10.0
--- a/bsp_q7s/core/ObjectFactory.cpp
+++ b/bsp_q7s/core/ObjectFactory.cpp
@ -23,7 +23,7 @@
 #include "linux/boardtest/UartTestClass.h"
 #include "linux/callbacks/gpioCallbacks.h"
 #include "linux/csp/CspComIF.h"
-#include "linux/devices/GPSHyperionLinuxController.h"
+#include "linux/devices/GpsHyperionLinuxController.h"
 #include "linux/devices/ScexUartReader.h"
 #include "linux/devices/devicedefinitions/PlocMPSoCDefinitions.h"
 #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
@ -474,7 +474,7 @@ void ObjectFactory::createAcsBoardComponents(LinuxLibgpioIF* gpioComIF, SerialCo
  RESET_ARGS_GNSS.gpioComIF = gpioComIF;
  RESET_ARGS_GNSS.waitPeriodMs = 100;
  auto gpsCtrl =
-      new GPSHyperionLinuxController(objects::GPS_CONTROLLER, objects::NO_OBJECT, debugGps);
+      new GpsHyperionLinuxController(objects::GPS_CONTROLLER, objects::NO_OBJECT, debugGps);
  gpsCtrl->setResetPinTriggerFunction(gps::triggerGpioResetPin, &RESET_ARGS_GNSS);

  AcsBoardHelper acsBoardHelper = AcsBoardHelper(
--- a/bsp_q7s/core/scheduling.cpp
+++ b/bsp_q7s/core/scheduling.cpp
@ -173,54 +173,25 @@ void scheduling::initTasks() {
  }
 #endif

-  PeriodicTaskIF* acsCtrlTask = factory->createPeriodicTask(
-      "ACS_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
 #if OBSW_ADD_GPS_CTRL == 1
-  result = acsCtrlTask->addComponent(objects::GPS_CONTROLLER);
+  PeriodicTaskIF* gpsTask = factory->createPeriodicTask(
+      "GPS_TASK", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
+  result = gpsTask->addComponent(objects::GPS_CONTROLLER);
  if (result != returnvalue::OK) {
    scheduling::printAddObjectError("GPS_CTRL", objects::GPS_CONTROLLER);
  }
 #endif /* OBSW_ADD_GPS_CTRL */

+  PeriodicTaskIF* acsSysTask = factory->createPeriodicTask(
+      "SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
+  static_cast<void>(acsSysTask);
+  // To be removed soon because it will be part of the ACS PST.
 #if OBSW_ADD_ACS_CTRL == 1
-  acsCtrlTask->addComponent(objects::ACS_CONTROLLER);
+  acsSysTask->addComponent(objects::ACS_CONTROLLER);
  if (result != returnvalue::OK) {
    scheduling::printAddObjectError("ACS_CTRL", objects::ACS_CONTROLLER);
  }
 #endif
-#if OBSW_Q7S_EM == 1
-  acsCtrlTask->addComponent(objects::MGM_0_LIS3_HANDLER);
-  acsCtrlTask->addComponent(objects::MGM_1_RM3100_HANDLER);
-  acsCtrlTask->addComponent(objects::MGM_2_LIS3_HANDLER);
-  acsCtrlTask->addComponent(objects::MGM_3_RM3100_HANDLER);
-  acsCtrlTask->addComponent(objects::IMTQ_HANDLER);
-  acsCtrlTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF);
-  acsCtrlTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF);
-  acsCtrlTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB);
-  acsCtrlTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB);
-  acsCtrlTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB);
-  acsCtrlTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB);
-  acsCtrlTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF);
-  acsCtrlTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF);
-  acsCtrlTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF);
-  acsCtrlTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF);
-  acsCtrlTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
-  acsCtrlTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB);
-  acsCtrlTask->addComponent(objects::GYRO_0_ADIS_HANDLER);
-  acsCtrlTask->addComponent(objects::GYRO_1_L3G_HANDLER);
-  acsCtrlTask->addComponent(objects::GYRO_2_ADIS_HANDLER);
-  acsCtrlTask->addComponent(objects::GYRO_3_L3G_HANDLER);
-  acsCtrlTask->addComponent(objects::GPS_CONTROLLER);
-  acsCtrlTask->addComponent(objects::STAR_TRACKER);
-  acsCtrlTask->addComponent(objects::RW1);
-  acsCtrlTask->addComponent(objects::RW2);
-  acsCtrlTask->addComponent(objects::RW3);
-  acsCtrlTask->addComponent(objects::RW4);
-#endif
-
-  PeriodicTaskIF* acsSysTask = factory->createPeriodicTask(
-      "SYS_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc);
-  static_cast<void>(acsSysTask);
 #if OBSW_ADD_ACS_BOARD == 1
  result = acsSysTask->addComponent(objects::ACS_BOARD_ASS);
  if (result != returnvalue::OK) {
@ -244,6 +215,35 @@ void scheduling::initTasks() {
    scheduling::printAddObjectError("ACS_SUBSYSTEM", objects::ACS_SUBSYSTEM);
  }

+#if OBSW_Q7S_EM == 1
+  acsSysTask->addComponent(objects::MGM_0_LIS3_HANDLER);
+  acsSysTask->addComponent(objects::MGM_1_RM3100_HANDLER);
+  acsSysTask->addComponent(objects::MGM_2_LIS3_HANDLER);
+  acsSysTask->addComponent(objects::MGM_3_RM3100_HANDLER);
+  acsSysTask->addComponent(objects::IMTQ_HANDLER);
+  acsSysTask->addComponent(objects::SUS_0_N_LOC_XFYFZM_PT_XF);
+  acsSysTask->addComponent(objects::SUS_6_R_LOC_XFYBZM_PT_XF);
+  acsSysTask->addComponent(objects::SUS_1_N_LOC_XBYFZM_PT_XB);
+  acsSysTask->addComponent(objects::SUS_7_R_LOC_XBYBZM_PT_XB);
+  acsSysTask->addComponent(objects::SUS_2_N_LOC_XFYBZB_PT_YB);
+  acsSysTask->addComponent(objects::SUS_8_R_LOC_XBYBZB_PT_YB);
+  acsSysTask->addComponent(objects::SUS_3_N_LOC_XFYBZF_PT_YF);
+  acsSysTask->addComponent(objects::SUS_9_R_LOC_XBYBZB_PT_YF);
+  acsSysTask->addComponent(objects::SUS_4_N_LOC_XMYFZF_PT_ZF);
+  acsSysTask->addComponent(objects::SUS_10_N_LOC_XMYBZF_PT_ZF);
+  acsSysTask->addComponent(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
+  acsSysTask->addComponent(objects::SUS_11_R_LOC_XBYMZB_PT_ZB);
+  acsSysTask->addComponent(objects::GYRO_0_ADIS_HANDLER);
+  acsSysTask->addComponent(objects::GYRO_1_L3G_HANDLER);
+  acsSysTask->addComponent(objects::GYRO_2_ADIS_HANDLER);
+  acsSysTask->addComponent(objects::GYRO_3_L3G_HANDLER);
+  acsSysTask->addComponent(objects::GPS_CONTROLLER);
+  acsSysTask->addComponent(objects::STAR_TRACKER);
+  acsSysTask->addComponent(objects::RW1);
+  acsSysTask->addComponent(objects::RW2);
+  acsSysTask->addComponent(objects::RW3);
+  acsSysTask->addComponent(objects::RW4);
+#endif
 #if OBSW_ADD_RTD_DEVICES == 1
  PeriodicTaskIF* tcsPollingTask = factory->createPeriodicTask(
      "TCS_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.5, missedDeadlineFunc);
@ -414,7 +414,9 @@ void scheduling::initTasks() {
  strHelperTask->startTask();
 #endif /* OBSW_ADD_STAR_TRACKER == 1 */

-  acsCtrlTask->startTask();
+#if OBSW_ADD_GPS_CTRL == 1
+  gpsTask->startTask();
+#endif
  acsSysTask->startTask();
 #if OBSW_ADD_RTD_DEVICES == 1
  tcsPollingTask->startTask();
--- a/linux/ObjectFactory.cpp
+++ b/linux/ObjectFactory.cpp
@ -121,8 +121,8 @@ void ObjectFactory::createSunSensorComponents(GpioIF* gpioComIF, SpiComIF* spiCo
  fdir = new SusFdir(objects::SUS_5_N_LOC_XFYMZB_PT_ZB);
  susHandlers[5]->setCustomFdir(fdir);

-  spiCookie = new SpiCookie(addresses::SUS_6, gpioIds::CS_SUS_6, SUS::MAX_CMD_SIZE, spi::SUS_MAX_1227_MODE,
-                            spi::SUS_MAX1227_SPI_FREQ);
+  spiCookie = new SpiCookie(addresses::SUS_6, gpioIds::CS_SUS_6, SUS::MAX_CMD_SIZE,
+                            spi::SUS_MAX_1227_MODE, spi::SUS_MAX1227_SPI_FREQ);
  susHandlers[6] =
      new SusHandler(objects::SUS_6_R_LOC_XFYBZM_PT_XF, 6, objects::SPI_MAIN_COM_IF, spiCookie);
  fdir = new SusFdir(objects::SUS_6_R_LOC_XFYBZM_PT_XF);
--- a/linux/devices/CMakeLists.txt
+++ b/linux/devices/CMakeLists.txt
@ -1,5 +1,5 @@
 if(EIVE_BUILD_GPSD_GPS_HANDLER)
-  target_sources(${OBSW_NAME} PRIVATE GPSHyperionLinuxController.cpp)
+  target_sources(${OBSW_NAME} PRIVATE GpsHyperionLinuxController.cpp)
 endif()

 target_sources(
--- a/linux/devices/GpsHyperionLinuxController.cpp
+++ b/linux/devices/GpsHyperionLinuxController.cpp
@ -1,5 +1,6 @@
-#include "GPSHyperionLinuxController.h"
+#include "GpsHyperionLinuxController.h"

+#include <fsfw/tasks/TaskFactory.h>
 #include <fsfw/timemanager/Stopwatch.h>

 #include "OBSWConfig.h"
@ -16,26 +17,20 @@
 #include <cmath>
 #include <ctime>

-GPSHyperionLinuxController::GPSHyperionLinuxController(object_id_t objectId, object_id_t parentId,
+GpsHyperionLinuxController::GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
                                                       bool debugHyperionGps)
    : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {
  timeUpdateCd.resetTimer();
 }

-GPSHyperionLinuxController::~GPSHyperionLinuxController() {
+GpsHyperionLinuxController::~GpsHyperionLinuxController() {
  gps_stream(&gps, WATCH_DISABLE, nullptr);
  gps_close(&gps);
 }

-void GPSHyperionLinuxController::performControlOperation() {
-#ifdef FSFW_OSAL_LINUX
-  readGpsDataFromGpsd();
-#endif
-}
+LocalPoolDataSetBase *GpsHyperionLinuxController::getDataSetHandle(sid_t sid) { return &gpsSet; }

-LocalPoolDataSetBase *GPSHyperionLinuxController::getDataSetHandle(sid_t sid) { return &gpsSet; }
-
-ReturnValue_t GPSHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
+ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
                                                           uint32_t *msToReachTheMode) {
  if (not modeCommanded) {
    if (mode == MODE_ON or mode == MODE_OFF) {
@ -54,7 +49,7 @@ ReturnValue_t GPSHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
  return returnvalue::OK;
 }

-ReturnValue_t GPSHyperionLinuxController::executeAction(ActionId_t actionId,
+ReturnValue_t GpsHyperionLinuxController::executeAction(ActionId_t actionId,
                                                        MessageQueueId_t commandedBy,
                                                        const uint8_t *data, size_t size) {
  switch (actionId) {
@ -72,7 +67,7 @@ ReturnValue_t GPSHyperionLinuxController::executeAction(ActionId_t actionId,
  return returnvalue::OK;
 }

-ReturnValue_t GPSHyperionLinuxController::initializeLocalDataPool(
+ReturnValue_t GpsHyperionLinuxController::initializeLocalDataPool(
    localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(GpsHyperion::ALTITUDE, new PoolEntry<double>({0.0}));
  localDataPoolMap.emplace(GpsHyperion::LONGITUDE, new PoolEntry<double>({0.0}));
@ -92,13 +87,27 @@ ReturnValue_t GPSHyperionLinuxController::initializeLocalDataPool(
  return returnvalue::OK;
 }

-void GPSHyperionLinuxController::setResetPinTriggerFunction(gpioResetFunction_t resetCallback,
+void GpsHyperionLinuxController::setResetPinTriggerFunction(gpioResetFunction_t resetCallback,
                                                            void *args) {
  this->resetCallback = resetCallback;
  resetCallbackArgs = args;
 }

-ReturnValue_t GPSHyperionLinuxController::initialize() {
+ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) {
+  handleQueue();
+  poolManager.performHkOperation();
+  while (true) {
+    bool callAgainImmediately = readGpsDataFromGpsd();
+    if (not callAgainImmediately) {
+      handleQueue();
+      poolManager.performHkOperation();
+    }
+  }
+  // Should never be reached.
+  return returnvalue::OK;
+}
+
+ReturnValue_t GpsHyperionLinuxController::initialize() {
  ReturnValue_t result = ExtendedControllerBase::initialize();
  if (result != returnvalue::OK) {
    return result;
@ -118,6 +127,7 @@ ReturnValue_t GPSHyperionLinuxController::initialize() {
    if (retval != 0) {
      openError("Socket", retval);
    }
+    gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr);
  } else if (readMode == ReadModes::SHM) {
    int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps);
    if (retval != 0) {
@ -127,57 +137,58 @@ ReturnValue_t GPSHyperionLinuxController::initialize() {
  return result;
 }

-ReturnValue_t GPSHyperionLinuxController::handleCommandMessage(CommandMessage *message) {
+ReturnValue_t GpsHyperionLinuxController::handleCommandMessage(CommandMessage *message) {
  return ExtendedControllerBase::handleCommandMessage(message);
 }

-#ifdef FSFW_OSAL_LINUX
+void GpsHyperionLinuxController::performControlOperation() {}

-void GPSHyperionLinuxController::readGpsDataFromGpsd() {
-  auto readError = [&](int error) {
+bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
+  auto readError = [&]() {
    if (gpsReadFailedSwitch) {
      gpsReadFailedSwitch = false;
      sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | "
                      "Error "
-                   << error << " | " << gps_errstr(error) << std::endl;
+                   << errno << " | " << gps_errstr(errno) << std::endl;
    }
  };
-  currentClientBuf = gps_data(&gps);
  if (readMode == ReadModes::SOCKET) {
-    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;
-    }
-    int result = gps_read(&gps);
-    if (result == -1) {
-      readError(result);
-      return;
-    }
-    if (MODE_SET != (MODE_SET & gps.set)) {
-      if (noModeSetCntr >= 0) {
-        noModeSetCntr++;
+    // Perform other necessary handling if not data seen for 0.2 seconds.
+    if (gps_waiting(&gps, 200000)) {
+      if (-1 == gps_read(&gps)) {
+        readError();
+        return false;
      }
-      if (noModeSetCntr == 10) {
-        // TODO: Trigger event here
-        sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be "
-                        "read for 10 consecutive reads"
-                     << std::endl;
-        noModeSetCntr = -1;
+      if (MODE_SET != (MODE_SET & gps.set)) {
+        if (mode == MODE_ON) {
+          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;
+          }
+          // did not event get mode, nothing to see.
+          return false;
+        }
      }
+      noModeSetCntr = 0;
+    } else {
+      return false;
    }
-    noModeSetCntr = 0;
  } else if (readMode == ReadModes::SHM) {
-    int result = gps_read(&gps);
-    if (result == -1) {
-      readError(result);
-      return;
-    }
+    sif::error << "GpsHyperionLinuxController::readGpsDataFromGpsdPermanentLoop: "
+                  "SHM read not implemented"
+               << std::endl;
  }
-  handleGpsRead();
+  handleGpsReadData();
+  return true;
 }

-ReturnValue_t GPSHyperionLinuxController::handleGpsRead() {
+ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
  PoolReadGuard pg(&gpsSet);
  if (pg.getReadResult() != returnvalue::OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
@ -314,5 +325,3 @@ ReturnValue_t GPSHyperionLinuxController::handleGpsRead() {
  }
  return returnvalue::OK;
 }
-
-#endif
--- a/linux/devices/GpsHyperionLinuxController.h
+++ b/linux/devices/GpsHyperionLinuxController.h
@ -20,18 +20,19 @@
 * This device handler can only be used on Linux system where the gpsd daemon with shared memory
 * export is running.
 */
-class GPSHyperionLinuxController : public ExtendedControllerBase {
+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,
+  GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
                             bool debugHyperionGps = false);
-  virtual ~GPSHyperionLinuxController();
+  virtual ~GpsHyperionLinuxController();

  using gpioResetFunction_t = ReturnValue_t (*)(const uint8_t* actionData, size_t len, void* args);

+  ReturnValue_t performOperation(uint8_t opCode) override;
  void setResetPinTriggerFunction(gpioResetFunction_t resetCallback, void* args);
  ReturnValue_t handleCommandMessage(CommandMessage* message) override;
  void performControlOperation() override;
@ -49,7 +50,7 @@ class GPSHyperionLinuxController : public ExtendedControllerBase {
  ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                        LocalDataPoolManager& poolManager) override;

-  ReturnValue_t handleGpsRead();
+  ReturnValue_t handleGpsReadData();

 private:
  GpsPrimaryDataset gpsSet;
@ -66,7 +67,9 @@ class GPSHyperionLinuxController : public ExtendedControllerBase {
  uint32_t timeIsConstantCounter = 0;
  Countdown timeUpdateCd = Countdown(60);

-  void readGpsDataFromGpsd();
+  // Returns true if the function should be called again or false if other
+  // controller handling can be done.
+  bool readGpsDataFromGpsd();
 };

 #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */
--- a/mission/devices/devicedefinitions/GPSDefinitions.h
+++ b/mission/devices/devicedefinitions/GPSDefinitions.h
@ -63,7 +63,7 @@ class GpsPrimaryDataset : public StaticLocalDataSet<18> {
      lp_var_t<uint32_t>(sid.objectId, GpsHyperion::UNIX_SECONDS, this);

 private:
-  friend class GPSHyperionLinuxController;
+  friend class GpsHyperionLinuxController;
  GpsPrimaryDataset(HasLocalDataPoolIF* hkOwner)
      : StaticLocalDataSet(hkOwner, GpsHyperion::DATASET_ID) {}
 };