89 lines
2.2 KiB
C++
89 lines
2.2 KiB
C++
|
/*
|
||
|
|
* P60DockTestTask.cpp
|
||
|
|
*
|
||
|
|
* Created on: 18.11.2020
|
||
|
|
* Author: jakob
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <gs/csp/drivers/can/can.h>
|
||
|
|
#include "P60DockTestTask.h"
|
||
|
|
|
||
|
|
P60DockTestTask::P60DockTestTask(object_id_t objectId_):
|
||
|
|
SystemObject(objectId_){
|
||
|
|
/* Init buffer system with 10 packets of maximum 320 bytes each */
|
||
|
|
// csp_buffer_init(10, 320);
|
||
|
|
|
||
|
|
uint8_t device = 0;
|
||
|
|
uint8_t csp_addr = 4; /* Current address of p60 dock */
|
||
|
|
uint8_t mtu = 320; /* Packets larger than the set mtu will be discarded */
|
||
|
|
char name[5] = "can0";
|
||
|
|
|
||
|
|
/* Init the CAN interface */
|
||
|
|
gs_error_t result = gs_csp_can_init(device, csp_addr, mtu, name, csp_if);
|
||
|
|
if(result != GS_OK){
|
||
|
|
sif::error << "gs_csp_can_init failed with error code: " << result
|
||
|
|
<< std::endl;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
ReturnValue_t P60DockTestTask::performOperation(uint8_t operationCode) {
|
||
|
|
|
||
|
|
char data[5] = "test";
|
||
|
|
int timeout_ms = 1000;
|
||
|
|
/* Send a csp packet to the can interface */
|
||
|
|
g_error_t result = csp_can_tx_frame(csp_if, canExtMsgId, (uint8*) data, sizeof(data),
|
||
|
|
timeout_ms);
|
||
|
|
if(result != GS_OK){
|
||
|
|
sif::error << "csp_can_tx_frame failed with error code " << result
|
||
|
|
<< std::endl;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
ReturnValue_t P60DockTestTask::sendPacket(void){
|
||
|
|
|
||
|
|
/* Get packet buffer for data */
|
||
|
|
csp_packet_t *packet = csp_buffer_get(data_size);
|
||
|
|
if (packet == NULL) {
|
||
|
|
/* Could not get buffer element */
|
||
|
|
sif::error("Failed to get buffer element\\n");
|
||
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Connect to host HOST, port PORT with regular UDP-like protocol and
|
||
|
|
* 1000 ms timeout */
|
||
|
|
csp_conn_t *conn = csp_connect(CSP_PRIO_NORM, HOST, PORT, 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 */
|
||
|
|
char *msg = "HELLO";
|
||
|
|
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);
|
||
|
|
|
||
|
|
return HasReturnvaluesIF::RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
P60DockTestTask::~P60DockTestTask() {
|
||
|
|
// TODO Auto-generated destructor stub
|
||
|
|
}
|
||
|
|
|