eive-obsw/bsp_linux/comIF/P60DockComIF.cpp

171 lines
5.1 KiB
C++

#include "P60DockComIF.h"
#include <csp/csp.h>
#include <csp/drivers/can_socketcan.h>
#include <bsp_linux/comIF/cookies/P60DockCookie.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <gomspace/libparam_client/include/gs/param/rparam.h>
P60DockComIF::P60DockComIF(object_id_t objectId) :
SystemObject(objectId) {
}
P60DockComIF::~P60DockComIF() {
}
ReturnValue_t P60DockComIF::initializeInterface(CookieIF *cookie) {
if(cookie == nullptr) {
return NULLPOINTER;
}
P60DockCookie* p60DockCookie = dynamic_cast<P60DockCookie*>(cookie);
if(p60DockCookie == nullptr) {
return NULLPOINTER;
}
char* canInterface = p60DockCookie->getCanIf();
int bitrate = p60DockCookie->getBitrate();
/* 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;
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t P60DockComIF::sendMessage(CookieIF *cookie,
const uint8_t * sendData, size_t sendLen) {
if(cookie == NULL){
return HasReturnvaluesIF::RETURN_FAILED;
}
P60DockCookie* p60DockCookie = dynamic_cast<P60DockCookie*> (cookie);
if(p60DockCookie == NULL){
return HasReturnvaluesIF::RETURN_FAILED;
}
MessageType_t messageType = p60DockCookie->getMessageType();
switch(messageType){
case(P60DockCookie::PING):{
uint32_t timeout = 1000; // ms
unsigned int pingSize = 100; // 100 bytes
uint8_t p60DockAddress = p60DockCookie->getCspAddress();
uint32_t replyTime = csp_ping(p60DockAddress, timeout, pingSize,
CSP_O_NONE);
sif::info << "Ping address: " << p60DockAddress << ", reply after "
<< replyTime << " ms" << std::endl;
/* Store reply time in reply buffer * */
memcpy(replyBuffer, &replyTime, sizeof(replyTime));
break;
}
case(P60DockCookie::REBOOT):{
csp_reboot(p60DockCookie->getCspAddress());
break;
}
default:
break;
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t P60DockComIF::getSendSuccess(CookieIF *cookie) {
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t P60DockComIF::requestReceiveMessage(CookieIF *cookie,
size_t requestLen) {
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t P60DockComIF::readReceivedMessage(CookieIF *cookie,
uint8_t** buffer, size_t* size) {
if(cookie == NULL){
return HasReturnvaluesIF::RETURN_FAILED;
}
P60DockCookie* p60DockCookie = dynamic_cast<P60DockCookie*> (cookie);
if(p60DockCookie == NULL){
return HasReturnvaluesIF::RETURN_FAILED;
}
MessageType_t messageType = p60DockCookie->getMessageType();
switch(messageType){
case(P60DockCookie::READ_MODULE_CONFIG):{
uint32_t timeout = 1000; // ms
uint8_t p60dockAddress = p60DockCookie->getCspAddress();
gs_param_table_instance_t table;
table.rows = (gs_param_table_row_t*)p60dock_config;
table.id = moduleCfgTableNum;
table.row_count = p60dock_config_count;
table.memory_size = P60DOCK_PARAM_SIZE;
table.memory = replyBuffer;
/* Read complete module configuration table from P60 Dock and store data
* in buffer */
int result = gs_rparam_get_full_table(&table, p60dockAddress,
table.id, GS_RPARAM_MAGIC_CHECKSUM, timeout);
*size = P60DOCK_PARAM_SIZE;
if (result != GS_OK) {
sif::info
<< "Failed retrieving module configuration from P60 dock "
<< "with error code " << result << std::endl;
return HasReturnvaluesIF::RETURN_FAILED;
}
break;
}
case(P60DockCookie::READ_HK):{
uint32_t timeout = 1000; // ms
uint8_t p60dockAddress = p60DockCookie->getCspAddress();
table.rows = (gs_param_table_row_t*)p60dock_hk;
table.id = tmTableNum;
table.row_count = p60dock_hk_count;
table.memory_size = P60DOCK_HK_SIZE;
table.memory = replyBuffer;
/* Read complete module configuration table from P60 Dock and store data
* in buffer */
int result = gs_rparam_get_full_table(&table, p60dockAddress,
table.id, GS_RPARAM_MAGIC_CHECKSUM, timeout);
*size = P60DOCK_HK_SIZE;
if (result != GS_OK) {
sif::info
<< "Failed retrieving telemetry from P60 dock with error "
<< "code " << result << std::endl;
return HasReturnvaluesIF::RETURN_FAILED;
}
break;
}
default:
break;
}
return HasReturnvaluesIF::RETURN_OK;
}