Merge remote-tracking branch 'origin/develop' into mueller/master

bsp_linux/InitMission.cpp

@@ -10,6 +10,7 @@
 #include <fsfw/tasks/TaskFactory.h>
 #include <fsfwconfig/objects/systemObjectList.h>
 #include <fsfwconfig/OBSWConfig.h>
+#include <fsfwconfig/pollingsequence/PollingSequenceFactory.h>
 
 #include <iostream>
 
@@ -125,16 +126,34 @@ void InitMission::initTasks(){
 		sif::error << "Object add component failed" << std::endl;
 	}
 
+//	PeriodicTaskIF* P60DockTask = TaskFactory::instance()->
+//			createPeriodicTask("P60Dock Task", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE*4,
+//					1.6, nullptr);
+//	result = P60DockTask->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", 50,
+					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()->
-			createFixedTimeslotTask("PST_TEST_TASK", 10,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
-	result = pst::pollingSequenceTestFunction(TestTimeslotTask);
-	if(result != HasReturnvaluesIF::RETURN_OK) {
-		sif::error << "InitMission::createTasks: Test PST initialization "
-				<< "failed!" << std::endl;
-	}
+//	FixedTimeslotTaskIF* TestTimeslotTask = TaskFactory::instance()->
+//			createFixedTimeslotTask("PST_TEST_TASK", 10,
+//					PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
+//	result = pst::pollingSequenceTestFunction(TestTimeslotTask);
+//	if(result != HasReturnvaluesIF::RETURN_OK) {
+//		sif::error << "InitMission::createTasks: Test PST initialization "
+//				<< "failed!" << std::endl;
+//	}
+
 #endif
 
 	//Main thread sleep
@@ -143,13 +162,18 @@ void InitMission::initTasks(){
 	UdpBridgeTask->startTask();
 	UdpPollingTask->startTask();
 
+	GomSpacePstTask->startTask();
+
 	PusVerification->startTask();
 	PusEvents->startTask();
 	PusHighPrio->startTask();
 	PusMedPrio->startTask();
 	PusLowPrio->startTask();
+
+//	P60DockTask->startTask();
+
 #if ADD_TEST_CODE == 1
-	TestTimeslotTask->startTask();
+//	TestTimeslotTask->startTask();
 #endif
 	sif::info << "Tasks started.." << std::endl;
 }

bsp_linux/bsp_linux.mk

@@ -1,4 +1,6 @@
 CXXSRC += $(wildcard $(CURRENTPATH)/*.cpp)
+CXXSRC += $(wildcard $(CURRENTPATH)/comIF/cookies/*.cpp)
+CXXSRC += $(wildcard $(CURRENTPATH)/comIF/*.cpp)
 CSRC += $(wildcard $(CURRENTPATH)/*.c)
 
 CSRC += $(wildcard $(CURRENTPATH)/boardconfig/*.c)

bsp_linux/comIF/CspComIF.cpp

@@ -0,0 +1,211 @@
+#include "CspComIF.h"
+#include <bsp_linux/comIF/cookies/CspCookie.h>
+#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
+#include <csp/drivers/can_socketcan.h>
+#include <fsfw/serialize/SerializeAdapter.h>
+
+CspComIF::CspComIF(object_id_t objectId) :
+		SystemObject(objectId) {
+
+}
+
+CspComIF::~CspComIF() {
+}
+
+ReturnValue_t CspComIF::initializeInterface(CookieIF *cookie)  {
+	if(cookie == nullptr) {
+		return NULLPOINTER;
+	}
+
+	CspCookie* cspCookie = dynamic_cast<CspCookie*>(cookie);
+	if(cspCookie == nullptr) {
+		return NULLPOINTER;
+	}
+
+	/* Perform CAN and CSP initialization only once */
+	if(cspDeviceMap.empty()){
+		/* Define the memory to allocate for the CSP stack */
+		int buf_count = 10;
+		int buf_size = 300;
+		/* Init CSP and CSP buffer system */
+		if (csp_init(cspClientAddress) != CSP_ERR_NONE
+				|| csp_buffer_init(buf_count, buf_size) != CSP_ERR_NONE) {
+			sif::error << "Failed to init CSP\r\n" << std::endl;
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+
+		int promisc = 0; // Set filter mode on
+		csp_iface_t *csp_if_ptr = &csp_if;
+		csp_if_ptr = csp_can_socketcan_init(canInterface, bitrate, promisc);
+
+		/* Set default route and start router */
+		uint8_t address = CSP_DEFAULT_ROUTE;
+		uint8_t netmask = 0;
+		uint8_t mac = CSP_NODE_MAC;
+		int result = csp_rtable_set(address, netmask, csp_if_ptr, mac);
+		if(result != CSP_ERR_NONE){
+			sif::error << "Failed to add can interface to router table"
+					<< std::endl;
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+
+		/* Start the route task */
+		unsigned int task_stack_size = 500;
+		unsigned int priority = 0;
+		result = csp_route_start_task(task_stack_size, priority);
+		if(result != CSP_ERR_NONE){
+			sif::error << "Failed to start csp route task" << std::endl;
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+	}
+
+	uint8_t cspAddress = cspCookie->getCspAddress();
+	uint16_t maxReplyLength = cspCookie->getMaxReplyLength();
+	if(cspDeviceMap.find(cspAddress) == cspDeviceMap.end()){
+		/* Insert device information in CSP map */
+		cspDeviceMap.emplace(cspAddress, vectorBuffer(maxReplyLength));
+	}
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CspComIF::sendMessage(CookieIF *cookie,
+		const uint8_t * sendData, size_t sendLen) {
+	int result;
+	if(cookie == NULL){
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	CspCookie* cspCookie = dynamic_cast<CspCookie*> (cookie);
+	if(cspCookie == NULL){
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	/* Extract csp port and bytes to query from command buffer */
+	uint8_t cspPort;
+	uint16_t querySize;
+	result = getPortAndQuerySize(&sendData, &sendLen, &cspPort, &querySize);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+		return result;
+	}
+	uint8_t cspAddress = cspCookie->getCspAddress();
+	switch(cspPort) {
+	case(Ports::CSP_PING): {
+		initiatePingRequest(cspAddress, querySize);
+		break;
+	}
+	case(Ports::CSP_REBOOT): {
+		csp_reboot(cspAddress);
+		break;
+	}
+	case(Ports::P60_PORT_GNDWDT_RESET):
+	case(Ports::P60_PORT_RPARAM): {
+		/* No CSP fixed port was selected. Send data to the specified port and
+		 * wait for querySize number of bytes */
+		result = cspTransfer(cspAddress, cspPort, sendData, sendLen,
+				querySize);
+		if(result != HasReturnvaluesIF::RETURN_OK){
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+		replySize = querySize;
+		break;
+	}
+	default:
+		sif::error << "CspComIF: Invalid port specified" << std::endl;
+		break;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CspComIF::getSendSuccess(CookieIF *cookie) {
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CspComIF::requestReceiveMessage(CookieIF *cookie,
+		size_t requestLen) {
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CspComIF::readReceivedMessage(CookieIF *cookie,
+		uint8_t** buffer, size_t* size) {
+	if(cookie == NULL){
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	CspCookie* cspCookie = dynamic_cast<CspCookie*> (cookie);
+	if(cspCookie == NULL){
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	uint8_t cspAddress = cspCookie->getCspAddress();
+
+	*buffer = cspDeviceMap[cspAddress].data();
+	*size = replySize;
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort,
+		const uint8_t* cmdBuffer, int cmdBufferLen, uint16_t querySize) {
+
+	uint32_t timeout_ms = 500;
+	vectorBufferIter iter = cspDeviceMap.find(cspAddress);
+	if(iter == cspDeviceMap.end()){
+		sif::error << "CSP device with address " << cspAddress << " no found in"
+				<< " device map" << std::endl;
+	}
+	uint8_t* replyBuffer = iter->second.data();
+	uint8_t tmpCmdBuffer[cmdBufferLen];
+	memcpy(tmpCmdBuffer, cmdBuffer, cmdBufferLen);
+
+	csp_conn_t * conn = csp_connect(CSP_PRIO_HIGH, cspAddress, cspPort, 0,
+			CSP_O_NONE);
+
+	int result = csp_transaction_persistent(conn, timeout_ms,
+			tmpCmdBuffer, cmdBufferLen, replyBuffer, querySize);
+	if(querySize != 0){
+		if(result != querySize){
+			sif::error << "CSP transfer failed to receive all requested bytes "
+					   << std::endl;
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+	} else {
+		if(result != 1){
+			sif::error << "CSP transfer failed" << std::endl;
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+	}
+
+	csp_close(conn);
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CspComIF::getPortAndQuerySize(const uint8_t** sendData,
+		size_t* sendLen, uint8_t* cspPort, uint16_t* querySize) {
+	ReturnValue_t result = SerializeAdapter::deSerialize(cspPort, sendData,
+			sendLen, SerializeIF::Endianness::BIG);
+	if(result != HasReturnvaluesIF::RETURN_OK){
+		sif::error << "CspComIF: Failed to deserialize CSP port from command "
+				<< "buffer" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	SerializeAdapter::deSerialize(querySize, sendData, sendLen,
+			SerializeIF::Endianness::BIG);
+	if(result != HasReturnvaluesIF::RETURN_OK){
+		sif::error << "CspComIF: Failed to deserialize querySize from command "
+				<< "buffer" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void CspComIF::initiatePingRequest(uint8_t cspAddress, uint16_t querySize){
+	uint32_t timeout_ms = 500;
+	uint32_t replyTime = csp_ping(cspAddress, timeout_ms, querySize,
+			CSP_O_NONE);
+	sif::info << "Ping address: " << cspAddress << ", reply after "
+			<< replyTime << " ms" << std::endl;
+	/* Store reply time in reply buffer * */
+	uint8_t* replyBuffer = cspDeviceMap[cspAddress].data();
+	memcpy(replyBuffer, &replyTime, sizeof(replyTime));
+	replySize = sizeof(replyTime);
+}

bsp_linux/comIF/CspComIF.h

@@ -0,0 +1,89 @@
+#ifndef BSP_LINUX_COMIF_COOKIES_CSPCOMIF_H_
+#define BSP_LINUX_COMIF_COOKIES_CSPCOMIF_H_
+
+#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
+#include <fsfw/objectmanager/SystemObject.h>
+#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+#include <csp/csp.h>
+
+#include <vector>
+#include <unordered_map>
+
+/**
+ * @brief	This class serves as the communication interface to devices
+ * 			supporting the CSP protocol. As physical layer can0 is used
+ * 			in this implementation.
+ * @author	J. Meier
+ */
+class CspComIF: public DeviceCommunicationIF, public SystemObject {
+public:
+	CspComIF(object_id_t objectId);
+	virtual ~CspComIF();
+
+	ReturnValue_t initializeInterface(CookieIF * cookie) override;
+	ReturnValue_t sendMessage(CookieIF *cookie, const uint8_t * sendData,
+			size_t sendLen) override;
+	ReturnValue_t getSendSuccess(CookieIF *cookie) override;
+	ReturnValue_t requestReceiveMessage(CookieIF *cookie,
+			size_t requestLen) override;
+	ReturnValue_t readReceivedMessage(CookieIF *cookie,
+			uint8_t **readData, size_t *readLen) override;
+
+private:
+
+	/**
+	 * @brief	This function initiates the CSP transfer.
+	 *
+	 * @param cspAddress	The CSP address of the target device.
+	 * @param cspPort		The port of the target device.
+	 * @param timeout		The timeout to wait for csp_send and csp_read
+	 * 						functions. Specifies how long the functions wait
+	 * 						for a successful operation.
+	 * @param cmdBuffer		The data to send.
+	 * @param cmpBuffer		The number of bytes to send.
+	 * @param querySize		The size of the requested message.
+	 */
+	ReturnValue_t cspTransfer(uint8_t cspAddress, uint8_t cspPort,
+			const uint8_t* cmdBuffer, int cmdBufferLen, uint16_t querySize);
+
+	enum Ports {
+		CSP_PING = 1,
+		CSP_REBOOT = 4,
+		P60_PORT_RPARAM = 7,
+		P60_PORT_GNDWDT_RESET = 9
+	};
+
+
+	typedef uint8_t node_t;
+	using vectorBuffer = std::vector<uint8_t>;
+	using VectorBufferMap = std::unordered_map<node_t, vectorBuffer>;
+	using vectorBufferIter = VectorBufferMap::iterator;
+
+	/* In this map assigns reply buffers to a CSP device */
+	VectorBufferMap cspDeviceMap;
+
+	uint16_t replySize = 0;
+
+	/* This is the CSP address of the OBC. */
+	node_t cspClientAddress = 1;
+
+	/* Interface struct for csp protocol stack */
+	csp_iface_t csp_if;
+
+	char canInterface[5] = "can0";
+	int bitrate = 1000;
+
+	/**
+	 * @brief	Function to extract the csp port and the query size from the
+	 * 			command buffer.
+	 */
+	ReturnValue_t getPortAndQuerySize(const uint8_t** sendData, size_t* sendLen,
+			uint8_t* cspPort, uint16_t* querySize);
+
+	/**
+	 * @brief	This function initiates the ping request.
+	 */
+	void initiatePingRequest(uint8_t cspAddress, uint16_t querySize);
+};
+
+#endif /* BSP_LINUX_COMIF_COOKIES_CSPCOMIF_H_ */

bsp_linux/comIF/cookies/CspCookie.cpp

@@ -0,0 +1,16 @@
+#include "CspCookie.h"
+
+CspCookie::CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_) :
+		maxReplyLength(maxReplyLength_), cspAddress(cspAddress_) {
+}
+
+CspCookie::~CspCookie() {
+}
+
+uint16_t CspCookie::getMaxReplyLength(){
+	return maxReplyLength;
+}
+
+uint8_t CspCookie::getCspAddress(){
+	return cspAddress;
+}

bsp_linux/comIF/cookies/CspCookie.h

@@ -0,0 +1,27 @@
+#ifndef BSP_LINUX_COMIF_COOKIES_CSPCOOKIE_H_
+#define BSP_LINUX_COMIF_COOKIES_CSPCOOKIE_H_
+
+#include <fsfw/devicehandlers/CookieIF.h>
+#include <stdint.h>
+
+/**
+ * @brief	This is the cookie for devices supporting the CSP (CubeSat Space
+ * 			Protocol).
+ * @author	J. Meier
+ */
+class CspCookie: public CookieIF {
+public:
+
+	CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_);
+	virtual ~CspCookie();
+
+	uint16_t getMaxReplyLength();
+	uint8_t getCspAddress();
+
+private:
+
+	uint16_t maxReplyLength;
+	uint8_t cspAddress;
+};
+
+#endif /* BSP_LINUX_COMIF_COOKIES_CSPCOOKIE_H_ */