extended p60 dock test task

test/testtasks/P60DockTestTask.cpp

@@ -9,6 +9,7 @@
 
 #include <csp/drivers/can_socketcan.h>
 #include "P60DockTestTask.h"
+#include "gomspace/include/rparam.h"
 
 P60DockTestTask::P60DockTestTask(object_id_t objectId_):
 SystemObject(objectId_){
@@ -73,6 +74,74 @@ ReturnValue_t P60DockTestTask::sendPacket(void){
 }
 
 
+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

test/testtasks/P60DockTestTask.h

@@ -39,6 +39,7 @@ private:
 
 	ReturnValue_t sendPacket(void);
 	ReturnValue_t initializeCSPStack(void);
+	ReturnValue_t getParameterTable(unit8_t tableId);
 
 };