-
Notifications
You must be signed in to change notification settings - Fork 1
/
RepeaterTask.cpp
142 lines (121 loc) · 4.78 KB
/
RepeaterTask.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
/**
******************************************************************************
* File Name : RepeaterTask.cpp
* Description : The base RepeaterTask, includes basic logic for receiving until either
* the buffer is full, or the end-frame byte for protocol is detected.
******************************************************************************
*/
#include "UARTTask.hpp"
#include "SystemDefines.hpp"
#include "RepeaterTask.hpp"
#include "cobs.h"
/**
* @brief Constructor, sets all member variables
*/
RepeaterTask::RepeaterTask(UARTDriver* uartDriver, uint16_t uartTaskCmd) : Task(TASK_REPEATER_QUEUE_DEPTH_OBJS),
kUart_(uartDriver),
uartTaskCommand(uartTaskCmd)
{
// Setup Buffers
protocolRxBuffer = soar_malloc(PROTOCOL_RX_BUFFER_SZ_BYTES + 1);
memset(protocolRxBuffer, 0, PROTOCOL_RX_BUFFER_SZ_BYTES + 1);
// Setup index and flags
protocolMsgIdx = 0;
isProtocolMsgReady = false;
}
/**
* @brief Runcode for the RepeaterTask
*/
void RepeaterTask::Run(void* pvParams)
{
// Arm the interrupt
ReceiveData();
while (1) {
Command cm;
// Wait forever for a command
qEvtQueue->ReceiveWait(cm);
// If this is a repeater Rx Complete event, process it
if (cm.GetCommand() == PROTOCOL_COMMAND && cm.GetTaskCommand() == REPEATER_TASK_COMMANDS::EVENT_REPEATER_RX_COMPLETE) {
SendData(protocolRxBuffer, protocolMsgIdx);
// We can mark the rx buffer as safe to write to now
protocolMsgIdx = 0;
isProtocolMsgReady = false;
}
cm.Reset();
}
}
/**
* @brief Receive data, currently receives by arming interrupt
*/
bool RepeaterTask::ReceiveData()
{
return kUart_->ReceiveIT(&protocolRxChar, this);
}
/**
* @brief Receive data to the buffer
* @return Whether the protocolRxBuffer is ready or not
*/
void RepeaterTask::InterruptRxData(uint8_t errors)
{
// If we already have an unprocessed protocol message, ignore this byte
if (!isProtocolMsgReady) {
// Check COBS byte for end of message
if (protocolRxChar == '\0' || protocolMsgIdx == PROTOCOL_RX_BUFFER_SZ_BYTES) {
// If the message is of insufficient length, reset the buffer
if (protocolMsgIdx < PROTOCOL_MINIMUM_MESSAGE_LENGTH) {
protocolMsgIdx = 0;
isProtocolMsgReady = false;
}
else {
// Null terminate and process
protocolRxBuffer[protocolMsgIdx++] = '\0';
isProtocolMsgReady = true;
// Notify the repeater task
Command cm(PROTOCOL_COMMAND, REPEATER_TASK_COMMANDS::EVENT_REPEATER_RX_COMPLETE);
bool res = qEvtQueue->SendFromISR(cm);
// If we failed to send the event, we should reset the buffer, that way ProtocolTask doesn't stall
if (res == false) {
protocolMsgIdx = 0;
isProtocolMsgReady = false;
}
}
}
else {
protocolRxBuffer[protocolMsgIdx++] = protocolRxChar;
}
}
//Re-arm the interrupt
ReceiveData();
}
/*
* @brief Interface function to quickly send a protobuf message by providing the write buffer and the message ID
* @param writeBuffer The write buffer containing the serialized protobuf message
* @param msgId The message ID of the message
*/
void RepeaterTask::SendProtobufMessage(EmbeddedProto::WriteBufferFixedSize<DEFAULT_PROTOCOL_WRITE_BUFFER_SIZE>& writeBuffer, Proto::MessageID msgId)
{
// Note: This function runs inside the calling task
uint8_t* data = writeBuffer.get_data();
uint16_t size = writeBuffer.get_size();
uint16_t msgSize = GET_COBS_MAX_LEN(size + PROTOCOL_OVERHEAD_BYTES);
// Temporary array to store the pre-COBS message
const uint16_t preCobsSize = size + PROTOCOL_OVERHEAD_BYTES;
uint8_t arr[preCobsSize];
// Wrap in the message header and checksum
arr[0] = (uint8_t)msgId;
memcpy(&(arr[1]), data, size);
uint16_t chkSum = Utils::getCRC16(arr, size + 1);
*((uint16_t*)&arr[preCobsSize - PROTOCOL_CHECKSUM_BYTES]) = chkSum;
// Send the data by wrapping in a COBS frame and sending direct to UART Task
Command protoTx(DATA_COMMAND, uartTaskCommand);
protoTx.AllocateData(msgSize);
// Encode in COBS
cobs_encode_result cobsEncRes = cobs_encode(protoTx.GetDataPointer(), msgSize, arr, preCobsSize);
if (cobsEncRes.status != COBS_ENCODE_OK) {
protoTx.Reset();
SOAR_PRINT("WARNING: COBS encode failed in repeater task\n");
}
SOAR_ASSERT(cobsEncRes.out_len + 1 == msgSize, "COBS Size Mismatch %d %d\n", cobsEncRes.out_len + 1, msgSize);
protoTx.GetDataPointer()[msgSize - 1] = 0x00;
UARTTask::Inst().SendCommandReference(protoTx);
}