Fixed Issue: emptyQueue in Hub was not filled correctly

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

@@ -17,6 +17,11 @@ GobotRPC_CI::GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, Que
     this->hardware = hardware;
     this->hardware->registerCB_RxData(GobotRPC_CI_rxData_cb, this);
 
+    this->cb_TxPacket = NULL;
+    this->cb_TxPacket_args = NULL;
+    this->cb_SetAddressMap = NULL;
+    this->cb_SetAddressMap_args = NULL;
+
     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);
 }
@@ -37,10 +42,20 @@ void GobotRPC_CI::onRxData(char *data, size_t len) {
                 this->cb_TxPacket(this->cb_TxPacket_args, data+6, len-6, addr);
             }
             break;
+
         case RESET_CI_PACKET:
-            // Register Reset Magic
-            softwareReset();
+            //softwareReset();
             break;
+
+        case SET_ADDR_PORT_MAP: {
+            if(this->cb_SetAddressMap != NULL) {
+                uint32_t addr = (data[1] << 24) | (data[2] << 16) | (data[3] << 8) | data[4];
+                uint8_t port = data[5];
+                this->cb_SetAddressMap(cb_SetAddressMap_args, addr, port);
+            }
+            break;
+        }
+
         default:
             break;
     }
@@ -51,6 +66,11 @@ void GobotRPC_CI::registerCB_TxPacket(callback_TxPacket cb, void *args) {
     this->cb_TxPacket_args = args;
 }
 
+void GobotRPC_CI::registerCB_SetAddress(callback_SetAddress cb, void *args) {
+    this->cb_SetAddressMap = cb;
+    this->cb_SetAddressMap_args = args;
+}
+
 void GobotRPC_CI::send_RxPacket(char *data, size_t len, uint32_t addr) {
     data[0] = RX_CI_PACKET;
     data[1] = len + CI_RX_PACKAGE_DATA_OFFSET;

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

@@ -116,6 +116,7 @@ void GobotRPC_CI_Hardware_RP2040_UART::rxProcessingTaskFn() {
             this->cb_rxData(this->cb_rxData_args, inputBuffer->data, inputBuffer->len);
         }
 
+        xQueueSend(emptyInputBuffersQueue, &inputBuffer, portMAX_DELAY);
         vTaskDelay(1000);
     }
 }  

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/ci/base.hpp

@@ -5,6 +5,7 @@
 enum GobotRPC_CI_CMD {
     TX_CI_PACKET = 0x01,
     RX_CI_PACKET = 0x02,
+    SET_ADDR_PORT_MAP = 0x03,
 
     SUCESS_TRANMISSION = 0xfc,
     ERROR_TRANMISSION = 0xfd,
@@ -21,6 +22,8 @@ enum GobotRPC_CI_CMD {
 #define CI_RX_PACKAGE_SIZE(data_len) (data_len + CI_RX_PACKAGE_DATA_OFFSET)
 
 typedef void (*callback_TxPacket)(void * args, char *data, size_t len, uint32_t addr);
+typedef void (*callback_SetAddress)(void * args, uint32_t addr, uint8_t port);
+
 void GobotRPC_CI_rxData_cb(void * args, char *data, size_t len);
 void GobotRPC_CI_heartBeatTaskFn(void *args);
 void GobotRPC_CI_txCIInstructionTaskFn(void *args);
@@ -32,6 +35,9 @@ private:
     callback_TxPacket cb_TxPacket;
     void * cb_TxPacket_args;
 
+    callback_SetAddress cb_SetAddressMap;
+    void * cb_SetAddressMap_args;
+
     TaskHandle_t heartBeatTaskHandle;
     TaskHandle_t txCIInstructionTaskHandle;
 
@@ -40,6 +46,8 @@ public:
     GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue);
 
     void registerCB_TxPacket(callback_TxPacket cb, void *args);
+    void registerCB_SetAddress(callback_SetAddress cb, void *args);
+
     void send_RxPacket(char *data, size_t len, uint32_t addr);
 
     void onRxData(char *data, size_t len);

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/include/util/protocol.hpp

@@ -4,7 +4,8 @@
 
 enum GobotRPCNumber {
     VACUM = 0x8,
-    RESET = 0xc
+    RESET = 0xc,
+    GET_INFO = 0xd
 };
 
 enum GobotRPCTypes {

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

@@ -9,6 +9,15 @@
 
 typedef void (*onPackageRxCallback)(void * args, char *data, uint16_t len, GobotRPCTypes type, GobotRPCNumber number);
 
+enum NODE_TYPE: uint8_t {
+    NODE_TYPE_VACUM = 0xa1
+};
+
+struct InfoData {
+    uint32_t addr;
+    NODE_TYPE type;
+};
+
 class GobotRPC_NI {
 private:
     onPackageRxCallback onPackageRx;
@@ -17,16 +26,18 @@ private:
     QueueHandle_t txQueue;
     QueueHandle_t rxQueue;
     TaskHandle_t rxTaskHandle;
+    
+    InfoData info;
 
     unsigned int core;
 
 public:
-    GobotRPC_NI(unsigned int core, QueueHandle_t txQueue, QueueHandle_t rxQueue);
+    GobotRPC_NI(unsigned int core, QueueHandle_t txQueue, QueueHandle_t rxQueue, InfoData info);
     
     void registerOnPackageRxCallback(onPackageRxCallback callback, void *context);
     void sendPackage(char *data, size_t len, GobotRPCTypes type, GobotRPCNumber number);
     
+    InfoData getInfo();
+
     void rxTask();
-
-
 };

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/node_interface/init.cpp

@@ -1,5 +1,6 @@
 #include "node_interface.hpp"
 
+#include "FreeRTOSConfig.h"
 #include "FreeRTOS.h"
 #include "task.h"
 #include <string.h>
@@ -9,8 +10,9 @@ static void rxTaskFn(void *pvParameters) {
     ni->rxTask();
 }
 
-GobotRPC_NI::GobotRPC_NI(unsigned int core, QueueHandle_t txQueue, QueueHandle_t rxQueue) {
+GobotRPC_NI::GobotRPC_NI(unsigned int core, QueueHandle_t txQueue, QueueHandle_t rxQueue, InfoData info) {
     this->core = core;
+    this->info = info;
 
     this->txQueue = txQueue;
     this->rxQueue = rxQueue;
@@ -65,3 +67,7 @@ void GobotRPC_NI::sendPackage(char *data, size_t len, GobotRPCTypes type, GobotR
     assembleCRC(package.data, len);
     xQueueSend(this->txQueue, &package, portMAX_DELAY);
 }
+
+InfoData GobotRPC_NI::getInfo() {
+    return info;
+}

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

@@ -1,6 +1,8 @@
 #include "transmission_interface.hpp"
 #include "hardware/i2c.h"
 
+#include <string.h>
+
 #include "pinConfig.hpp"
 
 GobotRPC_TI_Hardware_RP2040_I2C * g_GobotRPC_TI_Hardware_RP2040_I2C;

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

@@ -20,22 +20,24 @@ void GobotRPC_TI_Hardware_RP2040_I2C::i2cRxTask() {
             
                     uint32_t addr = intAddressMap[i];
 
+                    if(addr > 0xff)
+                        continue;
+
                     GoRPCPackage_Transport pkg;
 
                     xSemaphoreTake(i2cMutex, portMAX_DELAY);
                     bool read_res = readI2C(&pkg, addr);
                     xSemaphoreGive(i2cMutex);
 
-                    //if(read_res) {
+                    pkg.addr |= (i) << 8;
+
                     if(pushPackageCB != NULL)
                         pushPackageCB(&pkg, pushPackageCBArgs);
-                    //}
             
                     break;
                 }
             }
             
-
             pinStates = readIntPins();
         } while(pinStates & done_mask);
     }

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

@@ -32,6 +32,7 @@ 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;

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

@@ -15,7 +15,7 @@ void pullPackageCB(GoRPCPackage_Transport * dest, void *args) {
     xQueueReceive(queue, dest, portMAX_DELAY);
 }
 
-void pushPackage(GoRPCPackage_Transport * src, void *args) {
+void pushPackageCB(GoRPCPackage_Transport * src, void *args) {
     QueueHandle_t queue = appData.rxQueue;
     xQueueSend(queue, src, portMAX_DELAY);
 }
@@ -28,10 +28,10 @@ void pushCIInstruction(CI_Instruction_Transport * src, void *args) {
 void main_core2(void * pvParameters) {
     AppData appData = *((AppData *)pvParameters);
     GobotRPC_TI_Hardware_RP2040_I2C gobotrpc_ti_hardware(UART_CORE_MASK, i2c0);
-    gobotrpc_ti_hardware.setAddrMap(0x21, 0);
+    //gobotrpc_ti_hardware.setAddrMap(0x21, 0);
 
     gobotrpc_ti_hardware.registerPullPackageCB(pullPackageCB, appData.txQueue);
-    gobotrpc_ti_hardware.registerPushPackageCB(pushPackage, appData.rxQueue);
+    gobotrpc_ti_hardware.registerPushPackageCB(pushPackageCB, appData.rxQueue);
     gobotrpc_ti_hardware.registerPushCIInstructionCB(pushCIInstruction, appData.ciInstructionQueue);
 
     gobotrpc_ti_hardware.raiseInfoReset();