eive-obsw/test/testtasks/P60DockTestTask.cpp

/*
 * P60DockTestTask.cpp
 *
 *  Created on: 18.11.2020
 *      Author: Jakob Meier
 */

#include <fsfw/serviceinterface/ServiceInterfaceStream.h>

#include <csp/drivers/can_socketcan.h>
#include "P60DockTestTask.h"
#include "gomspace/include/rparam.h"

P60DockTestTask::P60DockTestTask(object_id_t objectId_):
SystemObject(objectId_){
	if(initializeCSPStack() != HasReturnvaluesIF::RETURN_OK){
		sif::error << "P60DockTestTask creation failed" << std::endl;
	}
}


ReturnValue_t P60DockTestTask::performOperation(uint8_t operationCode) {

	if(sendPacket() != HasReturnvaluesIF::RETURN_OK){
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	return HasReturnvaluesIF::RETURN_OK;
}


ReturnValue_t P60DockTestTask::sendPacket(void){

//	char *msg = "HELLO";
//	/* Get packet buffer for data */
//	csp_packet_t *packet = csp_buffer_get(strlen(msg));
//	if (packet == NULL) {
//		/* Could not get buffer element */
//		sif::error("Failed to get buffer element\\n");
//		return HasReturnvaluesIF::RETURN_FAILED;
//	}
//
//	/* Connect P60 Dock */
//	csp_conn_t *conn = csp_connect(CSP_PRIO_NORM, c, CSP_PING,
//			1000, CSP_O_NONE);
//
//	if (conn == NULL) {
//		/* Connect failed */
//		sif::error("Connection failed\\n");
//		/* Remember to free packet buffer */
//		csp_buffer_free(packet);
//	return HasReturnvaluesIF::RETURN_FAILED;
//	}
//
//	/* Copy message to packet */
//	strcpy(packet->data, msg);
//	/* Set packet length */
//	packet->length = strlen(msg);
//
//	/* Send packet */
//	if (!csp_send(conn, packet, 1000)) {
//		/* Send failed */
//		sif::error("Send failed\\n");
//		csp_buffer_free(packet);
//	}
//	/* Close connection */
//	csp_close(conn);

	uint32_t timeout = 1000;
	unsigned int pingSize = 100; // 100 bytes
	uint32_t replyTime = csp_ping(p60dockAddress, timeout, pingSize, CSP_O_NONE);
	sif::info << "Ping address: " << p60dockAddress << ", reply after "
			<< replyTime << "ms" << std::endl;
	return HasReturnvaluesIF::RETURN_OK;
}


ReturnValue_t P60DockTestTask::getParameterTable(unit8_t tableId){

	gs_rparam_query_t * query;
	csp_packet_t * request = csp_buffer_get(RPARAM_QUERY_LENGTH(query, 0));
	if (request == NULL) {
		sif::error << "Failed to get buffer for csp packet" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	csp_conn_t * conn = csp_connect(CSP_PRIO_HIGH, node, GS_CSP_PORT_RPARAM, timeout_ms, CSP_O_CRC32);
	if (!conn) {
		csp_buffer_free(request);
		sif::error << "CSP connection failure" << sif::error << std::endl;
		return HasReturnvaluesIF::RETURN_OK;
	}

	query = (gs_rparam_query_t *) request->data;
	query->action = RPARAM_GET;
	query->table_id = table_id;
	query->length = 0; // == get full table
	query->checksum = csp_hton16(checksum);
	query->seq = 0;
	query->total = 0;

	request->length = RPARAM_QUERY_LENGTH(query, 0);
	if (!csp_send(conn, request, timeout_ms)) {
		csp_buffer_free(request);
		csp_close(conn);
		sif::error << "CSP failed to send packet" << sif::error;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	csp_packet_t * reply;
	unsigned int total_bytes = 0;
	while ((reply = csp_read(conn, timeout_ms)) != NULL) {

		/* We have a reply */
		query = (void *) reply->data;
		const uint16_t qlength = csp_ntoh16(query->length);
		total_bytes += qlength;
		const uint16_t seq = csp_ntoh16(query->seq);
		const uint16_t total = csp_ntoh16(query->total);

		if (query->action == RPARAM_REPLY) {
			error = gs_param_deserialize(tinst, query->payload.packed, qlength, F_FROM_BIG_ENDIAN);
		}
		csp_buffer_free(reply);

		if (error || (seq >= total)) {
			break;
		}
	}

	if (reply == NULL) {
		error = GS_ERROR_TIMEOUT;
	}

	if ((error == GS_OK) && (expected_bytes != total_bytes)) {
		log_warning("%s: expected %u != received %u bytes", __FUNCTION__, expected_bytes, total_bytes);
		error = GS_ERROR_DATA;
	}

	csp_close(conn);

	return error;
}


ReturnValue_t P60DockTestTask::initializeCSPStack(void){
	/* Init CSP and CSP buffer system */
	if (csp_init(cspAddress) != CSP_ERR_NONE
			|| csp_buffer_init(10, 300) != CSP_ERR_NONE) {
		sif::error << "Failed to init CSP\r\n" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}

	csp_iface_t *csp_if_ptr = &csp_if;
	csp_if_ptr = csp_can_socketcan_init("can0", bitrate, promisc);

	/* Set default route and start router */
	int result = csp_rtable_set(CSP_DEFAULT_ROUTE, 0, csp_if_ptr, CSP_NODE_MAC);
	if(result != CSP_ERR_NONE){
		sif::error << "Failed to add can interface to router table"
				<< std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	result = csp_route_start_task(500, 0);
	if(result != CSP_ERR_NONE){
		sif::error << "Failed to start route task" << std::endl;
		return HasReturnvaluesIF::RETURN_FAILED;
	}
	return HasReturnvaluesIF::RETURN_OK;
}


P60DockTestTask::~P60DockTestTask() {
	// TODO Auto-generated destructor stub
}