diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index d8136d3a..d08bb848 100644 --- 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(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(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(); diff --git a/linux/devices/GpsHyperionLinuxController.cpp b/linux/devices/GpsHyperionLinuxController.cpp index e9832854..9f3ebbbd 100644 --- a/linux/devices/GpsHyperionLinuxController.cpp +++ b/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) - if (gps_waiting(&gps, 2000000)) { + // Perform other necessary handling if not data seen for 0.2 seconds. + 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); - if (result == -1) { - readError(result); - return; - } + sif::error << "GpsHyperionLinuxController::readGpsDataFromGpsdPermanentLoop: " + "SHM read not implemented" + << std::endl; } handleGpsReadData(); + return true; } ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() { diff --git a/linux/devices/GpsHyperionLinuxController.h b/linux/devices/GpsHyperionLinuxController.h index e0c3dd36..829857b2 100644 --- a/linux/devices/GpsHyperionLinuxController.h +++ b/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_ */