diff --git a/debug-app/CMakeLists.txt b/debug-app/CMakeLists.txt
index 5e39eb9e467a2d45d6552d3574dedb296d7d9821..6a5839b0d44cf37684c5e4cada455a7f9f954c88 100644
--- a/debug-app/CMakeLists.txt
+++ b/debug-app/CMakeLists.txt
@@ -1,4 +1,4 @@
zephyr_include_directories(include)
add_subdirectory(module)
-# add_subdirectory(drivers)
\ No newline at end of file
+add_subdirectory_ifndef(CONFIG_SOC_POSIX drivers)
\ No newline at end of file
diff --git a/debug-app/app/prj.conf b/debug-app/app/prj.conf
index 15d5d564b28cf35c5fcb8441486926b4f6ec378f..70d43fa3a61c0ca11331f82f368deff654c82792 100644
--- a/debug-app/app/prj.conf
+++ b/debug-app/app/prj.conf
@@ -14,6 +14,7 @@ CONFIG_THREAD_NAME=y
CONFIG_ASSERT=y
CONFIG_DEBUG=n
CONFIG_BOOT_BANNER=n
+CONFIG_SYSTEM_WORKQUEUE_STACK_SIZE=2048
# Peripheral
# CONFIG_ADC=y
@@ -32,6 +33,9 @@ CONFIG_ZBUS_LOG_LEVEL_INF=y
CONFIG_ZBUS_RUNTIME_OBSERVERS=y
CONFIG_ZBUS_CHANNEL_NAME=y
+#peripheral
+CONFIG_PWM=y
+
#ble
CONFIG_BT=y
CONFIG_BT_OBSERVER=y
@@ -65,3 +69,6 @@ CONFIG_NET_L2_ETHERNET=y
# Log
CONFIG_LOG=y
+CONFIG_LOG_PROCESS_THREAD=y
+CONFIG_LOG_BUFFER_SIZE=2048
+CONFIG_LOG_MODE_DEFERRED=y
\ No newline at end of file
diff --git a/debug-app/app/src/main.cpp b/debug-app/app/src/main.cpp
index 7cf10b0ac03848c00f74f78cbc229050258e32ac..f0eb08a9b80f67e42ff1c697b75754e413350a0e 100644
--- a/debug-app/app/src/main.cpp
+++ b/debug-app/app/src/main.cpp
@@ -56,7 +56,7 @@ int main()
{
#if !defined(CONFIG_SOC_POSIX)
WifiController wifiController;
- BleScanner bleScanner;
+ // BleScanner bleScanner;
#endif
DebugHandler debugHanddler;
MainHandler mainHandler;
diff --git a/debug-app/boards/esp32.overlay b/debug-app/boards/esp32.overlay
index 5256637eaac1d5df33f0d13f922bad538813a842..c11897f5cd1eb6e8ad5cb3323a020c49651dafa1 100644
--- a/debug-app/boards/esp32.overlay
+++ b/debug-app/boards/esp32.overlay
@@ -4,6 +4,62 @@
* SPDX-License-Identifier: Apache-2.0
*/
- &wifi {
+/{
+ aliases {
+ pwm-0 = &ledc0;
+ pwm-led0 = &pwm_led_blue;
+ };
+
+ gpio_in{
+ compatible = "gpio-keys";
+ ir_Sensor: ir_sensor{
+ gpios = <&gpio0 25 GPIO_ACTIVE_HIGH>;
+ label = "ir sensor interrupt";
+ status = "okay";
+ };
+ };
+
+ gpio_out{
+ compatible = "gpio-leds";
+ led_pin: led_pin{
+ gpios = <&gpio0 26 GPIO_ACTIVE_HIGH>;
+ label = "led pin";
+ status = "okay";
+
+ };
+ };
+
+ pwmServo {
+ compatible = "pwm-leds";
+ pwm_led_blue: pwm_led_gpio0_2 {
+ label = "PWM SERVO";
+ pwms = <&ledc0 0 1000 PWM_POLARITY_NORMAL>;
+ };
+ };
+};
+
+
+&pinctrl {
+ ledc0_default: ledc0_default {
+ group1 {
+ pinmux = <LEDC_CH0_GPIO2>;
+ output-enable;
+ };
+ };
+};
+
+&ledc0 {
+ pinctrl-0 = <&ledc0_default>;
+ pinctrl-names = "default";
+ status = "okay";
+ #address-cells = <1>;
+ #size-cells = <0>;
+ channel0@0 {
+ reg = <0x0>;
+ timer = <0>;
+ };
+};
+
+&wifi {
status = "okay";
};
\ No newline at end of file
diff --git a/debug-app/drivers/CMakeLists.txt b/debug-app/drivers/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6f943b64e44d9c4ef79f87f4c8a84333ed278020
--- /dev/null
+++ b/debug-app/drivers/CMakeLists.txt
@@ -0,0 +1,5 @@
+add_subdirectory(ble-scanner)
+add_subdirectory(wifi-controller)
+add_subdirectory(ir-driver)
+add_subdirectory(servo-driver)
+add_subdirectory(led-driver)
\ No newline at end of file
diff --git a/debug-app/module/ble-scanner/CMakeLists.txt b/debug-app/drivers/ble-scanner/CMakeLists.txt
similarity index 100%
rename from debug-app/module/ble-scanner/CMakeLists.txt
rename to debug-app/drivers/ble-scanner/CMakeLists.txt
diff --git a/debug-app/drivers/ble-scanner/src/ble-scanner.cpp b/debug-app/drivers/ble-scanner/src/ble-scanner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..71ce8136332f5e392b2bde9c3075c326f1d551cf
--- /dev/null
+++ b/debug-app/drivers/ble-scanner/src/ble-scanner.cpp
@@ -0,0 +1,71 @@
+#include <zephyr/logging/log.h>
+#include <zephyr/zbus/zbus.h>
+
+#include <stdio.h>
+#include <string.h>
+
+#include "utilities.h"
+#include "ble-scanner.h"
+
+LOG_MODULE_REGISTER(BleScanner);
+static BleScanner *context;
+
+static struct bt_le_scan_param scan_param = {
+ .type = BT_LE_SCAN_TYPE_PASSIVE,
+ .options = BT_LE_SCAN_OPT_NONE,
+ .interval = BT_GAP_SCAN_FAST_INTERVAL,
+ .window = BT_GAP_SCAN_FAST_WINDOW,
+ };
+
+BleScanner::BleScanner(){
+ context = this;
+ int err = bt_enable(NULL);
+ LOG_INF("start bt");
+ if (err != 0) {
+ printk("Bluetooth init failed (err %d)\n", err);
+ return;
+ }
+
+ k_work_init(&bleWork, BleInterruptHandler);
+}
+
+void BleScanner::BleInterruptHandler(struct k_work *item){
+ if(context->registeredCallback != NULL){
+ context->registeredCallback(context->currAddr, context->currData, context->currRssi);
+ }
+}
+
+void BleScanner::SetScan(bool enable){
+ if(enable){
+ bt_le_scan_start(&scan_param, BleDevicefound);
+ }
+ else{
+ bt_le_scan_stop();
+ }
+}
+
+void BleScanner::RegisterCallback(void (*callback)(std::string, std::vector<uint8_t>, int8_t)){
+ registeredCallback = callback;
+}
+
+RawBleData_t BleScanner::GetLatestData(){
+ RawBleData_t rawData = {
+ .addrStr = currAddr,
+ .bleData = currData,
+ .rssi = currRssi
+ };
+ return rawData;
+}
+
+void BleScanner::BleDevicefound(const bt_addr_le_t *addr, int8_t rssi, uint8_t type, struct net_buf_simple *ad){
+ char addr_str[BT_ADDR_LE_STR_LEN];
+ bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));
+ std::string addrStr(addr_str, sizeof(addr_str));
+ std::vector<uint8_t> dataVec(ad->data, ad->data + ad->len);
+
+ context->currAddr = addrStr;
+ context->currData = dataVec;
+ context->currRssi = rssi;
+
+ int err = k_work_submit(&context->bleWork);
+}
\ No newline at end of file
diff --git a/debug-app/drivers/ir-driver/CMakeLists.txt b/debug-app/drivers/ir-driver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..59e33a4b0c6a85fc3486b537121ecebc7e564366
--- /dev/null
+++ b/debug-app/drivers/ir-driver/CMakeLists.txt
@@ -0,0 +1 @@
+zephyr_library_sources(src/ir-driver.cpp)
\ No newline at end of file
diff --git a/debug-app/drivers/ir-driver/src/ir-driver.cpp b/debug-app/drivers/ir-driver/src/ir-driver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..97104aee3f78acefbd91d47e1e1a5cf88c7de9cf
--- /dev/null
+++ b/debug-app/drivers/ir-driver/src/ir-driver.cpp
@@ -0,0 +1,42 @@
+#include "ir-driver.h"
+#include <zephyr/drivers/gpio.h>
+
+#define IRSENSOR_PIN DT_NODELABEL(ir_sensor)
+
+struct gpio_dt_spec irSensorNode = GPIO_DT_SPEC_GET(IRSENSOR_PIN, gpios);
+struct gpio_callback irSensorCb;
+
+static IrDriver *context;
+
+IrDriver::IrDriver(){
+ context = this;
+ int err = gpio_pin_configure_dt(&irSensorNode, GPIO_INPUT);
+ gpio_pin_interrupt_configure_dt(&irSensorNode, GPIO_INT_EDGE_BOTH);
+ gpio_init_callback(&irSensorCb, InterruptCallback, BIT(irSensorNode.pin));
+ gpio_add_callback(irSensorNode.port, &irSensorCb);
+
+ k_work_init(&work, InterruptHandler);
+}
+
+IrDriver::~IrDriver(){
+
+}
+
+bool IrDriver::GetState(){
+
+ return gpio_pin_get_dt(&irSensorNode);
+}
+
+void IrDriver::RegisterInterrupt(void (*callback)(uint8_t)){
+ registeredCallback = callback;
+}
+
+void IrDriver::InterruptCallback(const struct device *dev, struct gpio_callback *cb, uint32_t pins){
+ k_work_submit(&context->work);
+}
+
+void IrDriver::InterruptHandler(struct k_work *item){
+ if(context->registeredCallback != NULL){
+ context->registeredCallback(gpio_pin_get_dt(&irSensorNode));
+ }
+}
\ No newline at end of file
diff --git a/debug-app/drivers/led-driver/CMakeLists.txt b/debug-app/drivers/led-driver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dc876e428e26c07dfeca7793a0adbea69d15a421
--- /dev/null
+++ b/debug-app/drivers/led-driver/CMakeLists.txt
@@ -0,0 +1 @@
+zephyr_library_sources(src/led-driver.cpp)
\ No newline at end of file
diff --git a/debug-app/drivers/led-driver/src/led-driver.cpp b/debug-app/drivers/led-driver/src/led-driver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..03a1e0983ea0407aa276964c29eef27fab9855b2
--- /dev/null
+++ b/debug-app/drivers/led-driver/src/led-driver.cpp
@@ -0,0 +1,23 @@
+#include "led-driver.h"
+#include <zephyr/drivers/gpio.h>
+
+#define LED_PIN DT_NODELABEL(led_pin)
+
+struct gpio_dt_spec ledPinNode = GPIO_DT_SPEC_GET(LED_PIN, gpios);
+
+LedDriver::LedDriver(){
+ int err = gpio_pin_configure_dt(&ledPinNode, GPIO_OUTPUT_INACTIVE);
+}
+
+LedDriver::~LedDriver(){
+
+}
+
+void LedDriver::SetState(bool enable){
+ gpio_pin_set_dt(&ledPinNode, enable);
+ currentState = enable;
+}
+
+bool LedDriver::GetState(){
+ return currentState;
+}
\ No newline at end of file
diff --git a/debug-app/drivers/servo-driver/CMakeLists.txt b/debug-app/drivers/servo-driver/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0b3a7788f0a0e5a83b22d13d4e359b411ccfdd37
--- /dev/null
+++ b/debug-app/drivers/servo-driver/CMakeLists.txt
@@ -0,0 +1 @@
+zephyr_library_sources(src/servo-driver.cpp)
\ 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
new file mode 100644
index 0000000000000000000000000000000000000000..968c021c5934381124b6f884a3b04a99ac20118d
--- /dev/null
+++ b/debug-app/drivers/servo-driver/src/servo-driver.cpp
@@ -0,0 +1,55 @@
+#include "servo-driver.h"
+#include <zephyr/drivers/pwm.h>
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(Servo);
+
+#define MIN_PERIOD PWM_SEC(1U) / 128U
+#define MAX_PERIOD PWM_SEC(1U)
+
+#define MAX_PWM 100
+
+static const struct pwm_dt_spec pwm_led0 = PWM_DT_SPEC_GET(DT_ALIAS(pwm_led0));
+
+
+ServoDriver::ServoDriver(){
+ uint8_t dir = 0U;
+ int ret;
+
+ if (!pwm_is_ready_dt(&pwm_led0)) {
+ LOG_ERR("Error: PWM device %s is not ready\n",
+ pwm_led0.dev->name);
+ }
+
+ /*
+ * In case the default MAX_PERIOD value cannot be set for
+ * some PWM hardware, decrease its value until it can.
+ *
+ * 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);
+ }
+ }
+}
+
+ServoDriver::~ServoDriver(){
+
+}
+
+void ServoDriver::SetPwm(uint8_t pwm){
+ if(pwm_set_dt(&pwm_led0, max_period, max_period * (pwm/MAX_PWM)) == 0){
+ period = pwm;
+ }
+}
+
+uint8_t ServoDriver::GetPwm(){
+ return period;
+}
\ No newline at end of file
diff --git a/debug-app/module/wifi-controller/CMakeLists.txt b/debug-app/drivers/wifi-controller/CMakeLists.txt
similarity index 100%
rename from debug-app/module/wifi-controller/CMakeLists.txt
rename to debug-app/drivers/wifi-controller/CMakeLists.txt
diff --git a/debug-app/module/wifi-controller/src/wifi-controller.cpp b/debug-app/drivers/wifi-controller/src/wifi-controller.cpp
similarity index 100%
rename from debug-app/module/wifi-controller/src/wifi-controller.cpp
rename to debug-app/drivers/wifi-controller/src/wifi-controller.cpp
diff --git a/debug-app/include/ble-scanner.h b/debug-app/include/ble-scanner.h
index 24742c1c062a1c2624e9a60565dbd245cc820bbc..446ec2cf09bbb2e257abffdecf4c759288a85ba7 100644
--- a/debug-app/include/ble-scanner.h
+++ b/debug-app/include/ble-scanner.h
@@ -1,6 +1,16 @@
+#pragma once
+
#include <zephyr/kernel.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/hci.h>
+#include <string>
+#include <vector>
+
+typedef struct RawBleData {
+ std::string addrStr;
+ std::vector<uint8_t> bleData;
+ int8_t rssi;
+} RawBleData_t;
class BleScanner {
public:
@@ -16,8 +26,24 @@ public:
static void DriverRequestListener(const struct zbus_channel *chan);
+ void RegisterCallback(void (*callback)(std::string, std::vector<uint8_t>, int8_t));
+
+ void SetScan(bool enable);
+
+ RawBleData_t GetLatestData();
+
private:
+ std::string currAddr;
+ std::vector<uint8_t> currData;
+ int8_t currRssi;
+
+ struct k_work bleWork;
+
+ void (*registeredCallback)(std::string, std::vector<uint8_t>, int8_t) = NULL;
+
static void BleDevicefound(const bt_addr_le_t *addr, int8_t rssi, uint8_t type, struct net_buf_simple *ad);
+
+ static void BleInterruptHandler(struct k_work *item);
};
\ No newline at end of file
diff --git a/debug-app/include/debug-handler.h b/debug-app/include/debug-handler.h
index 4a7b440eb0ba046d71030ae450176457dab69aed..942efe8f70a6872164d79824697ed7e6d00f912e 100644
--- a/debug-app/include/debug-handler.h
+++ b/debug-app/include/debug-handler.h
@@ -1,3 +1,5 @@
+#pragma once
+
#include <zephyr/kernel.h>
#include <vector>
diff --git a/debug-app/include/device-parser.h b/debug-app/include/device-parser.h
index eefa340aadde1d5fcb0287e0b83ff7bd17c75d93..c5f097d0510c2fe3a9f9acf75a5b2e01d28146a8 100644
--- a/debug-app/include/device-parser.h
+++ b/debug-app/include/device-parser.h
@@ -1,8 +1,10 @@
+#pragma once
+
#include <zephyr/kernel.h>
#include "utilities.h"
typedef struct BleSensorData {
- uint8_t tempSensor;
+ uint8_t sensorData;
} BleSensorData_t;
class DeviceParser {
diff --git a/debug-app/include/driver-hook.h b/debug-app/include/driver-hook.h
index 0ca8287b069f078917e4c8791a085d58c41d1d8b..bb7d5328c5903a2ebeb49e919773d5306a73a25c 100644
--- a/debug-app/include/driver-hook.h
+++ b/debug-app/include/driver-hook.h
@@ -2,6 +2,11 @@
#include <zephyr/kernel.h>
+#include "ble-scanner.h"
+#include "led-driver.h"
+#include "servo-driver.h"
+#include "ir-driver.h"
+
class DriverHook {
public:
/** Constructor.
@@ -21,5 +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/ir-driver.h b/debug-app/include/ir-driver.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0ca7b6dc6a6c4934b5c29256111bd9989d675f9
--- /dev/null
+++ b/debug-app/include/ir-driver.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <zephyr/kernel.h>
+#include <string>
+
+class IrDriver {
+public:
+ void (*registeredCallback)(uint8_t) = NULL;
+ struct k_work work;
+
+ /** Constructor.
+ *
+ **/
+ IrDriver();
+
+ /** Destructor.
+ *
+ **/
+ ~IrDriver();
+
+ bool GetState();
+
+ void RegisterInterrupt(void (*callback)(uint8_t));
+
+private:
+
+ static void InterruptCallback(const struct device *dev, struct gpio_callback *cb, uint32_t pins);
+
+ static void InterruptHandler(struct k_work *item);
+};
\ No newline at end of file
diff --git a/debug-app/include/led-driver.h b/debug-app/include/led-driver.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e90eaf9992cc20bacbdc456677081bde600d948
--- /dev/null
+++ b/debug-app/include/led-driver.h
@@ -0,0 +1,24 @@
+#pragma once
+
+#include <zephyr/kernel.h>
+#include <string>
+
+class LedDriver {
+public:
+ /** Constructor.
+ *
+ **/
+ LedDriver();
+
+ /** Destructor.
+ *
+ **/
+ ~LedDriver();
+
+ void SetState(bool enable);
+
+ bool GetState();
+
+private:
+ bool currentState;
+};
\ No newline at end of file
diff --git a/debug-app/include/main-handler.h b/debug-app/include/main-handler.h
index 1c4bc038b7770dca6237811b89510fc694bc37bd..284806cb76eea12153dcbc580c3416027aff2593 100644
--- a/debug-app/include/main-handler.h
+++ b/debug-app/include/main-handler.h
@@ -1,6 +1,15 @@
+#pragma once
+
#include <zephyr/kernel.h>
#include "utilities.h"
+typedef enum DeviceState{
+ STANDBY,
+ SCANNING,
+ USER_1,
+ USER_2
+} DeviceState_e;
+
class MainHandler {
public:
/** Constructor.
@@ -24,6 +33,7 @@ private:
DriverNumb_e requestedResponse;
DriverMessage_t driverResponse_g;
+ uint8_t currentState;
static void MainThread(MainHandler *context);
bool DriverRequest(DriverMessage_t *sentMessage, DriverMessage_t *driverResponse, k_timeout_t timeout);
diff --git a/debug-app/include/servo-driver.h b/debug-app/include/servo-driver.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca663a205c104167e5e397f2a30f9c1cf4003b2e
--- /dev/null
+++ b/debug-app/include/servo-driver.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <zephyr/kernel.h>
+#include <string>
+
+class ServoDriver {
+public:
+ /** Constructor.
+ *
+ **/
+ ServoDriver();
+
+ /** Destructor.
+ *
+ **/
+ ~ServoDriver();
+
+ void SetPwm(uint8_t pwm);
+
+ uint8_t GetPwm();
+
+private:
+ uint32_t max_period;
+ uint8_t period;
+};
\ No newline at end of file
diff --git a/debug-app/include/utilities.h b/debug-app/include/utilities.h
index 9c34753ba6ad1ab5d7eeb0c45fc852ec1fa209d8..6495d0760380e7b154b3bd4b8a81218fdcd29171 100644
--- a/debug-app/include/utilities.h
+++ b/debug-app/include/utilities.h
@@ -4,11 +4,10 @@
#include <string>
typedef enum DriverNumb{
- SENSOR_1,
- SENSOR_2,
- SENSOR_3,
BLUETOOTH,
- ACTUATOR_1,
+ SERVO,
+ LED,
+ IR_DRIVER
} DriverNumb_e;
typedef enum StateFlag{
@@ -17,8 +16,16 @@ typedef enum StateFlag{
DATA_2
} StateFlag_e;
+typedef enum ExchangeType{
+ REQUEST_GET,
+ REQUEST_SET,
+ RESPONSE_GET,
+ RESPONSE_SET
+} ExchangeType_e;
+
typedef struct DriverMessage {
DriverNumb_e number;
+ ExchangeType_e type;
std::vector<uint8_t> value;
} DriverMessage_t;
diff --git a/debug-app/include/wifi-controller.h b/debug-app/include/wifi-controller.h
index ebcc467062e8fb04a4320cd434567b8b5cc3a0e5..977ef619588449290e1adc800063b68a28a398d0 100644
--- a/debug-app/include/wifi-controller.h
+++ b/debug-app/include/wifi-controller.h
@@ -1,3 +1,5 @@
+#pragma once
+
#include <zephyr/kernel.h>
#include <zephyr/net/wifi.h>
#include <zephyr/net/wifi_mgmt.h>
diff --git a/debug-app/module/CMakeLists.txt b/debug-app/module/CMakeLists.txt
index 9d1648d553ce7fdcf0ebaf5d7ac5a19d8ef2c57f..379a5da3d34d7b05c5476f32e70196e2045c18ef 100644
--- a/debug-app/module/CMakeLists.txt
+++ b/debug-app/module/CMakeLists.txt
@@ -1,4 +1,2 @@
add_subdirectory(driver-hook)
-add_subdirectory(handler)
-add_subdirectory_ifndef(CONFIG_SOC_POSIX ble-scanner)
-add_subdirectory_ifndef(CONFIG_SOC_POSIX wifi-controller)
\ No newline at end of file
+add_subdirectory(handler)
\ No newline at end of file
diff --git a/debug-app/module/ble-scanner/src/ble-scanner.cpp b/debug-app/module/ble-scanner/src/ble-scanner.cpp
deleted file mode 100644
index cfd98b5e6fb437b28b86a91ce4e210f2258f92cc..0000000000000000000000000000000000000000
--- a/debug-app/module/ble-scanner/src/ble-scanner.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-#include <zephyr/logging/log.h>
-#include <zephyr/zbus/zbus.h>
-
-#include <stdio.h>
-#include <string.h>
-
-#include "utilities.h"
-#include "ble-scanner.h"
-
-LOG_MODULE_REGISTER(BleScanner);
-ZBUS_CHAN_DECLARE(driverRequestChannel, bleScanChannel);
-
-static BleScanner *context;
-
-static struct bt_le_scan_param scan_param = {
- .type = BT_LE_SCAN_TYPE_PASSIVE,
- .options = BT_LE_SCAN_OPT_NONE,
- .interval = BT_GAP_SCAN_FAST_INTERVAL,
- .window = BT_GAP_SCAN_FAST_WINDOW,
- };
-
-// ZBUS_LISTENER_DEFINE(bleScanListener, context->DriverRequestListener);
-
-BleScanner::BleScanner(){
- context = this;
- int err = bt_enable(NULL);
- LOG_INF("start bt");
- if (err != 0) {
- printk("Bluetooth init failed (err %d)\n", err);
- return;
- }
-
- // zbus_chan_add_obs(&driverResponseChannel, &debugDriverListener, K_NO_WAIT);
-
- //TODO: remove when bus communication has fully established
- bt_le_scan_start(&scan_param, BleDevicefound);
-
-
-}
-
-void BleScanner::BleDevicefound(const bt_addr_le_t *addr, int8_t rssi, uint8_t type, struct net_buf_simple *ad){
- char addr_str[BT_ADDR_LE_STR_LEN];
- bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));
- BleData_t sentMessage = {
- .address = std::string(addr_str, sizeof(addr_str)),
- .data = std::vector<uint8_t>(ad->data, ad->data + ad->len)
- };
- zbus_chan_pub(&bleScanChannel, &sentMessage, K_MSEC(200));
-}
-
-// void DriverRequestListener(const struct zbus_channel *chan){
-// const DriverMessage_t *msg;
-// if (&driverRequestChannel == chan) {
-// msg = (DriverMessage_t *)zbus_chan_const_msg(chan); // Direct message access
-// if(msg->number == BLUETOOTH){
-// if(msg->value[0]){
-// bt_le_scan_start(&scan_param, bleDevicefound);
-// }
-// }
-// }
-// else{
-// return;
-// }
-// }
diff --git a/debug-app/module/driver-hook/src/driver-hook.cpp b/debug-app/module/driver-hook/src/driver-hook.cpp
index 2fe05b456876c723d6dd0d0c1c344bcac8b4cded..977b796e351c26c7d649f63a3d0ed3d807b8c668 100644
--- a/debug-app/module/driver-hook/src/driver-hook.cpp
+++ b/debug-app/module/driver-hook/src/driver-hook.cpp
@@ -6,9 +6,13 @@
#include "driver-hook.h"
#include "utilities.h"
+#include "ble-scanner.h"
+#include "led-driver.h"
+#include "servo-driver.h"
+#include "ir-driver.h"
LOG_MODULE_REGISTER(Driver_Hook);
-ZBUS_CHAN_DECLARE(driverRequestChannel, driverResponseChannel);
+ZBUS_CHAN_DECLARE(driverRequestChannel, driverResponseChannel, bleScanChannel);
static DriverHook *context;
@@ -16,6 +20,15 @@ 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);
}
@@ -23,6 +36,28 @@ DriverHook::~DriverHook(){
}
+void DriverHook::IrSensorCallback(uint8_t data){
+ LOG_INF("Iterrupt triggeredd. pin %d", data);
+ std::vector<uint8_t> sensorData;
+ sensorData.push_back(data);
+
+ DriverMessage_t sentMessage = {
+ .number = IR_DRIVER,
+ .type = REQUEST_SET,
+ .value = sensorData
+ };
+
+ zbus_chan_pub(&driverResponseChannel, &sentMessage, K_FOREVER);
+}
+
+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(){
@@ -47,27 +82,77 @@ void DriverHook::DriverRequestListener(const struct zbus_channel *chan){
DriverMessage_t sentMessage = {
.number = msg->number,
- .value = data
+ .type = msg->type,
};
+#ifndef CONFIG_SOC_POSIX
+ switch (msg->number)
+ {
+ case BLUETOOTH:
+ if(msg->type == REQUEST_SET){
+ context->bleScanner->SetScan(msg->value[0]);
+ sentMessage.value.push_back(0);
+ }
+ break;
+
+ case LED:
+ if(msg->type == REQUEST_SET){
+ context->ledDriver->SetState(msg->value[0]);
+ }
+ else if(msg->type == REQUEST_GET){
+ bool ledState = context->ledDriver->GetState();
+ sentMessage.value.push_back(ledState);
+ }
+ break;
+ case SERVO:
+ if(msg->type == REQUEST_SET){
+ context->servoDriver->SetPwm(msg->value[0]);
+ }
+ else if(msg->type == REQUEST_GET){
+ uint8_t servoPwm = context->servoDriver->GetPwm();
+ sentMessage.value.push_back(servoPwm);
+ }
+ break;
+
+ default:
+ LOG_ERR("Invalid Sensor %d", msg->number);
+ break;
+ }
+#else
+ static bool ledState;
+ static uint8_t pwmState;
switch (msg->number)
{
- case SENSOR_1:
- LOG_INF("Sensor %d", msg->number);
+ case BLUETOOTH:
+ if(msg->type == REQUEST_SET){
+ sentMessage.value.push_back(0);
+ }
break;
- case SENSOR_2:
- LOG_INF("Sensor %d", msg->number);
+ case LED:
+ if(msg->type == REQUEST_SET){
+ ledState = msg->value[0];
+ sentMessage.value.push_back(ledState);
+ }
+ else if(msg->type == REQUEST_GET){
+ sentMessage.value.push_back(ledState);
+ }
break;
- case SENSOR_3:
- LOG_INF("Sensor %d", msg->number);
+ 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..1c89d522dd8f28c677c8ac64de11f7ae073f3214 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[2] + 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/device-parser/src/device-parser.cpp b/debug-app/module/handler/device-parser/src/device-parser.cpp
index bff5713bb42dc8058f134ad7d88c7f9c145be04f..ab6f21f10251c7ff97ebf0c8958f8abb02292335 100644
--- a/debug-app/module/handler/device-parser/src/device-parser.cpp
+++ b/debug-app/module/handler/device-parser/src/device-parser.cpp
@@ -3,7 +3,7 @@
#include <zephyr/zbus/zbus.h>
LOG_MODULE_REGISTER(DeviceParser);
-ZBUS_CHAN_DECLARE(bleScanChannel);
+ZBUS_CHAN_DECLARE(bleScanChannel, driverResponseChannel);
static DeviceParser *context;
@@ -19,10 +19,19 @@ void DeviceParser::BleScanListener(const struct zbus_channel *chan){
if (&bleScanChannel == 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.tempSensor);
- // }
- LOG_HEXDUMP_INF(msg->data.data(), msg->data.size(), "data raw");
+ if(context->BleDeviceParser(msg, &parsedData)){
+ LOG_INF("Device %s has data %d", msg->address.c_str(), parsedData.sensorData);
+ std::vector<uint8_t> dataValue;
+ dataValue.push_back(parsedData.sensorData);
+ DriverMessage_t sentMessage = {
+ .number = BLUETOOTH,
+ .type = REQUEST_SET,
+ .value = dataValue
+ };
+
+ zbus_chan_pub(&driverResponseChannel, &sentMessage, K_FOREVER);
+ }
+ // LOG_HEXDUMP_INF(msg->data.data(), msg->data.size(), "data raw");
}
else{
return;
@@ -32,7 +41,7 @@ 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->tempSensor = data->data[7];
+ parsedData->sensorData = data->data[7];
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 6d54bc6a001d9856c21fa19db8e3bdc0600ec580..d05d0408f0d26fbd896067439362da64c0c83f90 100644
--- a/debug-app/module/handler/main-handler/src/main-handler.cpp
+++ b/debug-app/module/handler/main-handler/src/main-handler.cpp
@@ -40,7 +40,7 @@ MainHandler::MainHandler(){
k_sem_init(&responseWait, 0, 1);
// zbus_chan_add_obs(&driverResponseChannel, &sensorSubs, K_MSEC(200));
- // zbus_chan_add_obs(&driverResponseChannel, &driverListener, K_NO_WAIT);
+ zbus_chan_add_obs(&driverResponseChannel, &driverListener, K_NO_WAIT);
}
void MainHandler::Init(){
@@ -53,8 +53,7 @@ int MainHandler::Deinit(){
void MainHandler::MainThread(MainHandler *context){
// const struct zbus_channel *chan;
- uint8_t currentState = 1;
- uint8_t stateCounter = 0;
+ context->currentState = STANDBY;
// while (!zbus_sub_wait(&stateSubs, &chan, K_MSEC(200))) {
// StateMessage_t state;
@@ -69,52 +68,87 @@ void MainHandler::MainThread(MainHandler *context){
{
LOG_INF("main thread run");
std::vector<uint8_t> stateVec;
- stateVec.push_back(currentState);
+ stateVec.push_back(context->currentState);
StateMessage_t stateMsg{
.flag = STATE,
.value = stateVec
};
+ DriverMessage_t configVector, responseVector;
zbus_chan_pub(&stateChannel, &stateMsg, K_MSEC(200));
- switch (currentState)
+ switch (context->currentState)
{
- case 1:{
- LOG_INF("entering state 1");
- // DriverMessage_t configVector, responseVector;
- // configVector.number = SENSOR_1;
- // configVector.value.push_back(SENSOR_1);
- // if(context->DriverRequest(&configVector, &responseVector, K_MSEC(200))){
- // LOG_INF("get sensor data %d", responseVector.number);
-
- // }
- // else{
- // LOG_INF("fail");
- // }
- k_msleep(500);
- stateCounter++;
- if(stateCounter > 10){
- stateCounter = 0;
- currentState = 2;
- }
+ case STANDBY:{
+ LOG_INF("Standby");
+
+ configVector.number = LED;
+ configVector.type = REQUEST_SET;
+ configVector.value.push_back(0);
+ context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+ configVector.value.clear();
+
+ configVector.number = SERVO;
+ configVector.type = REQUEST_SET;
+ configVector.value.push_back(0);
+ context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+ configVector.value.clear();
+
+ configVector.number = BLUETOOTH;
+ configVector.type = REQUEST_SET;
+ configVector.value.push_back(0);
+ context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+ configVector.value.clear();
+
break;
}
- case 2:
- LOG_INF("entering state 2");
- k_msleep(500);
- stateCounter++;
- if(stateCounter > 10){
- stateCounter = 0;
- currentState = 1;
- }
+ case SCANNING:
+ LOG_INF("Scanning");
+ configVector.number = BLUETOOTH;
+ configVector.type = REQUEST_SET;
+ configVector.value.push_back(1);
+ context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+ configVector.value.clear();
break;
- case 3:
- /* code */
- break;
+ case USER_1:
+ LOG_INF("User 1");
+ configVector.number = LED;
+ configVector.type = REQUEST_SET;
+ configVector.value.push_back(1);
+ context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+ configVector.value.clear();
+
+ configVector.number = SERVO;
+ configVector.type = REQUEST_SET;
+ configVector.value.push_back(0);
+ context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+ configVector.value.clear();
+
+ context->currentState = SCANNING;
+ break;
+
+ case USER_2:
+ LOG_INF("User 2");
+ configVector.number = LED;
+ configVector.type = REQUEST_SET;
+ configVector.value.push_back(0);
+ context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+ configVector.value.clear();
+
+ configVector.number = SERVO;
+ configVector.type = REQUEST_SET;
+ configVector.value.push_back(100);
+ context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+ configVector.value.clear();
+
+ context->currentState = SCANNING;
+ break;
+
default:
break;
}
+ k_msleep(500);
}
}
@@ -123,25 +157,40 @@ void MainHandler::DriverResponseListener(const struct zbus_channel *chan){
const DriverMessage_t *driver;
if (&driverResponseChannel == chan) {
driver = (DriverMessage_t *)zbus_chan_const_msg(chan); // Direct message access
- if(driver->number == context->requestedResponse){
- // k_sem_give(&responseWait);
- context->driverResponse_g = *driver;
+ context->driverResponse_g = *driver;
+
+ if(driver->number == BLUETOOTH){
+ if(driver->value[0] == 1){
+ context->currentState = USER_1;
}
+ else if(driver->value[0] == 2){
+ context->currentState = USER_2;
+ }
+ }
+
+ if(driver->number == IR_DRIVER){
+ if(driver->value.size() > 0){
+ if(driver->value[0] == 1){
+ if(context->currentState == SCANNING){
+ context->currentState = STANDBY;
+ }
+ else if(context->currentState == STANDBY){
+ context->currentState = SCANNING;
+ }
+ }
+ }
+ }
}
}
bool MainHandler::DriverRequest(DriverMessage_t *sentMessage, DriverMessage_t *driverResponse, k_timeout_t timeout){
zbus_chan_pub(&driverRequestChannel, sentMessage, K_MSEC(200));
- requestedResponse = sentMessage->number;
-
- if (k_sem_take(&responseWait, timeout) == 0) {
- *driverResponse = driverResponse_g;
- LOG_INF("data available!");
-
- } else {
- LOG_ERR("timeout");
- return false;
- }
+ // if(&driverResponse_g == NULL){
+ // return false;
+ // }
+ *driverResponse = driverResponse_g;
+ // *driverResponse_g = NULL;
+ LOG_INF("data available!");
return true;
}
\ No newline at end of file
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()