Implement I2C

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/cmake/CMakeLists.txt

@@ -1,13 +1,20 @@
 add_library(GobotRPC STATIC
     ${CMAKE_CURRENT_LIST_DIR}/../ctrl_interface/base.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../ctrl_interface/ci_instruction.cpp
     ${CMAKE_CURRENT_LIST_DIR}/../ctrl_interface/uart_rp2040/hardware_rp2040_uart.cpp
     ${CMAKE_CURRENT_LIST_DIR}/../ctrl_interface/uart_rp2040/rx.cpp
     ${CMAKE_CURRENT_LIST_DIR}/../crc16.cpp
     ${CMAKE_CURRENT_LIST_DIR}/../protocol_base.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../transmission_interface/rp2040_i2c_init.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../transmission_interface/rp2040_i2c_tx.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../transmission_interface/rp2040_i2c_rx.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../transmission_interface/rp2040_i2c_int.cpp
+    ${CMAKE_CURRENT_LIST_DIR}/../transmission_interface/rp2040_i2c_ci_instructions.cpp
 )
 
 target_include_directories(GobotRPC PUBLIC 
     ${CMAKE_CURRENT_LIST_DIR}/../include
+    ${CMAKE_CURRENT_LIST_DIR}/../include/ti
     ${CMAKE_CURRENT_LIST_DIR}/../../..
 )
 
@@ -16,4 +23,5 @@ target_link_libraries(GobotRPC
     pico_stdlib
     hardware_uart
     hardware_irq
+    hardware_i2c
 )

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/base.cpp

@@ -6,11 +6,14 @@
 #include "task.h"
 
 
-GobotRPC_CI::GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core) {
+GobotRPC_CI::GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue) {
+    this->ciInstructionQueue = ciInstructionQueue;
+    
     this->hardware = hardware;
     this->hardware->registerCB_RxData(GobotRPC_CI_rxData_cb, this);
 
     xTaskCreateAffinitySet(GobotRPC_CI_heartBeatTaskFn, "Heartbeat Task", 2048, this, 2, core, &this->heartBeatTaskHandle);
+    xTaskCreateAffinitySet(GobotRPC_CI_txCIInstructionTaskFn, "Tx CI Instruction Task", 2048, this, 3, core, &this->txCIInstructionTaskHandle);
 }
 
 // Rx Side
@@ -59,9 +62,23 @@ void GobotRPC_CI_heartBeatTaskFn(void *args) {
 }
 
 void GobotRPC_CI::heartBeartTaskFn() {
-    char heartBeatPacket[] = {0xff, 0x02};
+    char heartBeatPacket[] = {HEARTBEAT, 0x02};
     while(1) {
         this->hardware->send(heartBeatPacket, 2);
         vTaskDelay(GOBOTRPC_HEARTBEAT_INTERVAL / portTICK_PERIOD_MS);
     }
+}
+
+void GobotRPC_CI::send_ErrorTransmission(bool rx, uint64_t addr) {
+    char errorPacket[7];
+
+    errorPacket[0] = ERROR_TRANMISSION;
+    errorPacket[1] = 7;
+    errorPacket[2] = rx ? 0x01 : 0x00;
+    errorPacket[3] = (addr >> 24) & 0xff;
+    errorPacket[4] = (addr >> 16) & 0xff;
+    errorPacket[5] = (addr >> 8) & 0xff;
+    errorPacket[6] = addr & 0xff;
+
+    this->hardware->send(errorPacket, 7);
 }

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/ci_instruction.cpp

@@ -0,0 +1,27 @@
+#include "ctrl_interface.hpp"
+
+#include "ctrl_interface_instructions.hpp"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+
+void GobotRPC_CI_txCIInstructionTaskFn(void *args) {
+    GobotRPC_CI *gobotRPC = (GobotRPC_CI *)args;
+    gobotRPC->txCIInstructionTask();
+}
+
+void GobotRPC_CI::txCIInstructionTask() {
+    while(1) {
+        CI_Instruction_Transport ciInstruction;
+        xQueueReceive(ciInstructionQueue, &ciInstruction, portMAX_DELAY);
+
+        switch (ciInstruction.type) {
+        case CI_INSTRUCTION_SEND_TRANMISSION_ERROR:
+            uint32_t addr = ciInstruction.data[1] | (ciInstruction.data[2] << 8) | (ciInstruction.data[3] << 16) | (ciInstruction.data[4] << 24);
+            uint8_t rx = ciInstruction.data[0];
+            send_ErrorTransmission(rx, addr);
+            break;
+        }
+    }
+}

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/ctrl_interface/uart_rp2040/hardware_rp2040_uart.cpp

@@ -18,7 +18,7 @@ GobotRPC_CI_Hardware_RP2040_UART::GobotRPC_CI_Hardware_RP2040_UART(uart_inst_t *
 
     initRxTaskAndQueues();
 
-    txMutex = xSemaphoreCreateMutex();
+    uartTxMutex = xSemaphoreCreateMutex();
 
     this->uart = uart;
     uart_init(this->uart, baudrate);
@@ -47,8 +47,8 @@ void GobotRPC_CI_Hardware_RP2040_UART::registerCB_RxData(callback_rxData cb, voi
 
 // Sending data
 void GobotRPC_CI_Hardware_RP2040_UART::send(char *data, size_t len) {
-    xSemaphoreTake(txMutex, portMAX_DELAY);
+    xSemaphoreTake(uartTxMutex, portMAX_DELAY);
     uart_write_blocking(this->uart, (uint8_t *)(data), len);
     uart_tx_wait_blocking(this->uart);
-    xSemaphoreGive(txMutex);
+    xSemaphoreGive(uartTxMutex);
 }

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ctrl_interface.hpp

@@ -7,6 +7,8 @@ enum GobotRPC_CI_CMD {
     RX_CI_PACKET = 0x02,
     PERFORM_SCAN_CI_PACKET = 0x03,
     SCAN_RESULT_CI_PACKET = 0x04,
+    ERROR_TRANMISSION = 0xfd,
+    HEARTBEAT = 0xff
 };
 
 #define CI_TX_PACKAGE_DATA_OFFSET 6
@@ -18,6 +20,7 @@ enum GobotRPC_CI_CMD {
 typedef void (*callback_TxPacket)(void * args, char *data, size_t len, uint32_t addr);
 void GobotRPC_CI_rxData_cb(void * args, char *data, size_t len);
 void GobotRPC_CI_heartBeatTaskFn(void *args);
+void GobotRPC_CI_txCIInstructionTaskFn(void *args);
 
 class GobotRPC_CI {
 private:
@@ -27,9 +30,11 @@ private:
     void * cb_TxPacket_args;
 
     TaskHandle_t heartBeatTaskHandle;
+    TaskHandle_t txCIInstructionTaskHandle;
 
+    QueueHandle_t ciInstructionQueue;
 public:
-    GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core);
+    GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue);
 
     void registerCB_TxPacket(callback_TxPacket cb, void *args);
     void send_RxPacket(char *data, size_t len, uint32_t addr);
@@ -37,4 +42,8 @@ public:
     void onRxData(char *data, size_t len);
 
     void heartBeartTaskFn();
+
+    // CI Instruction Stuff
+    void txCIInstructionTask();
+    void send_ErrorTransmission(bool rx, uint64_t addr);
 };

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ctrl_interface_hardware.hpp

@@ -53,7 +53,7 @@ private:
     callback_rxData cb_rxData;
     void * cb_rxData_args;
 
-    SemaphoreHandle_t txMutex;
+    SemaphoreHandle_t uartTxMutex;
 
     UBaseType_t core;
 

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ctrl_interface_instructions.hpp

@@ -0,0 +1,14 @@
+enum CI_Instruction_Type {
+    CI_INSTRUCTION_SEND_TRANMISSION_ERROR
+};
+
+struct CI_Instruction_Transport {
+    CI_Instruction_Type type;
+    uint32_t addr;
+    char data[16];
+};
+
+struct CI_Instruction_Data_SEND_TRANMISSION_ERROR {
+    bool rx;
+    uint32_t addr;
+};

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/main.hpp

@@ -0,0 +1,14 @@
+#pragma once
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+
+struct AppData {
+    QueueHandle_t txQueue;
+    QueueHandle_t rxQueue;
+    QueueHandle_t ciInstructionQueue;
+};
+
+void main_core2(void * pvParameters);

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/pinConfig.hpp

@@ -3,4 +3,17 @@
 #define GOBOTRPC_CI_UART_RX 0
 #define GOBOTRPC_CI_UART_TX 1
 
-#define GOBOTRPC_HEARTBEAT_INTERVAL 3000
+#define GOBOTRPC_TI_I2C_SDA 4
+#define GOBOTRPC_TI_I2C_SCL 5
+#define GOBOTRPC_TI_CLOCK_SPEED 10000
+
+#define GOBOTRPC_TI_INT_PIN_START 10
+#define GOBOTRPC_TI_INT_NUM 4
+#define GOBOTRPC_TI_COMBINED_INT_PIN 2
+
+#define GOBOTRPC_TI_EXTERNAL_PULLUP 0
+
+#define GOBOTRPC_HEARTBEAT_INTERVAL 3000
+
+#define UART_CORE_MASK 0b01
+#define I2C_CORE_MASK  0b10

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ti/transmission_interface.hpp

@@ -0,0 +1,83 @@
+#pragma once
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "semphr.h"
+
+#include <stdint.h>
+#include <strings.h>
+
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+
+#include "pinConfig.hpp"
+
+#include "ctrl_interface_instructions.hpp"
+
+struct GoRPCPackage_Transport {
+    uint32_t addr;
+    uint32_t len;
+    char data[256];
+};
+
+typedef void (*callback_pullPackage)(GoRPCPackage_Transport *src, void *args);
+typedef void (*callback_pushPackage)(GoRPCPackage_Transport *dest, void *args);
+typedef void (*callback_pushCIInstruction)(CI_Instruction_Transport *src, void *args);
+
+class GobotRPC_TI_Hardware {
+public:
+    virtual void registerPullPackageCB(callback_pullPackage cb, void *args) = 0;
+    virtual void registerPushPackageCB(callback_pushPackage cb, void *args) = 0;
+    virtual void registerPushCIInstructionCB(callback_pushCIInstruction cb, void *args) = 0;
+    virtual void setAddrMap(uint32_t addr, int intNum) = 0;
+};
+
+void i2cRxTaskFn(void * args);
+void i2cTxTaskFn(void * args);
+
+void intPin_ISR(uint gpio, uint32_t events);
+
+class GobotRPC_TI_Hardware_RP2040_I2C : public GobotRPC_TI_Hardware {
+private:
+    callback_pullPackage pullPackageCB;
+    void *pullPackageCBArgs;
+
+    callback_pushPackage pushPackageCB;
+    void *pushPackageCBArgs;
+
+    callback_pushCIInstruction pushCIInstructionCB;
+    void *pushCIInstructionCBArgs;
+
+    SemaphoreHandle_t i2cMutex;
+    SemaphoreHandle_t i2cRXSemaphore;
+
+    TaskHandle_t i2cRxTaskHandle;
+    TaskHandle_t i2cTxTaskHandle;
+    UBaseType_t core;
+
+    i2c_inst_t * i2c;
+
+    uint32_t intAddressMap[GOBOTRPC_TI_INT_NUM];
+
+    void initTasks();
+    bool readI2C(GoRPCPackage_Transport * pkg, uint32_t addr);
+
+public:
+    GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c);
+
+    void registerPullPackageCB(callback_pullPackage cb, void *args);
+    void registerPushPackageCB(callback_pushPackage cb, void *args);
+    void registerPushCIInstructionCB(callback_pushCIInstruction cb, void *args);
+    
+    void setAddrMap(uint32_t addr, int intNum);
+
+    void i2cRxTask();
+    void i2cTxTask();
+    void intPinISR(BaseType_t * xHigherPriorityTaskWoken);
+    uint32_t readIntPins();
+
+    void raiseTranmissionError(bool rx, uint32_t addr);
+};
+
+
+extern GobotRPC_TI_Hardware_RP2040_I2C * g_GobotRPC_TI_Hardware_RP2040_I2C;

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_ci_instructions.cpp

@@ -0,0 +1,16 @@
+#include "transmission_interface.hpp"
+
+#include "FreeRTOS.h"
+#include "queue.h"
+
+void GobotRPC_TI_Hardware_RP2040_I2C::raiseTranmissionError(bool rx, uint32_t addr) {
+    CI_Instruction_Transport ciInstruction;
+    ciInstruction.type = CI_INSTRUCTION_SEND_TRANMISSION_ERROR;
+    ciInstruction.data[0] = rx;
+    ciInstruction.data[1] = addr & 0xff;
+    ciInstruction.data[2] = (addr >> 8) & 0xff;
+    ciInstruction.data[3] = (addr >> 16) & 0xff;
+    ciInstruction.data[4] = (addr >> 24) & 0xff;
+    
+    this->pushCIInstructionCB(&ciInstruction, pushCIInstructionCBArgs);
+}

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_init.cpp

@@ -0,0 +1,72 @@
+#include "transmission_interface.hpp"
+#include "hardware/i2c.h"
+
+#include "pinConfig.hpp"
+
+GobotRPC_TI_Hardware_RP2040_I2C * g_GobotRPC_TI_Hardware_RP2040_I2C;
+
+GobotRPC_TI_Hardware_RP2040_I2C::GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c) {
+    this->core = core;
+    this->i2c = i2c0;
+
+    g_GobotRPC_TI_Hardware_RP2040_I2C = this;
+
+    i2c_init(i2c, GOBOTRPC_TI_CLOCK_SPEED);
+    gpio_set_function(GOBOTRPC_TI_I2C_SDA, GPIO_FUNC_I2C);
+    gpio_set_function(GOBOTRPC_TI_I2C_SCL, GPIO_FUNC_I2C);
+
+    if(!GOBOTRPC_TI_EXTERNAL_PULLUP) {
+        gpio_pull_up(GOBOTRPC_TI_I2C_SDA);
+        gpio_pull_up(GOBOTRPC_TI_I2C_SCL);
+    }
+
+    for(int i=0; i<GOBOTRPC_TI_INT_NUM; i++) {
+        intAddressMap[i] = 0xffffffff;
+        gpio_init(GOBOTRPC_TI_INT_PIN_START + i);
+        gpio_set_dir(GOBOTRPC_TI_INT_PIN_START< + i, GPIO_IN);
+        gpio_pull_down(GOBOTRPC_TI_INT_PIN_START + i);
+    }
+
+    initTasks();
+
+    gpio_init(GOBOTRPC_TI_COMBINED_INT_PIN);
+    gpio_set_dir(GOBOTRPC_TI_COMBINED_INT_PIN, GPIO_IN);
+    gpio_pull_down(GOBOTRPC_TI_COMBINED_INT_PIN);
+    gpio_set_irq_enabled_with_callback(GOBOTRPC_TI_COMBINED_INT_PIN, GPIO_IRQ_EDGE_RISE , true, &intPin_ISR);
+
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::initTasks() {
+    i2cMutex = xSemaphoreCreateMutex();
+    i2cRXSemaphore = xSemaphoreCreateBinary();
+
+    pushPackageCB = NULL;
+    pullPackageCB = NULL;
+    pushPackageCBArgs = NULL;
+    pullPackageCBArgs = NULL;
+
+    volatile int a = xPortGetFreeHeapSize();
+
+    xTaskCreateAffinitySet(i2cRxTaskFn, "i2c Rx Task", 4096, this, 3, core, &i2cRxTaskHandle);
+    xTaskCreateAffinitySet(i2cTxTaskFn, "i2c Tx Task", 4096, this, 3, core, &i2cTxTaskHandle);
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::registerPullPackageCB(callback_pullPackage cb, void *args) {
+    pullPackageCB = cb;
+    pullPackageCBArgs = args;
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::registerPushPackageCB(callback_pushPackage cb, void *args) {
+    pushPackageCB = cb;
+    pushPackageCBArgs = args;
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::registerPushCIInstructionCB(callback_pushCIInstruction cb, void *args) {
+    pushCIInstructionCB = cb;
+    pushCIInstructionCBArgs = args;
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::setAddrMap(uint32_t addr, int intNum) {
+    intAddressMap[intNum] = addr;
+}
+

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_int.cpp

@@ -0,0 +1,34 @@
+#include "transmission_interface.hpp"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+
+uint32_t GobotRPC_TI_Hardware_RP2040_I2C::readIntPins() {
+    uint32_t intPinsStates = 0;
+
+    for(int i=0; i<GOBOTRPC_TI_INT_NUM; i++) {
+        intPinsStates |= gpio_get(GOBOTRPC_TI_INT_PIN_START + i) << i;
+    }
+    return intPinsStates;
+}
+
+void intPin_ISR(uint gpio, uint32_t events) {
+    GobotRPC_TI_Hardware_RP2040_I2C * hw = g_GobotRPC_TI_Hardware_RP2040_I2C;
+
+    uint32_t intPinsStates = hw->readIntPins();
+
+    if(intPinsStates == 0) {
+        return;
+    }
+
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+    hw->intPinISR(&xHigherPriorityTaskWoken);
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::intPinISR(BaseType_t * xHigherPriorityTaskWoken) {
+    xSemaphoreGiveFromISR(i2cRXSemaphore, xHigherPriorityTaskWoken);
+}
+
+

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_rx.cpp

@@ -0,0 +1,59 @@
+#include "transmission_interface.hpp"
+
+void i2cRxTaskFn(void * args) {
+    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)args;
+    hw->i2cRxTask();
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::i2cRxTask() {
+    while (1) {
+        xSemaphoreTake(i2cRXSemaphore, portMAX_DELAY);
+
+        uint32_t done_mask = 0xffffffff;
+        uint32_t pinStates = readIntPins();
+
+        do {
+            for(int i=0; i<GOBOTRPC_TI_INT_NUM; i++) {
+                if((pinStates & done_mask) & (1 << i)) {
+                    done_mask &= ~(1 << i);
+            
+                    uint32_t addr = intAddressMap[i];
+
+                    GoRPCPackage_Transport pkg;
+
+                    xSemaphoreTake(i2cMutex, portMAX_DELAY);
+                    bool read_res = readI2C(&pkg, addr);
+                    xSemaphoreGive(i2cMutex);
+
+                    if(read_res) {
+                        if(pushPackageCB != NULL)
+                            pushPackageCB(&pkg, pushPackageCBArgs);
+                    }
+            
+                    break;
+                }
+            }
+            
+
+            pinStates = readIntPins();
+        } while(pinStates & done_mask);
+    }
+}
+
+bool GobotRPC_TI_Hardware_RP2040_I2C::readI2C(GoRPCPackage_Transport * pkg, uint32_t addr) {
+    
+    uint res = i2c_read_burst_blocking(i2c, addr, (uint8_t *)pkg->data, 2);
+    
+    if(res == PICO_ERROR_GENERIC)
+        return false; // I2C error
+    
+    size_t len = pkg->data[1];
+    res = i2c_read_burst_blocking(i2c, addr, ((uint8_t *)pkg->data) + 2, len);
+
+    if(res == PICO_ERROR_GENERIC)
+        return false; // I2C error
+    
+    pkg->addr = addr;
+
+    return true;
+}

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/transmission_interface/rp2040_i2c_tx.cpp

@@ -0,0 +1,27 @@
+#include "transmission_interface.hpp"
+
+void i2cTxTaskFn(void * args) {
+    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)args;
+    hw->i2cTxTask();
+}
+
+void GobotRPC_TI_Hardware_RP2040_I2C::i2cTxTask() {
+    while (1) {
+        GoRPCPackage_Transport pkg;
+
+        if(pullPackageCB == NULL) {
+            vTaskDelay(pdMS_TO_TICKS(100));
+            continue;
+        }
+        
+        pullPackageCB(&pkg, pullPackageCBArgs);
+
+        xSemaphoreTake(i2cMutex, portMAX_DELAY);
+        unsigned int res = i2c_write_burst_blocking(i2c, pkg.addr & 0x7f, (uint8_t *)(&pkg.data), pkg.len);
+        xSemaphoreGive(i2cMutex);
+
+        if(res == PICO_ERROR_GENERIC) {
+            raiseTranmissionError(false, pkg.addr);
+        }
+    }
+}

i2c-hub/firmware/i2c-hub-firmware/src/main.cpp

@@ -1,55 +0,0 @@
-#include <stdio.h>
-#include "pico/stdlib.h"
-
-#include "ctrl_interface.hpp"
-#include "ctrl_interface_hardware.hpp"
-
-#include "FreeRTOSConfig.h"
-#include "FreeRTOS.h"
-#include "task.h"
-
-#include "protocol.hpp"
-
-#define LED_PIN 25
-
-#define UART_CORE_MASK 0b01
-#define I2C_CORE_MASK  0b10
-
-void vTaskMain(void * pvParameters) {
-    while(1) {
-        gpio_put(LED_PIN, 1);
-        vTaskDelay(pdMS_TO_TICKS(500));
-        gpio_put(LED_PIN, 0);
-        vTaskDelay(pdMS_TO_TICKS(500));
-    }
-}
-
-void onTxPacket(void * args, char *data, size_t len, uint32_t addr) {
-    GobotRPC_CI * gobotRPC_ci = (GobotRPC_CI *)args;
-
-    char bufferPackage[CI_RX_PACKAGE_SIZE(CALC_SIZE_GobotRPC_PACKAGE(0))];
-    char * gorpcPackageOffset = bufferPackage + CI_RX_PACKAGE_DATA_OFFSET;
-
-    assembleGobotRPCHeader(gorpcPackageOffset, GobotRPCNumber::VACUM, GobotRPCTypes::RESPONSE, 0);
-    assembleCRC(gorpcPackageOffset, 0);
-
-    gobotRPC_ci->send_RxPacket(bufferPackage, CALC_SIZE_GobotRPC_PACKAGE(0), addr);
-}
-
-int main() {
-    gpio_init(LED_PIN);
-    gpio_set_dir(LED_PIN, true);
-
-    GobotRPC_CI_Hardware_RP2040_UART gobotrpc_ci_hardware(uart0, 115200, 0b01);
-    GobotRPC_CI gobotRPC_ci(&gobotrpc_ci_hardware, 0b01);
-    
-    gobotRPC_ci.registerCB_TxPacket(onTxPacket, &gobotRPC_ci);
-
-    TaskHandle_t taskHandle;
-    xTaskCreateAffinitySet(vTaskMain, "Main Task", 2048, &gobotRPC_ci, 1, UART_CORE_MASK, &taskHandle);
-    vTaskCoreAffinitySet(taskHandle, 0);
-
-    vTaskStartScheduler();
-
-    while(1) {}
-}

i2c-hub/firmware/i2c-hub-firmware/src/main_core0.cpp

@@ -0,0 +1,66 @@
+#include <stdio.h>
+#include <string.h>
+#include "pico/stdlib.h"
+
+#include "ctrl_interface.hpp"
+#include "ctrl_interface_hardware.hpp"
+#include "transmission_interface.hpp"
+
+#include "main.hpp"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "message_buffer.h"
+
+#include "protocol.hpp"
+
+#define LED_PIN 25
+
+AppData appData;
+
+void vTaskMain(void * pvParameters) {
+    while(1) {
+        gpio_put(LED_PIN, 1);
+        vTaskDelay(pdMS_TO_TICKS(500));
+        gpio_put(LED_PIN, 0);
+        vTaskDelay(pdMS_TO_TICKS(500));
+    }
+}
+
+void onTxPacket(void * args, char *data, size_t len, uint32_t addr) {
+    GobotRPC_CI * gobotRPC_ci = (GobotRPC_CI *)args;
+
+    GoRPCPackage_Transport pkg;
+    memcpy(pkg.data, data, len);
+    pkg.len = len;
+    pkg.addr = addr;
+    
+    xQueueSend(appData.txQueue, &pkg, portMAX_DELAY);
+
+    //gobotRPC_ci->send_RxPacket(pkg.data, CALC_SIZE_GobotRPC_PACKAGE(0), addr);
+}
+
+int main() {
+    gpio_init(LED_PIN);
+    gpio_set_dir(LED_PIN, true);
+
+    appData.txQueue = xQueueCreate(5, sizeof(GoRPCPackage_Transport));
+    appData.rxQueue = xQueueCreate(5, sizeof(GoRPCPackage_Transport));
+    appData.ciInstructionQueue = xQueueCreate(5, sizeof(CI_Instruction_Transport));
+
+    GobotRPC_CI_Hardware_RP2040_UART gobotrpc_ci_hardware(uart0, 115200, UART_CORE_MASK);
+
+    GobotRPC_CI gobotRPC_ci(&gobotrpc_ci_hardware, 0b01, appData.ciInstructionQueue);
+    
+    gobotRPC_ci.registerCB_TxPacket(onTxPacket, &gobotRPC_ci);
+
+    TaskHandle_t taskHandleCore0;
+    TaskHandle_t taskHandleCore1;
+    xTaskCreateAffinitySet(vTaskMain, "Main Task Core 0", 2048, &gobotRPC_ci, 1, UART_CORE_MASK, &taskHandleCore0);
+    xTaskCreateAffinitySet(main_core2, "Main Task Core 1", 2048, &gobotRPC_ci, 1, I2C_CORE_MASK, &taskHandleCore1);
+
+    vTaskStartScheduler();
+
+    while(1) {}
+}

i2c-hub/firmware/i2c-hub-firmware/src/main_core1.cpp

@@ -0,0 +1,39 @@
+#include "main.hpp"
+#include "transmission_interface.hpp"
+
+#include "FreeRTOSConfig.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+
+#include "pinConfig.hpp"
+
+extern AppData appData;
+
+void pullPackageCB(GoRPCPackage_Transport * dest, void *args) {
+    QueueHandle_t queue = appData.txQueue;
+    xQueueReceive(queue, dest, portMAX_DELAY);
+}
+
+void pushPackage(GoRPCPackage_Transport * src, void *args) {
+    QueueHandle_t queue = appData.rxQueue;
+    xQueueSend(queue, src, portMAX_DELAY);
+}
+
+void pushCIInstruction(CI_Instruction_Transport * src, void *args) {
+    QueueHandle_t queue = appData.ciInstructionQueue;
+    xQueueSend(queue, src, portMAX_DELAY);
+}
+
+void main_core2(void * pvParameters) {
+    AppData appData = *((AppData *)pvParameters);
+    GobotRPC_TI_Hardware_RP2040_I2C gobotrpc_ti_hardware(I2C_CORE_MASK, i2c0);
+
+    gobotrpc_ti_hardware.registerPullPackageCB(pullPackageCB, appData.txQueue);
+    gobotrpc_ti_hardware.registerPushPackageCB(pushPackage, appData.rxQueue);
+    gobotrpc_ti_hardware.registerPushCIInstructionCB(pushCIInstruction, appData.ciInstructionQueue);
+
+    while(1) {
+        vTaskDelay(pdMS_TO_TICKS(10000));
+    }
+}