alexander/gobot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Build RPC Parsers

Browse Source
This commit is contained in:
AlexanderHD27
2024-12-30 00:46:36 +01:00
parent ec2225aa4e
commit 877040362c
13 changed files with 1162 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/cmake/CMakeLists.txt Normal file
View File

@@ -0,0 +1,15 @@
add_library(GobotRPC STATIC
${CMAKE_CURRENT_LIST_DIR}/../rx.cpp
${CMAKE_CURRENT_LIST_DIR}/../init.cpp
${CMAKE_CURRENT_LIST_DIR}/../protocol.cpp
${CMAKE_CURRENT_LIST_DIR}/../error_msg.cpp
)
target_include_directories(GobotRPC PUBLIC
${CMAKE_CURRENT_LIST_DIR}/../include
${CMAKE_CURRENT_LIST_DIR}/../../..
)
target_link_libraries(GobotRPC
FreeRTOS-Kernel-Heap4
)

43
i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/error_msg.cpp Normal file
View File

@@ -0,0 +1,43 @@
#include "gobotrpc.hpp"
#include <stdio.h>
const char * TEST_MSG_RPC_RESULT_GOOD = "Good";
const char * TEST_MSG_RPC_RESULT_PACKAGE_READY = "Package Ready";
const char * TEST_MSG_RPC_RESULT_ERROR_INVALID_FRAMESUBMITIONRESULT = "Invalid FrameSubmitionResult";
const char * TEST_MSG_RPC_RESULT_ERROR_INVALID_RPC_TYPE = "Invalid RPC Type (0b10)";
const char * TEST_MSG_RPC_RESULT_ERROR_INVALID_RPC_NUMBER = "Invalid RPC Number";
const char * TEST_MSG_RPC_RESULT_ERROR_NO_EMPTY_OR_FITTING_SLOT = "No empty or fitting slot for submitted frame";
char * mapFrameSubmitionResult2String(FrameSubmitionResult res) {
switch (res) {
case PENDING: return (char *)(TEST_MSG_RPC_RESULT_GOOD);
case COMPLEATE: return (char *) TEST_MSG_RPC_RESULT_PACKAGE_READY;
case ERROR_INVALID_RPC_TYPE: return (char *) TEST_MSG_RPC_RESULT_ERROR_INVALID_RPC_TYPE;
case ERROR_INVALID_RPC_NUMBER: return (char *) TEST_MSG_RPC_RESULT_ERROR_INVALID_RPC_NUMBER;
case ERROR_NO_EMPTY_OR_FITTING_SLOT: return (char *) TEST_MSG_RPC_RESULT_ERROR_INVALID_FRAMESUBMITIONRESULT;
}
return (char *) TEST_MSG_RPC_RESULT_ERROR_INVALID_FRAMESUBMITIONRESULT;
}
void GobotRPCParser::print_out_slots() {
for(int i = 0; i < NUM_SLOTS; i++) {
printf("[%d] %d%d (%x-%d) %d T%08x ", i,
buffer[i].is_complete, buffer[i].is_in_use, buffer[i].sender_address,
buffer[i].rpcNum, buffer[i].type, buffer[i].timestamp
);
for(int j = 0; j < MAX_PAGE_SIZES; j++) {
printf("%02x", buffer[i].buffer[j]);
}
printf(" ");
for(int j = 0; j < 32; j++) {
printf("%d", (buffer[i].used_bit_masked >> (31-j)) & 1);
}
printf("\n");
}
}

64
i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/gobotrpc.hpp Normal file
View File

@@ -0,0 +1,64 @@
#pragma once
#include <stdint.h>
#include <strings.h>
#include "protocol.hpp"
#define MAX_PAGE_SIZES 4*7
enum FrameSubmitionResult {
PENDING,
COMPLEATE,
ERROR_INVALID_RPC_TYPE,
ERROR_INVALID_RPC_NUMBER,
ERROR_NO_EMPTY_OR_FITTING_SLOT,
};
struct RPC_RX_PackageSlot {
char buffer[MAX_PAGE_SIZES];
bool is_complete;
bool is_in_use;
uint32_t sender_address;
enum RPCNumber rpcNum;
enum RPCType type;
uint32_t used_bit_masked;
uint32_t timestamp;
};
struct RPCPackage {
char buffer[MAX_PAGE_SIZES];
size_t length;
uint32_t addr;
RPCNumber rpcNum;
RPCType type;
};
#define NUM_SLOTS 8
class GobotRPCParser {
private:
RPC_RX_PackageSlot buffer[NUM_SLOTS];
uint32_t masked [NUM_SLOTS];
uint32_t address_mask;
size_t finishedScanIndex;
void find_fitting_slot(RPCHeader header, uint32_t addr, int * fitting_slot, int * empty_slot);
static void insertFrameInPackageSlot(RPC_RX_PackageSlot * package, char * data, size_t segment, size_t length);
void freePackageSlot(size_t index);
public:
GobotRPCParser();
void print_out_slots();
enum FrameSubmitionResult submit_frame(char * data, size_t length, uint32_t addr, uint32_t timestamp);
int getFinishedIndexPackages();
void retrivePackage(RPCPackage * dest, int index);
};
char * mapFrameSubmitionResult2String(FrameSubmitionResult res);
FrameSubmitionResult getPackageStatus(RPC_RX_PackageSlot * package);

32
i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/protocol.hpp Normal file
View File

@@ -0,0 +1,32 @@
#pragma once
#include <map>
#include <stdint.h>
enum RPCNumber {
Get_Info = 0x0,
Home = 0x01,
Move_Step = 0x2,
Move_XY = 0x3,
Set_Padding = 0x4,
Release_Motors = 0x5,
Drop_Stone = 0x6,
Get_Stone_Status = 0x7,
Move_Z_Axis = 0x8,
Set_Vacum = 0x9,
Invalid = 0xf
};
const uint32_t RPC_NUMBER_MAX = 0x9;
enum RPCType {
REQUEST = 0b00,
RESPONSE_RPC = 0b01,
INVALID = 0b10,
ERROR = 0b11
};
struct RPCHeader {
RPCNumber rpcNum : 4;
RPCType type : 2;
unsigned char segment: 2;
};

19
i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/init.cpp Normal file
View File

@@ -0,0 +1,19 @@
#include "gobotrpc.hpp"
#include <string.h>
GobotRPCParser::GobotRPCParser() {
for(int i = 0; i < NUM_SLOTS; i++) {
buffer[i].timestamp = 0;
buffer[i].is_complete = false;
buffer[i].is_in_use = false;
buffer[i].used_bit_masked = 0;
buffer[i].sender_address = 0;
buffer[i].rpcNum = Invalid;
buffer[i].type = INVALID;
memset(buffer[i].buffer, 0, MAX_PAGE_SIZES);
}
finishedScanIndex = 0;
address_mask = 0xFFFFFFFF;
}

64
i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/protocol.cpp Normal file
View File

@@ -0,0 +1,64 @@
#include "protocol.hpp"
#include "gobotrpc.hpp"
#include <stdint.h>
uint32_t getRpcRequestCompleteMask(RPCNumber rpcNum) {
switch (rpcNum) {
case Get_Info: return 0b00000000000000000000000000000000;
case Home: return 0b00000000000000000000000000000011;
case Move_Step: return 0b00000000000000000000000000111111;
case Move_XY: return 0b00000000000000000000000000111111;
case Set_Padding: return 0b00000000000000001101111110111111;
case Release_Motors: return 0b00000000000000000000000001111111;
case Drop_Stone: return 0b00000000000000000000000000000000;
case Get_Stone_Status: return 0b00000000000000000000000000000000;
case Move_Z_Axis: return 0b00000000000000000000000000000001;
case Set_Vacum: return 0b00000000000000000000000000000001;
default: return 0b00000000000000000000000000000000;
}
}
uint32_t getRpcResponseCompleteMask(RPCNumber rpcNum) {
switch (rpcNum) {
case Get_Info: return 0b00001111111111111111111111111111;
case Home: return 0b00000000000000000000000000111111;
case Move_Step: return 0b00000000000000000000000000000000;
case Move_XY: return 0b00000000000000000000000000000000;
case Set_Padding: return 0b00000000000000000000000000000000;
case Release_Motors: return 0b00000000000000000000000000000000;
case Drop_Stone: return 0b00000000000000000000000000000000;
case Get_Stone_Status: return 0b00000000000000000000000000000000;
case Move_Z_Axis: return 0b00000000000000000000000000000000;
case Set_Vacum: return 0b00000000000000000000000000000000;
default: return 0b00000000000000000000000000000000;
}
}
uint32_t ERROR_MASK = 0b00000000000000000000000000000000;
FrameSubmitionResult getPackageStatus(RPC_RX_PackageSlot * package) {
uint32_t mask;
switch (package->type) {
case REQUEST:
mask = getRpcRequestCompleteMask(package->rpcNum);
break;
case RESPONSE_RPC:
mask = getRpcResponseCompleteMask(package->rpcNum);
break;
case ERROR:
mask = ERROR_MASK;
break;
default:
return ERROR_INVALID_RPC_TYPE;
};
uint32_t masked_bits = package->used_bit_masked & mask;
if(masked_bits == mask) {
return COMPLEATE;
}
return PENDING;
}

127
i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/rx.cpp Normal file
View File

@@ -0,0 +1,127 @@
#include "gobotrpc.hpp"
#include "protocol.hpp"
#include <string.h>
RPCHeader parseRPCHeader(char data)
{
RPCHeader header;
header.rpcNum = (RPCNumber)(data & 0b00001111);
header.type = (RPCType)((data & 0b00110000) >> 4);
header.segment = (data & 0b11000000) >> 6;
return header;
}
FrameSubmitionResult GobotRPCParser::submit_frame(char *data, size_t length, uint32_t addr, uint32_t timestamp)
{
RPCHeader header = parseRPCHeader(data[0]);
if (header.type == INVALID) {
return ERROR_INVALID_RPC_TYPE;
}
if (header.rpcNum > RPC_NUMBER_MAX) {
return ERROR_INVALID_RPC_NUMBER;
}
int fitting_slot;
int empty_slot;
find_fitting_slot(header, addr, &fitting_slot, &empty_slot);
int slotNum;
// Check if packageSlot is complete
if (fitting_slot != -1) {
slotNum = fitting_slot;
}
else if (empty_slot != -1) {
slotNum = empty_slot;
this->buffer[slotNum].is_in_use = true;
this->buffer[slotNum].is_complete = false;
this->buffer[slotNum].sender_address = addr;
this->buffer[slotNum].rpcNum = header.rpcNum;
this->buffer[slotNum].type = header.type;
}
else {
return ERROR_NO_EMPTY_OR_FITTING_SLOT;
}
insertFrameInPackageSlot(&this->buffer[slotNum], data, header.segment, length);
this->buffer[slotNum].timestamp = timestamp;
volatile FrameSubmitionResult res = getPackageStatus(&(this->buffer[slotNum]));
if (res == COMPLEATE) {
this->buffer[slotNum].is_complete = true;
}
return res;
}
void GobotRPCParser::insertFrameInPackageSlot(RPC_RX_PackageSlot *packageSlot, char *data, size_t segment, size_t length)
{
size_t offset = segment * 7;
for (int i = 0; i < length - 1; i++) {
packageSlot->buffer[offset + i] = data[i + 1];
packageSlot->used_bit_masked |= 1 << (offset + i);
}
}
void GobotRPCParser::find_fitting_slot(RPCHeader header, uint32_t addr, int *fitting_slot, int *empty_slot)
{
uint32_t masked_addr = addr & this->address_mask;
*fitting_slot = -1;
*empty_slot = -1;
for (int i = 0; i < NUM_SLOTS; i++) {
if (this->buffer[i].is_complete) {
continue;
}
uint32_t masked_slot_addr = this->buffer[i].sender_address & this->address_mask;
if (masked_slot_addr == masked_addr && header.rpcNum == this->buffer[i].rpcNum) {
*fitting_slot = i;
return;
}
if (*empty_slot == -1 && !buffer[i].is_in_use) {
*empty_slot = i;
}
}
}
int GobotRPCParser::getFinishedIndexPackages() {
for (int i = 0; i < NUM_SLOTS; i++) {
size_t index = (i + this->finishedScanIndex) % NUM_SLOTS;
if (this->buffer[i].is_complete) {
this->finishedScanIndex = (index + 1) % NUM_SLOTS;
return index;
}
}
return -1;
}
void GobotRPCParser::retrivePackage(RPCPackage *dest, int index) {
memcpy(dest->buffer, buffer[index].buffer, MAX_PAGE_SIZES);
dest->addr = buffer[index].sender_address;
dest->rpcNum = buffer[index].rpcNum;
dest->type = buffer[index].type;
freePackageSlot(index);
}
void GobotRPCParser::freePackageSlot(size_t index) {
this->buffer[index].timestamp = 0;
this->buffer[index].is_complete = false;
this->buffer[index].is_in_use = false;
this->buffer[index].used_bit_masked = 0;
this->buffer[index].sender_address = 0;
this->buffer[index].rpcNum = Invalid;
this->buffer[index].type = INVALID;
}

55
i2c-hub/firmware/i2c-hub-firmware/src/main.cpp
View File

@@ -7,8 +7,53 @@
#define LED_PIN 25
#include "gobotrpc/include/gobotrpc.hpp"
void TaskFn(void * args) {
printf("\n\n\n\n");
GobotRPCParser rpcRXParser;
char data0[] = "\x04\x01\x02\x03\x04\x05\x06";
char data1[] = "\x44\x11\x12\x13\x14\x15\x16";
char data3[] = "\x11\xaa\xbb\xcc\xdd\xee\xff";
char data2[] = "\x84\xaa\xbb";
rpcRXParser.submit_frame(data0, 7, 0x42, xTaskGetTickCount());
rpcRXParser.print_out_slots();
printf("\n");
rpcRXParser.submit_frame(data3, 7, 0x43, xTaskGetTickCount());
rpcRXParser.print_out_slots();
printf("\n");
rpcRXParser.submit_frame(data1, 7, 0x42, xTaskGetTickCount());
rpcRXParser.print_out_slots();
printf("\n");
vTaskDelay(10 / portTICK_PERIOD_MS);
rpcRXParser.submit_frame(data3, 7, 0x43, xTaskGetTickCount());
rpcRXParser.print_out_slots();
printf("\n");
rpcRXParser.submit_frame(data2, 3, 0x42, xTaskGetTickCount());
rpcRXParser.print_out_slots();
printf("\n");
size_t index = rpcRXParser.getFinishedIndexPackages();
printf("Index: %d\n", index);
RPCPackage package;
rpcRXParser.retrivePackage(&package, index);
for(int i=0; i<MAX_PAGE_SIZES; i++) {
printf("%02x", package.buffer[i]);
}
printf("\n");
rpcRXParser.print_out_slots();
while (true) {
gpio_put(LED_PIN, false);
vTaskDelay(500 / portTICK_PERIOD_MS);
@@ -18,19 +63,19 @@ void TaskFn(void * args) {
}
int main()
{
int main() {
stdio_init_all();
gpio_init(LED_PIN);
gpio_set_dir(LED_PIN, true);
stdio_init_all();
printf("HelloWorld!\n");
TaskHandle_t taskHandle;
BaseType_t res = xTaskCreate(TaskFn, "UART Task", 128, NULL, 1, &taskHandle);
printf("%d\n");
xTaskCreate(TaskFn, "UART Task", 1024, NULL, 1, &taskHandle);
vTaskStartScheduler();