fixes for scheduling

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);
+  gpsTask->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();

linux/devices/GpsHyperionLinuxController.cpp

@@ -27,12 +27,6 @@ GpsHyperionLinuxController::~GpsHyperionLinuxController() {
   gps_close(&gps);
 }
 
-void GpsHyperionLinuxController::performControlOperation() {
-#ifdef FSFW_OSAL_LINUX
-  readGpsDataFromGpsd();
-#endif
-}
-
 LocalPoolDataSetBase *GpsHyperionLinuxController::getDataSetHandle(sid_t sid) { return &gpsSet; }
 
 ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
@@ -98,6 +92,18 @@ void GpsHyperionLinuxController::setResetPinTriggerFunction(gpioResetFunction_t
   resetCallbackArgs = args;
 }
 
+ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) {
+  while (true) {
+    bool noDataRead = readGpsDataFromGpsd();
+    if (noDataRead) {
+      handleQueue();
+      poolManager.performHkOperation();
+    }
+  }
+  // Should never be reached.
+  return returnvalue::OK;
+}
+
 ReturnValue_t GpsHyperionLinuxController::initialize() {
   ReturnValue_t result = ExtendedControllerBase::initialize();
   if (result != returnvalue::OK) {
@@ -133,7 +139,7 @@ ReturnValue_t GpsHyperionLinuxController::handleCommandMessage(CommandMessage *m
 
 #ifdef FSFW_OSAL_LINUX
 
-void GpsHyperionLinuxController::readGpsDataFromGpsd() {
+bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
   auto readError = [&](int error) {
     if (gpsReadFailedSwitch) {
       gpsReadFailedSwitch = false;
@@ -145,15 +151,15 @@ void GpsHyperionLinuxController::readGpsDataFromGpsd() {
   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)
+    // Perform other necessary handling if not data seen for 0.2 seconds.
-    if (gps_waiting(&gps, 2000000)) {
+    if (gps_waiting(&gps, 200000)) {
       int result = gps_read(&gps);
       while (result > 0) {
         result = gps_read(&gps);
       }
       if (result == -1) {
         readError(result);
-        return;
+        return false;
       }
       if (MODE_SET != (MODE_SET & gps.set)) {
         if (mode == MODE_ON) {
@@ -168,19 +174,20 @@ void GpsHyperionLinuxController::readGpsDataFromGpsd() {
             noModeSetCntr = -1;
           }
           // did not event get mode, nothing to see.
-          return;
+          return false;
         }
       }
       noModeSetCntr = 0;
+    } else {
+      return false;
     }
   } else if (readMode == ReadModes::SHM) {
-    int result = gps_read(&gps);
+    sif::error << "GpsHyperionLinuxController::readGpsDataFromGpsdPermanentLoop: "
-    if (result == -1) {
+                  "SHM read not implemented"
-      readError(result);
+               << std::endl;
-      return;
-    }
   }
   handleGpsReadData();
+  return true;
 }
 
 ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {

linux/devices/GpsHyperionLinuxController.h

@@ -32,6 +32,7 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
 
   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;
@@ -66,7 +67,10 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
   uint32_t timeIsConstantCounter = 0;
   Countdown timeUpdateCd = Countdown(60);
 
-  void readGpsDataFromGpsd();
+  // Returns true if data was read and function should be called again
+  // immediately, and false if the gps_waiting blocked for 0.2 seconds
+  // without new data arriving.
+  bool readGpsDataFromGpsd();
 };
 
 #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */