working debug

debug-app/include/driver-hook.h

@@ -26,9 +26,15 @@ public:
 
     static void DriverRequestListener(const struct zbus_channel *chan);
 
+    static void IrSensorCallback(uint8_t data);
+
+    static void  BleDataCallback(std::string addr, std::vector<uint8_t> data, int8_t rssi);
+
 private:
+#ifndef CONFIG_SOC_POSIX
     BleScanner *bleScanner;
     LedDriver *ledDriver;
     ServoDriver *servoDriver;
     IrDriver *irDriver;
+#endif
 };
\ No newline at end of file

debug-app/include/utilities.h

@@ -4,9 +4,6 @@
 #include <string>
 
 typedef enum DriverNumb{
-    SENSOR_1,
-    SENSOR_2,
-    SENSOR_3,
     BLUETOOTH,
     SERVO,
     LED,

debug-app/module/driver-hook/src/driver-hook.cpp

@@ -12,7 +12,7 @@
 #include "ir-driver.h"
 
 LOG_MODULE_REGISTER(Driver_Hook);
-ZBUS_CHAN_DECLARE(driverRequestChannel, driverResponseChannel);
+ZBUS_CHAN_DECLARE(driverRequestChannel, driverResponseChannel, bleScanChannel);
 
 static DriverHook *context;
 
@@ -20,12 +20,14 @@ ZBUS_LISTENER_DEFINE(driverHookListener, context->DriverRequestListener);
 
 DriverHook::DriverHook(){
     context = this;
+#ifndef CONFIG_SOC_POSIX
     bleScanner = new BleScanner();
     ledDriver = new LedDriver();
     servoDriver = new ServoDriver();
     irDriver = new IrDriver();
     irDriver->RegisterInterrupt(IrSensorCallback);
     bleScanner->RegisterCallback(BleDataCallback);
+#endif
 
     zbus_chan_add_obs(&driverRequestChannel, &driverHookListener, K_NO_WAIT);
 }
@@ -50,6 +52,11 @@ void DriverHook::IrSensorCallback(uint8_t data){
 
 void DriverHook::BleDataCallback(std::string addr, std::vector<uint8_t> data, int8_t rssi){
     LOG_HEXDUMP_INF(data.data(), data.size(), "BLE data:");
+    BleData_t sentMessage = {
+		.address = addr,
+		.data = data
+	};
+	zbus_chan_pub(&bleScanChannel, &sentMessage, K_MSEC(200));
 }
 
 void DriverHook::Init(){
@@ -77,7 +84,7 @@ void DriverHook::DriverRequestListener(const struct zbus_channel *chan){
         .number = msg->number,
         .type = msg->type,
     };
-
+#ifndef CONFIG_SOC_POSIX
     switch (msg->number)
     {
     case BLUETOOTH:
@@ -111,6 +118,40 @@ void DriverHook::DriverRequestListener(const struct zbus_channel *chan){
         LOG_ERR("Invalid Sensor %d", msg->number);
         break;
     }
+#else
+    static bool ledState;
+    static uint8_t pwmState;
+    switch (msg->number)
+    {
+    case BLUETOOTH:
+        if(msg->type == REQUEST_SET){
+            sentMessage.value.push_back(0);
+        }
+        break;
+
+    case LED:
+        if(msg->type == REQUEST_SET){
+            ledState = msg->value[0];
+        }
+        else if(msg->type == REQUEST_GET){
+            sentMessage.value.push_back(ledState);
+        }
+        break;
+
+    case SERVO:
+        if(msg->type == REQUEST_SET){
+            pwmState = msg->value[0];
+        }
+        else if(msg->type == REQUEST_GET){
+            sentMessage.value.push_back(pwmState);
+        }
+        break;
+    
+    default:
+        LOG_ERR("Invalid Sensor %d", msg->number);
+        break;
+    }
+#endif
 
     int status = zbus_chan_pub(&driverResponseChannel, &sentMessage, K_MSEC(200));
     LOG_INF("Status pub %d", status);

debug-app/module/handler/debug-handler/src/debug-handler.cpp

@@ -136,9 +136,10 @@ void DebugHandler::DebugThread(DebugHandler *context){
 		uint16_t driverLen = 0;
 		
 		if(isDriverAvailable){
-			driverData.insert(driverData.end(), &driverArray[0], &driverArray[driverLen]); 
+			driverData.insert(driverData.end(), &driverArray[2], &driverArray[driverLen]); 
 			DriverMessage_t sentMessage = {
-				.number = (DriverNumb_e)0, //temporary
+				.number = (DriverNumb_e)driverArray[0],
+				.type = (ExchangeType_e)driverArray[1],
 				.value = driverData
 			};
 			zbus_chan_pub(&driverResponseChannel, &sentMessage, K_MSEC(200));
@@ -164,9 +165,17 @@ void DebugHandler::DriverResponseListener(const struct zbus_channel *chan){
 	if (&driverResponseChannel == chan) {
 			driver = (DriverMessage_t *)zbus_chan_const_msg(chan); // Direct message access
 			// LOG_INF("From listener -> driver =%d, value 0=%d", driver->number, driver->value[0]);
-			// if(isDebugMode){
-			// 	send(client, p, len, 0);
-			// }
+			if(isDebugMode){
+				std::vector<uint8_t> dataSent;
+				uint16_t payloadSize = driver->value.size() + 2;
+				dataSent.push_back(DEBUG_DRIVER);
+				dataSent.push_back((payloadSize >> 8) & 0xff);
+				dataSent.push_back(payloadSize & 0xff);
+				dataSent.push_back(driver->number);
+				dataSent.push_back(driver->type);
+				copy(driver->value.begin(), driver->value.end(), back_inserter(dataSent));
+				context->SendPacket((uint8_t *)dataSent.data(), dataSent.size());
+			}
 	}
 }
 
@@ -205,18 +214,18 @@ void DebugHandler::BleListener(const struct zbus_channel *chan){
 
 int DebugHandler::SendPacket(uint8_t *buf, uint16_t size){
 #ifndef CONFIG_SOC_POSIX
-	int sentlength, recvLength;
-	recvLength = size;
+	int sentlength, initialLength;
+	initialLength = size;
 	do {
-		sentlength = send(client, buf, recvLength, 0);
+		sentlength = send(client, buf, initialLength, 0);
 		if (sentlength < 0) {
 			LOG_ERR("error: send: %d\n", errno);
 			break;
 		}
 		buf += sentlength;
-		recvLength-= sentlength;
-	} while (recvLength);
-	return recvLength;
+		initialLength-= sentlength;
+	} while (initialLength);
+	return initialLength;
 #else
 	return 0;
 #endif

debug-app/module/handler/main-handler/src/main-handler.cpp

@@ -95,6 +95,7 @@ void MainHandler::MainThread(MainHandler *context){
 		}
 
 		case SCANNING:
+			LOG_INF("Scanning");
 			configVector.number = BLUETOOTH;
 			configVector.type = REQUEST_SET;
 			configVector.value.push_back(1);
@@ -102,6 +103,7 @@ void MainHandler::MainThread(MainHandler *context){
 			break;
 		
 		case USER_1:
+			LOG_INF("User 1");
 			configVector.number = LED;
 			configVector.type = REQUEST_SET;
 			configVector.value.push_back(1);
@@ -114,6 +116,7 @@ void MainHandler::MainThread(MainHandler *context){
 			break;
 
 		case USER_2:
+			LOG_INF("User 2");
 			configVector.number = LED;
 			configVector.type = REQUEST_SET;
 			configVector.value.push_back(0);
@@ -123,12 +126,12 @@ void MainHandler::MainThread(MainHandler *context){
 			configVector.type = REQUEST_SET;
 			configVector.value.push_back(100);
 			context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+			break;
 
-
-			break;		
 		default:
 			break;
 		}
+		k_msleep(500);
 	}
 	
 }
@@ -149,7 +152,7 @@ void MainHandler::DriverResponseListener(const struct zbus_channel *chan){
 		}
 
 		if(driver->number == IR_DRIVER){
-			if(driver->value[0] == 0){
+			if(driver->value[0] == 0 && context->currentState == SCANNING){
 				context->currentState = STANDBY;
 			}
 			else if(driver->value[0] == 1){

debug-python/debug.py

@@ -31,13 +31,13 @@ class StateBp(gdb.Breakpoint):
         # SocketStart()
         state = GetState()
         if state >= 0:
-            print('set var currentState = ' + str(state))
-            gdb.execute('set var currentState = ' + str(state))
+            print('set var context->currentState = ' + str(state))
+            gdb.execute('set var context->currentState = ' + str(state))
         return False
 
 class DriverBp(gdb.Breakpoint):
     def __init__(self):
-        gdb.Breakpoint.__init__(self, "debug-handler.cpp:136")
+        gdb.Breakpoint.__init__(self, "debug-handler.cpp:138")
 
     def stop(self):
         bleData = GetBle()