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;
@@ -18,15 +27,17 @@ private:
     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();

vacum-control/vacum-control-firmware/include/pinConfigNode.hpp

@@ -15,3 +15,5 @@
 
 #define INTERRUPTION_DURATION ((int)(2.5 * 1000))
 #define INTERRUPTION_DELAY 5 * 1000
+
+#define I2C_ADDR 0x21

vacum-control/vacum-control-firmware/src/main.cpp

@@ -37,6 +37,18 @@ void onRxPackage(void * args, char *data, uint16_t len, GobotRPCTypes type, Gobo
             break;
         }
 
+        case GobotRPCNumber::GET_INFO: {
+            InfoData info = appData->gobotrpc_ni->getInfo();
+            txBuffer[0] = (info.addr >> 24) & 0xFF;
+            txBuffer[1] = (info.addr >> 16) & 0xFF;
+            txBuffer[2] = (info.addr >> 8) & 0xFF;
+            txBuffer[3] = info.addr & 0xFF;
+            txBuffer[4] = info.type;
+
+            g_appData.gobotrpc_ni->sendPackage(txBuffer, 5, GobotRPCTypes::RESPONSE, GobotRPCNumber::GET_INFO);
+            break;
+        }
+
         case GobotRPCNumber::RESET: {
             softwareReset();
             break;
@@ -52,7 +64,7 @@ void core1_main(void *pvParameters) {
     AppData * appData = (AppData *) pvParameters;
     GobotRPC_NI_Hardware_RP2040_I2C gobotrpc_ni_hardware(
         appData->txQueue, appData->rxQueue, CORE_MASK_VACUM,
-        i2c0, 0x21, GORPC_SDA_PIN, GORPC_SCL_PIN, GORPC_INT_PIN
+        i2c0, I2C_ADDR, GORPC_SDA_PIN, GORPC_SCL_PIN, GORPC_INT_PIN
     );
 
     while (true) {
@@ -85,7 +97,9 @@ int main() {
     vacumControl.setState(false);
     g_appData.vacumControl = &vacumControl;
 
-    GobotRPC_NI gobotrpc_ni(CORE_MASK_VACUM, g_appData.txQueue, g_appData.rxQueue);
+    InfoData info = {.addr=I2C_ADDR, .type=NODE_TYPE_VACUM};
+
+    GobotRPC_NI gobotrpc_ni(CORE_MASK_VACUM, g_appData.txQueue, g_appData.rxQueue, info);
     gobotrpc_ni.registerOnPackageRxCallback(onRxPackage, &g_appData);
 
     g_appData.gobotrpc_ni = &gobotrpc_ni;