read module cfg and read hk from p60 dock, intermediate state

bsp_linux/InitMission.cpp

@@ -11,6 +11,7 @@
 #include <fsfw/tasks/TaskFactory.h>
 #include <fsfwconfig/objects/systemObjectList.h>
 #include <fsfwconfig/OBSWConfig.h>
+#include <fsfwconfig/pollingsequence/PollingSequenceFactory.h>
 
 #include <iostream>
 
@@ -129,15 +130,16 @@ void InitMission::initTasks(){
 		sif::error << "Object add component failed" << std::endl;
 	}
 
-	/* Device Handler */
+	FixedTimeslotTaskIF* GomSpacePstTask = TaskFactory::instance()->
-	PeriodicTaskIF* DeviceHandler = TaskFactory::instance()->
+			createFixedTimeslotTask("GS_PST_TASK", 10,
-			createPeriodicTask("Device Handler", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE,
+					PeriodicTaskIF::MINIMUM_STACK_SIZE*4, 1.0, nullptr);
-					1, nullptr);
+	result = pst::gomspacePstInit(GomSpacePstTask);
-	result = DeviceHandler->addComponent(objects::P60DOCK_HANDLER);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
-		sif::error << "Object add component failed" << std::endl;
+		sif::error << "InitMission::initTasks: GomSpace PST initialization "
+				<< "failed!" << std::endl;
 }
 
+
 #if ADD_TEST_CODE == 1
 //	FixedTimeslotTaskIF* TestTimeslotTask = TaskFactory::instance()->
 //			createFixedTimeslotTask("PST_TEST_TASK", 10,
@@ -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();

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<P60DockCookie*> (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_size = p60dock_config_size;
-		moduleConfig.memory = *buffer;
+		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 "
+					<< "Failed retrieving module configuration from P60 dock "
-					<< result << std::endl;
+					<< "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;

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 boardConfigTableNum = 0;
-	uint8_t moduleConfigTable = 1;
+	uint8_t moduleCfgTableNum = 1;
-	uint8_t calibrationParamTable = 2;
+	uint8_t calibrationParamTableNum = 2;
-	uint8_t tmDataTable = 4;
+	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];
 
 };
 

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;
 }

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:

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;
+	}
+}
+

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_ */

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_ */

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

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<P60DockCookie*> (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;
 	}

mission/devices/P60DockHandler.h

@@ -2,6 +2,7 @@
 #define MISSION_DEVICES_P60DOCKHANDLER_H_
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
+#include <bsp_linux/comIF/cookies/P60DockCookie.h>
 
 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_ */