hyl
119 lines
3.7 KiB
C
119 lines
3.7 KiB
C
#include "canard.h"
|
|
#include <stdlib.h>
|
|
|
|
CanardInstance canard;
|
|
CanardTxQueue txQueue;
|
|
|
|
void* canard_memAllocate(CanardInstance* const canard, const size_t amount)
|
|
{
|
|
(void) canard;
|
|
return x_malloc(amount);
|
|
}
|
|
|
|
void canard_memFree(CanardInstance* const canard, void* const pointer)
|
|
{
|
|
(void) canard;
|
|
free(pointer);
|
|
}
|
|
|
|
void slave_comm_init()
|
|
{
|
|
canard = canardInit(&canard_memAllocate, &canard_memFree);
|
|
canard.node_id = savePara.id;
|
|
txQueue = canardTxInit(1536, CANARD_MTU_CAN_CLASSIC); // Set MTU = 64 bytes. There is also CANARD_MTU_CAN_CLASSIC.
|
|
}
|
|
|
|
void slave_comm_tx_process()
|
|
{
|
|
for (const CanardTxQueueItem* ti = NULL; (ti = canardTxPeek(&txQueue)) != NULL;) // Peek at the top of the queue.
|
|
{
|
|
if ((0U == ti->tx_deadline_usec) || (ti->tx_deadline_usec > rt_tick_get_millisecond()*1000)) // Check the deadline.
|
|
{
|
|
if (!slave_send_ext(ti->frame.extended_can_id,(void *)ti->frame.payload,ti->frame.payload_size)) // Send the frame over this redundant CAN iface.
|
|
{
|
|
break; // If the driver is busy, break and retry later.
|
|
}
|
|
}
|
|
|
|
// After the frame is transmitted or if it has timed out while waiting, pop it from the queue and deallocate:
|
|
canard.memory_free(&canard, canardTxPop(&txQueue, ti));
|
|
}
|
|
}
|
|
|
|
rt_inline uint8_t slave_send_ext(uint32_t id,uint8_t *sendBuf,uint8_t len)
|
|
{
|
|
struct rt_can_msg txMsg = {0};
|
|
|
|
txMsg.id = id;
|
|
txMsg.ide = RT_CAN_EXTID;
|
|
txMsg.rtr = RT_CAN_DTR;
|
|
txMsg.len = len;
|
|
for(rt_uint8_t i=0;i<len;i++)
|
|
{
|
|
txMsg.data[i] = sendBuf[i];
|
|
}
|
|
|
|
|
|
return rt_device_write(slaveDev,0,&txMsg,sizeof(txMsg));
|
|
|
|
}
|
|
|
|
void slave_comm_rx_process()
|
|
{
|
|
CanardRxTransfer transfer;
|
|
CanardFrame receivedFrame;
|
|
struct rt_can_msg canRxMsg = {0};
|
|
uint32_t rxTimestampUsec;
|
|
int8_t result;
|
|
|
|
while(rt_mq_recv(&slave_rec_msgq,&canRxMsg,sizeof(canRxMsg),RT_WAITING_NO) == RT_EOK)
|
|
{
|
|
|
|
receivedFrame.extended_can_id = canRxMsg.id;
|
|
receivedFrame.payload_size = canRxMsg.len;
|
|
receivedFrame.payload = canRxMsg.data;
|
|
|
|
rxTimestampUsec = rt_tick_get_millisecond()*1000;
|
|
|
|
result = canardRxAccept(&canard,
|
|
rxTimestampUsec, // When the frame was received, in microseconds.
|
|
&receivedFrame, // The CAN frame received from the bus.
|
|
0, // If the transport is not redundant, use 0.
|
|
&transfer,
|
|
NULL);
|
|
if (result < 0)
|
|
{
|
|
// An error has occurred: either an argument is invalid or we've ran out of memory.
|
|
// It is possible to statically prove that an out-of-memory will never occur for a given application if
|
|
// the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
|
|
// Reception of an invalid frame is NOT an error.
|
|
}
|
|
else if (result == 1)
|
|
{
|
|
void process_received_transfer(const uint8_t index,CanardRxTransfer* const transfer);
|
|
process_received_transfer(0, &transfer); // A transfer has been received, process it.
|
|
canard.memory_free(&canard, transfer.payload); // Deallocate the dynamic memory afterwards.
|
|
}
|
|
else
|
|
{
|
|
// Nothing to do.
|
|
// The received frame is either invalid or it's a non-last frame of a multi-frame transfer.
|
|
// Reception of an invalid frame is NOT reported as an error because it is not an error.
|
|
}
|
|
}
|
|
}
|
|
|
|
void process_received_transfer(const uint8_t index,CanardRxTransfer* const transfer)
|
|
{
|
|
LOG_D("slave rec id:%d size:%d",transfer->metadata.remote_node_id,transfer->payload_size);
|
|
if(transfer->metadata.remote_node_id == canard.node_id)
|
|
{
|
|
slavePackDef *p = (slavePackDef *)transfer->payload;
|
|
recCmd = p->packCmd;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|