67 lines
1.8 KiB
C++
67 lines
1.8 KiB
C++
#include "node_interface.hpp"
|
|
|
|
#include "FreeRTOS.h"
|
|
#include "task.h"
|
|
#include <string.h>
|
|
|
|
static void rxTaskFn(void *pvParameters) {
|
|
GobotRPC_NI *ni = (GobotRPC_NI *)pvParameters;
|
|
ni->rxTask();
|
|
}
|
|
|
|
GobotRPC_NI::GobotRPC_NI(unsigned int core, QueueHandle_t txQueue, QueueHandle_t rxQueue) {
|
|
this->core = core;
|
|
|
|
this->txQueue = txQueue;
|
|
this->rxQueue = rxQueue;
|
|
|
|
this->onPackageRx = NULL;
|
|
this->onPackageRxArgs = NULL;
|
|
|
|
xTaskCreateAffinitySet(
|
|
rxTaskFn,
|
|
"GobotRPC_NI RX Task",
|
|
2048,
|
|
this,
|
|
4,
|
|
this->core,
|
|
&this->rxTaskHandle
|
|
);
|
|
}
|
|
|
|
void GobotRPC_NI::registerOnPackageRxCallback(onPackageRxCallback callback, void *context) {
|
|
this->onPackageRx = callback;
|
|
this->onPackageRxArgs = context;
|
|
}
|
|
|
|
void GobotRPC_NI::rxTask() {
|
|
while (true) {
|
|
GobotRPC_NI_Package_Transport package;
|
|
|
|
if (xQueueReceive(this->rxQueue, &package, portMAX_DELAY) == pdTRUE) {
|
|
GobotRPCHeaderInfo header = extractGobotRPCHeader(package.data);
|
|
|
|
if(this->onPackageRx != NULL) {
|
|
if (!checkCRC(package.data, header.len)) {
|
|
volatile int a = 0;
|
|
continue;
|
|
}
|
|
|
|
this->onPackageRx(
|
|
onPackageRxArgs,
|
|
package.data + GobotRPC_Package_DATA_OFFSET,
|
|
header.len, header.type, header.number
|
|
);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void GobotRPC_NI::sendPackage(char *data, size_t len, GobotRPCTypes type, GobotRPCNumber number) {
|
|
GobotRPC_NI_Package_Transport package;
|
|
memcpy(package.data + GobotRPC_Package_DATA_OFFSET, data, len);
|
|
assembleGobotRPCHeader(package.data, number, type, len);
|
|
assembleCRC(package.data, len);
|
|
xQueueSend(this->txQueue, &package, portMAX_DELAY);
|
|
}
|