diff --git a/bsp_linux/InitMission.cpp b/bsp_linux/InitMission.cpp index 04e79a12..94f58991 100644 --- a/bsp_linux/InitMission.cpp +++ b/bsp_linux/InitMission.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include @@ -129,14 +130,15 @@ void InitMission::initTasks(){ sif::error << "Object add component failed" << std::endl; } - /* Device Handler */ - PeriodicTaskIF* DeviceHandler = TaskFactory::instance()-> - createPeriodicTask("Device Handler", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, - 1, nullptr); - result = DeviceHandler->addComponent(objects::P60DOCK_HANDLER); - if(result!=HasReturnvaluesIF::RETURN_OK){ - sif::error << "Object add component failed" << std::endl; - } + FixedTimeslotTaskIF* GomSpacePstTask = TaskFactory::instance()-> + createFixedTimeslotTask("GS_PST_TASK", 10, + PeriodicTaskIF::MINIMUM_STACK_SIZE*4, 1.0, nullptr); + result = pst::gomspacePstInit(GomSpacePstTask); + if(result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "InitMission::initTasks: GomSpace PST initialization " + << "failed!" << std::endl; +} + #if ADD_TEST_CODE == 1 // FixedTimeslotTaskIF* TestTimeslotTask = TaskFactory::instance()-> @@ -163,13 +165,14 @@ void InitMission::initTasks(){ UdpBridgeTask->startTask(); UdpPollingTask->startTask(); + GomSpacePstTask->startTask(); + PusVerification->startTask(); PusEvents->startTask(); PusHighPrio->startTask(); PusMedPrio->startTask(); PusLowPrio->startTask(); - DeviceHandler->startTask(); #if ADD_TEST_CODE == 1 // TestTimeslotTask->startTask(); P60DockTestTask->startTask(); diff --git a/bsp_linux/comIF/P60DockComIF.cpp b/bsp_linux/comIF/P60DockComIF.cpp index 5bc1d4f9..d37e652b 100644 --- a/bsp_linux/comIF/P60DockComIF.cpp +++ b/bsp_linux/comIF/P60DockComIF.cpp @@ -86,7 +86,14 @@ ReturnValue_t P60DockComIF::requestReceiveMessage(CookieIF *cookie, ReturnValue_t P60DockComIF::readReceivedMessage(CookieIF *cookie, uint8_t** buffer, size_t* size) { + if(cookie == NULL){ + return HasReturnvaluesIF::RETURN_FAILED; + } P60DockCookie* p60DockCookie = dynamic_cast (cookie); + if(p60DockCookie == NULL){ + return HasReturnvaluesIF::RETURN_FAILED; + } + MessageType_t messageType = p60DockCookie->getMessageType(); switch(messageType){ @@ -95,10 +102,10 @@ ReturnValue_t P60DockComIF::readReceivedMessage(CookieIF *cookie, uint8_t p60dockAddress = p60DockCookie->getCspAddress(); gs_param_table_instance_t moduleConfig; moduleConfig.rows = (gs_param_table_row_t*)p60dock_config; - moduleConfig.id = p60dockAddress; + moduleConfig.id = moduleCfgTableNum; moduleConfig.row_count = p60dock_config_count; - moduleConfig.memory_size = moduleCfgTableSize; - moduleConfig.memory = *buffer; + moduleConfig.memory_size = p60dock_config_size; + moduleConfig.memory = replyBuffer; /* Read complete module configuration table from P60 Dock and store data * in buffer */ int result = gs_rparam_get_full_table(&moduleConfig, p60dockAddress, @@ -106,8 +113,30 @@ ReturnValue_t P60DockComIF::readReceivedMessage(CookieIF *cookie, *size = moduleCfgTableSize; if (result != GS_OK) { sif::info - << "Failed retrieving module configuration from P60 dock with error code " - << result << std::endl; + << "Failed retrieving module configuration from P60 dock " + << "with error code " << result << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + break; + } + case(P60DockCookie::READ_HK):{ + uint32_t timeout = 1000; + uint8_t p60dockAddress = p60DockCookie->getCspAddress(); + gs_param_table_instance_t tmData; + tmData.rows = (gs_param_table_row_t*)p60dock_hk; + tmData.id = tmTableNum; + tmData.row_count = p60dock_hk_count; + tmData.memory_size = tmTableSize; + tmData.memory = replyBuffer; + /* Read complete module configuration table from P60 Dock and store data + * in buffer */ + int result = gs_rparam_get_full_table(&tmData, p60dockAddress, + tmData.id, GS_RPARAM_MAGIC_CHECKSUM, timeout); + *size = tmTableSize; + if (result != GS_OK) { + sif::info + << "Failed retrieving telemetry from P60 dock with error " + << "code " << result << std::endl; return HasReturnvaluesIF::RETURN_FAILED; } break; diff --git a/bsp_linux/comIF/P60DockComIF.h b/bsp_linux/comIF/P60DockComIF.h index 4595e9dd..411b3b99 100644 --- a/bsp_linux/comIF/P60DockComIF.h +++ b/bsp_linux/comIF/P60DockComIF.h @@ -25,6 +25,8 @@ */ class P60DockComIF: public DeviceCommunicationIF, public SystemObject { public: + static const uint8_t maxReplyLength = 188; + P60DockComIF(object_id_t objectId); virtual ~P60DockComIF(); @@ -42,12 +44,14 @@ private: csp_iface_t csp_if; /* Table definitions. According to gomspace software documentation there * exist four tables each identified by a number*/ - uint8_t boardConfigTable = 0; - uint8_t moduleConfigTable = 1; - uint8_t calibrationParamTable = 2; - uint8_t tmDataTable = 4; + uint8_t boardConfigTableNum = 0; + uint8_t moduleCfgTableNum = 1; + uint8_t calibrationParamTableNum = 2; + uint8_t tmTableNum = 4; unsigned int moduleConfigTableRows = 32; - uint16_t moduleCfgTableSize = 412; + uint8_t moduleCfgTableSize = 188; + uint8_t tmTableSize = 188; + uint8_t replyBuffer[P60DockComIF::maxReplyLength]; }; diff --git a/bsp_linux/comIF/cookies/P60DockCookie.cpp b/bsp_linux/comIF/cookies/P60DockCookie.cpp index ccc6e09b..b4b83bbe 100644 --- a/bsp_linux/comIF/cookies/P60DockCookie.cpp +++ b/bsp_linux/comIF/cookies/P60DockCookie.cpp @@ -33,6 +33,10 @@ void P60DockCookie::setReadModuleCfgMessage(){ nextMessage = READ_MODULE_CONFIG; } +void P60DockCookie::setReadHkMessage(){ + nextMessage = READ_HK; +} + MessageType_t P60DockCookie::getMessageType(){ return nextMessage; } diff --git a/bsp_linux/comIF/cookies/P60DockCookie.h b/bsp_linux/comIF/cookies/P60DockCookie.h index eef7b417..e8a1c42c 100644 --- a/bsp_linux/comIF/cookies/P60DockCookie.h +++ b/bsp_linux/comIF/cookies/P60DockCookie.h @@ -27,6 +27,7 @@ public: void setPingMessage(); void setRebootMessage(); void setReadModuleCfgMessage(); + void setReadHkMessage(); MessageType_t getMessageType(); /* Message type defines the type of the next data transfer between the @@ -35,6 +36,7 @@ public: static const MessageType_t PING = 0x1; static const MessageType_t REBOOT = 0x4; static const MessageType_t READ_MODULE_CONFIG = 0x71; + static const MessageType_t READ_HK = 0x74; private: diff --git a/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp b/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp index 22c00034..6370efa5 100644 --- a/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp +++ b/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp @@ -28,3 +28,25 @@ ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence) } } +ReturnValue_t pst::gomspacePstInit(FixedTimeslotTaskIF *thisSequence){ + + uint32_t length = thisSequence->getPeriodMs(); + + thisSequence->addSlot(objects::P60DOCK_HANDLER, + length * 0, DeviceHandlerIF::SEND_WRITE); + thisSequence->addSlot(objects::P60DOCK_HANDLER, + length * 0.25, DeviceHandlerIF::GET_WRITE); + thisSequence->addSlot(objects::P60DOCK_HANDLER, + length * 0.5, DeviceHandlerIF::SEND_READ); + thisSequence->addSlot(objects::P60DOCK_HANDLER, + length * 0.75, DeviceHandlerIF::GET_READ); + + if (thisSequence->checkSequence() == HasReturnvaluesIF::RETURN_OK) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + sif::error << "Initialization of GomSpace PST failed" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } +} + diff --git a/fsfwconfig/pollingsequence/PollingSequenceFactory.h b/fsfwconfig/pollingsequence/PollingSequenceFactory.h index b6cc2f1e..0b12e6b8 100644 --- a/fsfwconfig/pollingsequence/PollingSequenceFactory.h +++ b/fsfwconfig/pollingsequence/PollingSequenceFactory.h @@ -26,6 +26,13 @@ namespace pst { /* 0.4 second period init*/ ReturnValue_t pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence); +/** + * @brief This function creates the PST for all gomspace devices. They are + * scheduled in a separate PST because the gomspace library uses + * blocking calls when requesting data from devices. + */ +ReturnValue_t gomspacePstInit(FixedTimeslotTaskIF *thisSequence); } + #endif /* POLLINGSEQUENCEINIT_H_ */ diff --git a/gomspace/p60-dock_client/include/gs/p60-dock/param/p60dock_hk.h b/gomspace/p60-dock_client/include/gs/p60-dock/param/p60dock_hk.h index 63797e37..3db66959 100644 --- a/gomspace/p60-dock_client/include/gs/p60-dock/param/p60dock_hk.h +++ b/gomspace/p60-dock_client/include/gs/p60-dock/param/p60dock_hk.h @@ -47,5 +47,6 @@ extern const param_table_t p60dock_hk[]; extern const int p60dock_hk_count; +extern const int p60dock_config_size; #endif /* P60DOCK_HK_H_ */ diff --git a/gomspace/p60-dock_client/src/p60dock_client.c b/gomspace/p60-dock_client/src/p60dock_client.c index 2fa3232b..5ca69126 100644 --- a/gomspace/p60-dock_client/src/p60dock_client.c +++ b/gomspace/p60-dock_client/src/p60dock_client.c @@ -64,6 +64,7 @@ const param_table_t p60dock_config[] = { }; const int p60dock_config_count = sizeof(p60dock_config) / sizeof(p60dock_config[0]); +const int p60dock_config_size = sizeof(p60dock_config); /** * Setup info about calibration parameters diff --git a/mission/devices/P60DockHandler.cpp b/mission/devices/P60DockHandler.cpp index c7cd8402..24a9c200 100644 --- a/mission/devices/P60DockHandler.cpp +++ b/mission/devices/P60DockHandler.cpp @@ -6,6 +6,14 @@ P60DockHandler::P60DockHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie):DeviceHandlerBase(objectId, comIF, comCookie) { + mode = MODE_NORMAL; + if(comCookie == NULL){ + sif::error << "P60DockHandler invalid com cookie" << std::endl; + } + p60DockCookie = dynamic_cast (comCookie); + if(p60DockCookie == NULL){ + sif::error << "P60DockHandler failed to get P60DockCookie" << std::endl; + } } P60DockHandler::~P60DockHandler() { @@ -13,7 +21,6 @@ P60DockHandler::~P60DockHandler() { void P60DockHandler::doStartUp(){ - } void P60DockHandler::doShutDown(){ @@ -34,7 +41,15 @@ ReturnValue_t P60DockHandler::buildCommandFromCommand( size_t commandDataLen) { switch(deviceCommand) { case(READ_MODULE_CFG):{ + p60DockCookie->setReadModuleCfgMessage(); internalState = InternalStates::READ_MODULE_CFG; + this->pstStep = GET_READ; + break; + } + case(READ_HK):{ + p60DockCookie->setReadHkMessage(); + internalState = InternalStates::READ_HK; + this->pstStep = GET_READ; break; } case(PING): { @@ -48,6 +63,7 @@ ReturnValue_t P60DockHandler::buildCommandFromCommand( void P60DockHandler::fillCommandAndReplyMap(){ this->insertInCommandAndReplyMap(READ_MODULE_CFG, 3); + this->insertInCommandAndReplyMap(READ_HK, 3); } ReturnValue_t P60DockHandler::scanForReply(const uint8_t *start, @@ -58,6 +74,11 @@ ReturnValue_t P60DockHandler::scanForReply(const uint8_t *start, *foundLen = moduleCfgTableSize; break; } + case(InternalStates::READ_HK): { + *foundId = READ_HK; + *foundLen = hkTableSize; + break; + } default: return IGNORE_REPLY_DATA; } @@ -71,6 +92,10 @@ ReturnValue_t P60DockHandler::interpretDeviceReply(DeviceCommandId_t id, handleDeviceTM((SerializeIF*)packet, id, true, true); break; } + case(READ_HK): { + handleDeviceTM((SerializeIF*)packet, id, true, true); + break; + } default: break; } diff --git a/mission/devices/P60DockHandler.h b/mission/devices/P60DockHandler.h index 1c093f4b..a55580ea 100644 --- a/mission/devices/P60DockHandler.h +++ b/mission/devices/P60DockHandler.h @@ -2,6 +2,7 @@ #define MISSION_DEVICES_P60DOCKHANDLER_H_ #include +#include class P60DockHandler: public DeviceHandlerBase { public: @@ -26,15 +27,19 @@ protected: private: static const DeviceCommandId_t PING = 0x1; //!< [EXPORT] : [COMMAND] - static const DeviceCommandId_t READ_MODULE_CFG = 0xE; //!< [EXPORT] : [COMMAND] + static const DeviceCommandId_t READ_MODULE_CFG = 0x71; //!< [EXPORT] : [COMMAND] + static const DeviceCommandId_t READ_HK = 0x72; //!< [EXPORT] : [COMMAND] enum class InternalStates { STATE_NONE, - READ_MODULE_CFG + READ_MODULE_CFG, + READ_HK }; InternalStates internalState = InternalStates::STATE_NONE; + uint8_t moduleCfgTableSize = 188; + uint8_t hkTableSize = 188; - size_t moduleCfgTableSize = 188; + P60DockCookie* p60DockCookie; }; #endif /* MISSION_DEVICES_P60DOCKHANDLER_H_ */