diff --git a/debug-app/app/src/main.cpp b/debug-app/app/src/main.cpp index f0eb08a9b80f67e42ff1c697b75754e413350a0e..001d5f6a4efb70eb7b10adbd82b28b590e71f6ac 100644 --- a/debug-app/app/src/main.cpp +++ b/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; diff --git a/debug-app/boards/esp32.overlay b/debug-app/boards/esp32.overlay index c11897f5cd1eb6e8ad5cb3323a020c49651dafa1..4f08485c54d30b3652e5589eaf6e118717fea1b7 100644 --- a/debug-app/boards/esp32.overlay +++ b/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"; }; }; diff --git a/debug-app/drivers/ble-scanner/src/ble-scanner.cpp b/debug-app/drivers/ble-scanner/src/ble-scanner.cpp index 71ce8136332f5e392b2bde9c3075c326f1d551cf..1dd99c6025c9c23dbbc48ed0a32468ba766e8bc1 100644 --- a/debug-app/drivers/ble-scanner/src/ble-scanner.cpp +++ b/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 +} diff --git a/debug-app/drivers/servo-driver/src/servo-driver.cpp b/debug-app/drivers/servo-driver/src/servo-driver.cpp index 968c021c5934381124b6f884a3b04a99ac20118d..d2707dd423ae7a62eb16e67187f6eefaa86ada74 100644 --- a/debug-app/drivers/servo-driver/src/servo-driver.cpp +++ b/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(){ diff --git a/debug-app/drivers/wifi-controller/src/wifi-controller.cpp b/debug-app/drivers/wifi-controller/src/wifi-controller.cpp index 57f001e88d7b2011aef08a5ea27be18ed2ff8bab..a065b63a5a506cd4b281842b8c8d40aa8fbbb3a2 100644 --- a/debug-app/drivers/wifi-controller/src/wifi-controller.cpp +++ b/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; } diff --git a/debug-app/include/device-parser.h b/debug-app/include/device-parser.h index c5f097d0510c2fe3a9f9acf75a5b2e01d28146a8..aa197b700ef07848c190581416871247942d18fc 100644 --- a/debug-app/include/device-parser.h +++ b/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 { diff --git a/debug-app/module/driver-hook/src/driver-hook.cpp b/debug-app/module/driver-hook/src/driver-hook.cpp index 977b796e351c26c7d649f63a3d0ed3d807b8c668..7305421ecab4e15e93da71a3dd9e5a83de9303f8 100644 --- a/debug-app/module/driver-hook/src/driver-hook.cpp +++ b/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 diff --git a/debug-app/module/handler/debug-handler/src/debug-handler.cpp b/debug-app/module/handler/debug-handler/src/debug-handler.cpp index 1c89d522dd8f28c677c8ac64de11f7ae073f3214..8f31169ef223e1e55861331e13e37bbdfe617878 100644 --- a/debug-app/module/handler/debug-handler/src/debug-handler.cpp +++ b/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: "); diff --git a/debug-app/module/handler/device-parser/src/device-parser.cpp b/debug-app/module/handler/device-parser/src/device-parser.cpp index ab6f21f10251c7ff97ebf0c8958f8abb02292335..b54eee3a104ce2495bfa6058c1178a3e4096814c 100644 --- a/debug-app/module/handler/device-parser/src/device-parser.cpp +++ b/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; } } diff --git a/debug-app/module/handler/main-handler/src/main-handler.cpp b/debug-app/module/handler/main-handler/src/main-handler.cpp index d05d0408f0d26fbd896067439362da64c0c83f90..7ea91e232d1d9f5812c06905b33c5ee6a1b52712 100644 --- a/debug-app/module/handler/main-handler/src/main-handler.cpp +++ b/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 diff --git a/debug-python/debug.py b/debug-python/debug.py index 059b94766c8daa9605a7a71039ed5ea79c35c1ed..da64027264976e96beac6ab3f3e9bf50734c7e5b 100644 --- a/debug-python/debug.py +++ b/debug-python/debug.py @@ -1,17 +1,8 @@ #!/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)) - 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 +bpState = StateBp() +bpDriver = DriverBp() \ No newline at end of file diff --git a/debug-python/socket-run.py b/debug-python/socket-run.py index def87e56141f4ede5ec9513d2845bf70e3e40528..e4d41e4a0be9aec319c08541e10075b2ce1a3b19 100755 --- a/debug-python/socket-run.py +++ b/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) diff --git a/debug-python/test-connection.py b/debug-python/test-connection.py new file mode 100644 index 0000000000000000000000000000000000000000..98e52c8ba048055d74869b9545366200a69c8b31 --- /dev/null +++ b/debug-python/test-connection.py @@ -0,0 +1,44 @@ +#!/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