diff --git a/debug-app/app/prj.conf b/debug-app/app/prj.conf
index 0afc48b8df70ff252073f3a7c68018332f6374b6..70d43fa3a61c0ca11331f82f368deff654c82792 100644
--- a/debug-app/app/prj.conf
+++ b/debug-app/app/prj.conf
@@ -33,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
diff --git a/debug-app/drivers/servo-driver/src/servo-driver.cpp b/debug-app/drivers/servo-driver/src/servo-driver.cpp
index 3ea914d2c7f35ea2fc38a17034b8d073e7d288ba..968c021c5934381124b6f884a3b04a99ac20118d 100644
--- a/debug-app/drivers/servo-driver/src/servo-driver.cpp
+++ b/debug-app/drivers/servo-driver/src/servo-driver.cpp
@@ -1,7 +1,43 @@
#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(){
@@ -9,9 +45,11 @@ 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 0;
+ return period;
}
\ No newline at end of file
diff --git a/debug-app/include/servo-driver.h b/debug-app/include/servo-driver.h
index ef7ca12d7a3b3c9fffc3bec218263914208629ab..ca663a205c104167e5e397f2a30f9c1cf4003b2e 100644
--- a/debug-app/include/servo-driver.h
+++ b/debug-app/include/servo-driver.h
@@ -20,4 +20,6 @@ public:
uint8_t GetPwm();
private:
+ uint32_t max_period;
+ uint8_t period;
};
\ No newline at end of file
diff --git a/debug-app/module/driver-hook/src/driver-hook.cpp b/debug-app/module/driver-hook/src/driver-hook.cpp
index 6327f33bd5a7da27166926cc44e5668fb6f746ea..977b796e351c26c7d649f63a3d0ed3d807b8c668 100644
--- a/debug-app/module/driver-hook/src/driver-hook.cpp
+++ b/debug-app/module/driver-hook/src/driver-hook.cpp
@@ -47,11 +47,11 @@ void DriverHook::IrSensorCallback(uint8_t data){
.value = sensorData
};
- zbus_chan_pub(&driverResponseChannel, &sentMessage, K_MSEC(200));
+ 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:");
+ // LOG_HEXDUMP_INF(data.data(), data.size(), "BLE data:");
BleData_t sentMessage = {
.address = addr,
.data = data
@@ -132,6 +132,7 @@ void DriverHook::DriverRequestListener(const struct zbus_channel *chan){
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);
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 ed110811eca03864aa207a05f628618c1f5deda6..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,7 +136,7 @@ void DebugHandler::DebugThread(DebugHandler *context){
uint16_t driverLen = 0;
if(isDriverAvailable){
- driverData.insert(driverData.end(), &driverArray[2], &driverArray[driverLen]);
+ driverData.insert(driverData.end(), &driverArray[2], &driverArray[2] + driverLen);
DriverMessage_t sentMessage = {
.number = (DriverNumb_e)driverArray[0],
.type = (ExchangeType_e)driverArray[1],
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 04f45a4f57c698078a1537b3da1cb88dfd67ded5..8f7248bc6b5353e28e1c9c130b16ba282fa4acb7 100644
--- a/debug-app/module/handler/main-handler/src/main-handler.cpp
+++ b/debug-app/module/handler/main-handler/src/main-handler.cpp
@@ -53,7 +53,7 @@ int MainHandler::Deinit(){
void MainHandler::MainThread(MainHandler *context){
// const struct zbus_channel *chan;
- uint8_t stateCounter = 0;
+ context->currentState = STANDBY;
// while (!zbus_sub_wait(&stateSubs, &chan, K_MSEC(200))) {
// StateMessage_t state;
@@ -91,6 +91,11 @@ void MainHandler::MainThread(MainHandler *context){
configVector.value.push_back(0);
context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+ configVector.number = BLUETOOTH;
+ configVector.type = REQUEST_SET;
+ configVector.value.push_back(0);
+ context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+
break;
}
@@ -113,6 +118,8 @@ void MainHandler::MainThread(MainHandler *context){
configVector.type = REQUEST_SET;
configVector.value.push_back(0);
context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+
+ context->currentState = SCANNING;
break;
case USER_2:
@@ -126,6 +133,8 @@ void MainHandler::MainThread(MainHandler *context){
configVector.type = REQUEST_SET;
configVector.value.push_back(100);
context->DriverRequest(&configVector, &responseVector, K_MSEC(200));
+
+ context->currentState = SCANNING;
break;
default:
@@ -152,11 +161,15 @@ void MainHandler::DriverResponseListener(const struct zbus_channel *chan){
}
if(driver->number == IR_DRIVER){
- if(driver->value[0] == 0 && context->currentState == SCANNING){
- context->currentState = STANDBY;
- }
- else if(driver->value[0] == 1){
- context->currentState = SCANNING;
+ 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;
+ }
+ }
}
}
}