fix main app and debug feature

debug-app/app/src/main.cpp

@@ -54,10 +54,6 @@ ZBUS_CHAN_DEFINE(stateChannel,       /* Name */
 
 int main()
 {
-#if !defined(CONFIG_SOC_POSIX)
-	WifiController wifiController;
-	// BleScanner bleScanner;
-#endif
 	DebugHandler debugHanddler;
 	MainHandler mainHandler;
 	DriverHook driverHook;

debug-app/boards/esp32.overlay

@@ -25,6 +25,12 @@
 			gpios = <&gpio0 26 GPIO_ACTIVE_HIGH>;
 			label = "led pin";
 			status = "okay";
+		};
+
+		wifipin: wifipin{
+			gpios = <&gpio0 5 GPIO_ACTIVE_HIGH>;
+			label = "led pin";
+			status = "okay";
 		
 		};
 	};

debug-app/drivers/ble-scanner/src/ble-scanner.cpp

@@ -68,4 +68,4 @@ void BleScanner::BleDevicefound(const bt_addr_le_t *addr, int8_t rssi, uint8_t t
 	context->currRssi = rssi;
 
 	int err = k_work_submit(&context->bleWork);
-}
+}
\ No newline at end of file

debug-app/drivers/servo-driver/src/servo-driver.cpp

@@ -10,6 +10,8 @@ LOG_MODULE_REGISTER(Servo);
 #define MAX_PWM 100
 
 static const struct pwm_dt_spec pwm_led0 = PWM_DT_SPEC_GET(DT_ALIAS(pwm_led0));
+static const uint32_t minPulse = PWM_USEC(1000);
+static const uint32_t maxPulse = PWM_USEC(2000);
 
 
 ServoDriver::ServoDriver(){
@@ -28,16 +30,16 @@ ServoDriver::ServoDriver(){
 	 * Keep its value at least MIN_PERIOD * 4 to make sure
 	 * the sample changes frequency at least once.
 	 */
-	LOG_INF("Calibrating for channel %d...\n", pwm_led0.channel);
-	max_period = MAX_PERIOD;
-	while (pwm_set_dt(&pwm_led0, max_period, max_period / 2U) < 0) {
-		max_period /= 2U;
-		if (max_period < (4U * MIN_PERIOD)) {
-			LOG_INF("Error: PWM device "
-			       "does not support a period at least %lu\n",
-			       4U * MIN_PERIOD);
-		}
-	}
+	// LOG_INF("Calibrating for channel %d...\n", pwm_led0.channel);
+	// max_period = MAX_PERIOD;
+	// while (pwm_set_dt(&pwm_led0, max_period, max_period / 2U) < 0) {
+	// 	max_period /= 2U;
+	// 	if (max_period < (4U * MIN_PERIOD)) {
+	// 		LOG_INF("Error: PWM device "
+	// 		       "does not support a period at least %lu\n",
+	// 		       4U * MIN_PERIOD);
+	// 	}
+	// }
 }
 
 ServoDriver::~ServoDriver(){
@@ -45,9 +47,12 @@ ServoDriver::~ServoDriver(){
 }
 
 void ServoDriver::SetPwm(uint8_t pwm){
-    if(pwm_set_dt(&pwm_led0, max_period, max_period * (pwm/MAX_PWM)) == 0){
-        period = pwm;
-    }
+	if(period != pwm){
+		uint32_t pulse = minPulse + ((maxPulse - minPulse) * (pwm / MAX_PWM));
+		if(pwm_set_dt(&pwm_led0, max_period, pulse) == 0){
+			period = pwm;
+		}
+	}
 }
 
 uint8_t ServoDriver::GetPwm(){

debug-app/drivers/wifi-controller/src/wifi-controller.cpp

@@ -4,6 +4,8 @@
 #include <stdio.h>
 #include <string.h>
 
+#include "esp_private/system_internal.h"
+
 #include "utilities.h"
 #include "wifi-controller.h"
 #include <zephyr/net/net_ip.h>
@@ -15,6 +17,8 @@
 // #include <zephyr/posix/unistd.h>
 #include <zephyr/sys/slist.h>
 #include <zephyr/init.h>
+#include <zephyr/drivers/gpio.h>
+#include <zephyr/sys/reboot.h>
 
 #define AUTO_CONNECT_SSID "tselhome-605E"
 #define AUTO_CONNECT_SSID_PSK "Sapiular0"
@@ -24,9 +28,13 @@
 				NET_EVENT_WIFI_CONNECT_RESULT |		\
 				NET_EVENT_WIFI_DISCONNECT_RESULT)
 
+#define WIFI_LED DT_NODELABEL(wifipin)
+
 LOG_MODULE_REGISTER(WifiController);
 // ZBUS_CHAN_DECLARE(driverRequestChannel, bleScanChannel);
 
+struct gpio_dt_spec wifiLedNode = GPIO_DT_SPEC_GET(WIFI_LED, gpios);
+
 static WifiController *context;
 static struct k_sem net_cb_sem;
 static struct net_mgmt_event_callback cb;
@@ -43,6 +51,8 @@ WifiController::WifiController(){
 	wifi_args.ssid = (const uint8_t *)AUTO_CONNECT_SSID;
 	wifi_args.ssid_length = strlen(AUTO_CONNECT_SSID);
 
+	gpio_pin_configure_dt(&wifiLedNode, GPIO_OUTPUT_LOW);
+
 	// Init semaphore
 	k_sem_init(&net_cb_sem, 0, 1);
 
@@ -68,7 +78,10 @@ WifiController::WifiController(){
 
 	// Wait for connection.....
 	LOG_INF("Wait for connection.....");
-	k_sem_take(&net_cb_sem, K_FOREVER );
+	if(k_sem_take(&net_cb_sem, K_MSEC(5000)) != 0){
+		// sys_reboot(SYS_REBOOT_COLD);
+		esp_restart_noos();
+	}
 	LOG_INF("SUCCESFULLY CONNECTED");
 }
 
@@ -79,6 +92,7 @@ void WifiController::WifiEventCallback( struct net_mgmt_event_callback *cb, uint
 			if (!status->status) {
 				// Connected
 				k_sem_give(&net_cb_sem);
+				gpio_pin_set_dt(&wifiLedNode, true);
 			}
 			break;
 	}

debug-app/include/device-parser.h

@@ -3,8 +3,11 @@
 #include <zephyr/kernel.h>
 #include "utilities.h"
 
+#define MAX_SENSOR_DATA 32
+
 typedef struct BleSensorData {
-    uint8_t sensorData;
+    uint8_t sensorData[MAX_SENSOR_DATA];
+    uint16_t length;
 } BleSensorData_t;
 
 class DeviceParser {

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

@@ -10,8 +10,14 @@
 #include "led-driver.h"
 #include "servo-driver.h"
 #include "ir-driver.h"
+#include "wifi-controller.h"
+
+#ifndef CONFIG_SOC_POSIX
+LOG_MODULE_REGISTER(Driver_Hook, LOG_LEVEL_INF);
+#else
+LOG_MODULE_REGISTER(Driver_Hook, LOG_LEVEL_DBG);
+#endif
 
-LOG_MODULE_REGISTER(Driver_Hook);
 ZBUS_CHAN_DECLARE(driverRequestChannel, driverResponseChannel, bleScanChannel);
 
 static DriverHook *context;
@@ -21,6 +27,7 @@ ZBUS_LISTENER_DEFINE(driverHookListener, context->DriverRequestListener);
 DriverHook::DriverHook(){
     context = this;
 #ifndef CONFIG_SOC_POSIX
+    WifiController *wifiController = new WifiController();
     bleScanner = new BleScanner();
     ledDriver = new LedDriver();
     servoDriver = new ServoDriver();
@@ -52,6 +59,7 @@ 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:");
+    LOG_DBG("ble driver hook");
     BleData_t sentMessage = {
 		.address = addr,
 		.data = data
@@ -155,5 +163,5 @@ void DriverHook::DriverRequestListener(const struct zbus_channel *chan){
 #endif
 
     int status = zbus_chan_pub(&driverResponseChannel, &sentMessage, K_MSEC(200));
-    LOG_INF("Status pub %d", status);
+    LOG_DBG("Status pub %d", status);
 }
\ No newline at end of file

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

@@ -117,6 +117,12 @@ void DebugHandler::DebugThread(DebugHandler *context){
 			if(memcmp(pass, buf, sizeof(pass)) == 0){
 				LOG_INF("DEBUG ACTIVE");
 				isDebugMode = true;
+#if 0 //For testing purpose. will block indefinetely
+				while(1){
+					uint8_t dummy[2048];
+					context->SendPacket(dummy, sizeof(dummy));
+				}
+#endif				
 			}
 
 			LOG_HEXDUMP_INF(buf, (uint32_t)len, "DATA TCP: ");

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

@@ -2,7 +2,12 @@
 #include <zephyr/logging/log.h>
 #include <zephyr/zbus/zbus.h>
 
-LOG_MODULE_REGISTER(DeviceParser);
+#ifndef CONFIG_SOC_POSIX
+LOG_MODULE_REGISTER(DeviceParser, LOG_LEVEL_INF);
+#else
+LOG_MODULE_REGISTER(DeviceParser, LOG_LEVEL_DBG);
+#endif
+
 ZBUS_CHAN_DECLARE(bleScanChannel, driverResponseChannel);
 
 static DeviceParser *context;
@@ -20,9 +25,9 @@ void DeviceParser::BleScanListener(const struct zbus_channel *chan){
 			msg = (BleData_t *)zbus_chan_const_msg(chan); // Direct message access
             BleSensorData_t parsedData;
 			if(context->BleDeviceParser(msg, &parsedData)){
-                LOG_INF("Device %s has data %d", msg->address.c_str(), parsedData.sensorData);
+                LOG_DBG("Device %s has data %d", msg->address.c_str(), parsedData.sensorData[0]);
                 std::vector<uint8_t> dataValue;
-                dataValue.push_back(parsedData.sensorData);
+                dataValue.push_back(parsedData.sensorData[0]);
                 DriverMessage_t sentMessage = {
                     .number = BLUETOOTH,
                     .type = REQUEST_SET,
@@ -31,7 +36,7 @@ void DeviceParser::BleScanListener(const struct zbus_channel *chan){
 
                 zbus_chan_pub(&driverResponseChannel, &sentMessage, K_FOREVER);
             }
-            // LOG_HEXDUMP_INF(msg->data.data(), msg->data.size(), "data raw");
+            LOG_HEXDUMP_DBG(msg->data.data(), msg->data.size(), "data raw");
 	}
     else{
         return;
@@ -41,7 +46,10 @@ void DeviceParser::BleScanListener(const struct zbus_channel *chan){
 bool DeviceParser::BleDeviceParser(const BleData_t *data, BleSensorData_t *parsedData){
     if(data->data.size() >= 8){
         if(data->data[5] == 0x1a && data->data[6] == 0x18){
-            parsedData->sensorData = data->data[7];
+            parsedData->length = (data->data[7] << 8) | (data->data[8]);
+            for(int i = 0; i < parsedData->length; i++){
+                parsedData->sensorData[i] = data->data[9 + i];
+            }
             return true;
         }
     }

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

@@ -10,7 +10,11 @@
 #define MAIN_THREAD_STACK_SIZE 2048
 #define MAIN_THREAD_PRIORITY 5
 
-LOG_MODULE_REGISTER(Main_Handler);
+#ifndef CONFIG_SOC_POSIX
+LOG_MODULE_REGISTER(Main_Handler, LOG_LEVEL_INF);
+#else
+LOG_MODULE_REGISTER(Main_Handler, LOG_LEVEL_DBG);
+#endif
 
 // Internal
 static MainHandler *context;
@@ -66,7 +70,7 @@ void MainHandler::MainThread(MainHandler *context){
 	// }
     while (true)
 	{
-		LOG_INF("main thread run");
+		LOG_DBG("main thread run");
 		std::vector<uint8_t> stateVec;
 		stateVec.push_back(context->currentState);
 		StateMessage_t stateMsg{
@@ -191,6 +195,6 @@ bool MainHandler::DriverRequest(DriverMessage_t *sentMessage, DriverMessage_t *d
 	// }
 	*driverResponse = driverResponse_g;
 	// *driverResponse_g = NULL;
-    LOG_INF("data available!");
+    LOG_DBG("data available!");
 	return true;
 }
\ No newline at end of file

debug-python/debug.py

 #!/usr/bin/env python
 
-import socket
-import select
 import gdb
 import rpyc
 
-
-TCP_IP = '192.168.8.149'
-TCP_PORT = 12345
-BUFFER_SIZE = 1024
-MESSAGE = b'ABC'
-received = 0
-
 c = rpyc.connect("localhost", 18080)
 
 def GetState():
@@ -23,6 +14,29 @@ def GetBle():
 def GetDriver():
     return c.root.getDriver()
 
+def ClearState():
+    c.root.clearState()
+
+def RefreshState():
+    bpState.enabled = True
+    ClearState()
+
+def ClearBle():
+    c.root.clearBle()
+
+def ClearDriver():
+    c.root.clearDriver()
+
+def ClearAll():
+    ClearState()
+    ClearBle()
+    ClearDriver()
+
+def ClearAndContinue():
+    ClearAll()
+    bpState.enabled = True
+    gdb.execute('continue')
+
 class StateBp(gdb.Breakpoint):
     def __init__(self):
         gdb.Breakpoint.__init__(self, "main-handler.cpp:69")
@@ -33,11 +47,12 @@ class StateBp(gdb.Breakpoint):
         if state >= 0:
             print('set var context->currentState = ' + str(state))
             gdb.execute('set var context->currentState = ' + str(state))
+            self.enabled = False
         return False
 
 class DriverBp(gdb.Breakpoint):
     def __init__(self):
-        gdb.Breakpoint.__init__(self, "debug-handler.cpp:138")
+        gdb.Breakpoint.__init__(self, "debug-handler.cpp:144")
 
     def stop(self):
         bleData = GetBle()
@@ -47,7 +62,6 @@ class DriverBp(gdb.Breakpoint):
             gdb.execute('set var isBleAvailable = true')
             # gdb.execute('print bleLen')
             # gdb.execute('print isBleAvailable')
-            print(bleData)
             for i, data in enumerate(bleData):
                 gdb.execute('set var bleArray[' +str(i) + '] =' + str(data))
             # gdb.execute('print bleArray')
@@ -60,30 +74,5 @@ class DriverBp(gdb.Breakpoint):
 
         return False
 
-def SocketStart():
-    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-    s.connect((TCP_IP, TCP_PORT))
+bpState = StateBp()
+bpDriver = DriverBp()
\ No newline at end of file
-    s.setblocking(False)
-    s.send(MESSAGE)
-
-    inputs = [s]
-    outputs = [s]
-    while inputs:
-        readable,writable,exceptional = select.select(inputs,outputs,inputs,1)
-        data = []
-
-        for s in readable:
-            data = s.recv(1024)
-            # logging.info(f'Non Blocking - data: {len(data)}')
-            # logging.info(f'Non Blocking - closing...')
-        if(len(data) >0):
-            if(data[0]==0):
-                global received
-                received = data[3]
-                print(received)
-                break
-
-        s.close()
-
-bp1 = StateBp()
-bp2 = DriverBp()
\ No newline at end of file

debug-python/socket-run.py

@@ -83,6 +83,21 @@ class DebugService(rpyc.Service):
         else :
             return []
 
+    def exposed_clearState(self):
+        print(bleData)
+        stateData.clear()
+        print(stateData)
+
+    def exposed_clearDriver(self):
+        print(bleData)
+        driverData.clear()
+        print(driverData)        
+    
+    def exposed_clearBle(self):
+        print(bleData)
+        bleData.clear()
+        print(bleData)
+
 
 def SocketStart():
     s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

debug-python/test-connection.py

+#!/usr/bin/env python
+
+import socket
+import select
+import time
+
+TCP_IP = '192.168.8.149'
+TCP_PORT = 12345
+MESSAGE = b'ABC'
+received = 0
+
+stateData = []
+bleData = []
+driverData = []
+
+def socketStart():
+    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    s.connect((TCP_IP, TCP_PORT))
+    s.setblocking(False)
+    start = time.time()
+    dataLen = 0
+    s.send(MESSAGE)
+
+    inputs = [s]
+    outputs = [s]
+    while inputs:
+        readable,writable,exceptional = select.select(inputs,outputs,inputs,1)
+        data = []
+
+        # startTransaction = time.time()
+        for s in readable:
+            data = s.recv(65535)
+            dataLen += len(data)
+            timeData = time.time() - start
+            if(timeData >= 1):
+                print(f"get {dataLen} bytes in {timeData} second")
+                dataLen = 0
+                start = time.time()
+            # print(f'get data length {len(data)} in {end - start}')
+            # print(data)
+        # endTransaction = time.time()
+        # print(f"single transaction in {endTransaction - startTransaction}")
+            
+socketStart()
\ No newline at end of file