diff --git a/debug-app/include/driver-hook.h b/debug-app/include/driver-hook.h
index cfb43623a3e0b788cf2f73db22c2bb20a5197007..bb7d5328c5903a2ebeb49e919773d5306a73a25c 100644
--- a/debug-app/include/driver-hook.h
+++ b/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
diff --git a/debug-app/include/utilities.h b/debug-app/include/utilities.h
index 85902fc726a298cec15af81918a53c7747adc471..6495d0760380e7b154b3bd4b8a81218fdcd29171 100644
--- a/debug-app/include/utilities.h
+++ b/debug-app/include/utilities.h
@@ -4,9 +4,6 @@
#include <string>
typedef enum DriverNumb{
- SENSOR_1,
- SENSOR_2,
- SENSOR_3,
BLUETOOTH,
SERVO,
LED,
diff --git a/debug-app/module/driver-hook/src/driver-hook.cpp b/debug-app/module/driver-hook/src/driver-hook.cpp
index 18dd26a7d4768c49cc3b266d3a4b160b8479ce92..6327f33bd5a7da27166926cc44e5668fb6f746ea 100644
--- a/debug-app/module/driver-hook/src/driver-hook.cpp
+++ b/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);
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 cecf9664d22fabd772e6e601d86b349f581a0969..ed110811eca03864aa207a05f628618c1f5deda6 100644
--- a/debug-app/module/handler/debug-handler/src/debug-handler.cpp
+++ b/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
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 5a1a44dd57a35d415e73ba83efc3899cc9078c40..04f45a4f57c698078a1537b3da1cb88dfd67ded5 100644
--- a/debug-app/module/handler/main-handler/src/main-handler.cpp
+++ b/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){
diff --git a/debug-python/debug.py b/debug-python/debug.py
index dd8649bba48ffa4d04c0464cbaec49efe6e3490d..059b94766c8daa9605a7a71039ed5ea79c35c1ed 100644
--- a/debug-python/debug.py
+++ b/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()