eive-obsw/test/testtasks/P60DockTestTask.cpp

165 lines
4.9 KiB
C++

/*
* P60DockTestTask.cpp
*
* Created on: 18.11.2020
* Author: Jakob Meier
*/
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include "P60DockTestTask.h"
#include <csp/drivers/can_socketcan.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;
}
if(getParameters() != 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::getParameters(void) {
// int result = rparam_get_full_table(&node_hk, p60dock_node, P60_PORT_RPARAM,
uint32_t timeout = 1000;
node_hk.rows = (gs_param_table_row_t*)p60dock_hk;
node_hk.id = P60DOCK_HK;
node_hk.row_count = p60dock_hk_count;
node_hk.memory_size = P60DOCK_HK_SIZE;
node_hk.memory = hk_mem;
int result = gs_rparam_get_full_table(&node_hk, p60dockAddress, node_hk.id,
GS_RPARAM_MAGIC_CHECKSUM, timeout);
if (result != 0) {
sif::info << "Error retrieving P60 Dock housekeeping\n" << std::endl;
return HasReturnvaluesIF::RETURN_FAILED;
} else {
uint8_t tableOffsetTemperature = 0x44;
int16_t temperature[2];
size_t parameterSize = sizeof(temperature);
uint32_t flags = 0;
result = gs_param_get_data((gs_param_table_instance_t*) &node_hk,
tableOffsetTemperature, temperature, parameterSize, flags);
sif::info << "P60 Dock Temperature 1: " << temperature[0] << std::endl;
sif::info << "P60 Dock Temperature 2: " << temperature[1] << std::endl;
// sif::info << "Retrieved P60 Dock housekeeping\n" << std::endl;
// /* List all out_en[] values, using parameter name */
// const param_table_t * param = param_find_name(node_hk.table,
// node_hk.count, "out_en");
// if (param != NULL) {
// for (uint8_t index = 0; index < 13; index++) {
// /* Read parameter using name */
// uint8_t *out_en = param_read_addr(
// param->addr + param->size * index, &node_hk,
// param->size);
// sif::info << "out_en" << index << ": " << *out_en << std::endl;
// }
// }
// /* List all c_out[] values, using parameter address */
// param = param_find_addr(node_hk.table, node_hk.count, 0x0000);
// if (param != NULL) {
// for (uint8_t index = 0; index < 13; index++) {
// /* Read parameter using address */
// int16_t *c_out = param_read_addr(
// param->addr + param->size * index, &node_hk,
// param->size);
// sif::info << "c_out" << index << ": " << *c_out << "mA"
// << std::endl;
// }
// }
}
return HasReturnvaluesIF::RETURN_OK;
}
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
}