110 lines
3.2 KiB
C++
110 lines
3.2 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(pingP60dock() != HasReturnvaluesIF::RETURN_OK){
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
|
|
if(getParameters() != HasReturnvaluesIF::RETURN_OK){
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
|
|
ReturnValue_t P60DockTestTask::pingP60dock(void){
|
|
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) {
|
|
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 = hkMem;
|
|
/* Retriev all houskeeping data from the P60 dock and store it in hkMem
|
|
* array */
|
|
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" << std::endl;
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
} else {
|
|
uint8_t tableOffsetTemperature = 0x44;
|
|
int16_t temperature;
|
|
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: " << temperature << std::endl;
|
|
|
|
uint16_t vbat_v;
|
|
parameterSize = sizeof(vbat_v);
|
|
uint8_t vbat_v_offset = 0x74;
|
|
result = gs_param_get_data((gs_param_table_instance_t*) &node_hk,
|
|
vbat_v_offset, &vbat_v, parameterSize, flags);
|
|
sif::info << "VBAT_V: " << vbat_v << std::endl;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
|
|
ReturnValue_t P60DockTestTask::initializeCSPStack(void){
|
|
/* Init CSP and CSP buffer system */
|
|
if (csp_init(cspClientAddress) != 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
|
|
}
|
|
|