wip: onboard pid

2025-09-12 15:45:14 +02:00
parent fb9fb9bdf0
commit 52b0dd74c0
39 changed files with 552 additions and 423 deletions
--- a/src/led_controller/led_controller.cpp
+++ b/src/led_controller/led_controller.cpp
@@ -0,0 +1,92 @@
+#include "led_controller.hpp"
+#include "devices.hpp"
+#include "rcl_util.hpp"
+
+#include <zephyr/drivers/gpio.h>
+#include <rcl/rcl.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+#include <std_msgs/msg/bool.h>
+
+std_msgs__msg__Bool led_msg;
+rcl_node_t LedController::node;
+rcl_subscription_t LedController::led0_subscription;
+rcl_subscription_t LedController::led1_subscription;
+rcl_subscription_t LedController::led2_subscription;
+
+
+void led0_callback(const void *msg){
+    gpio_pin_configure_dt(&led0, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
+}
+
+
+void led1_callback(const void *msg){
+    gpio_pin_configure_dt(&led1, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
+}
+
+
+void led2_callback(const void *msg){
+    gpio_pin_configure_dt(&led2, ((std_msgs__msg__Bool *)msg)->data ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
+}
+
+
+
+rcl_ret_t LedController::setup(rclc_support_t* support, rclc_executor_t* executor) {
+    RCL_RETURN_ON_ERROR(rclc_node_init_default(
+        &LedController::node, 
+        "led_controller",
+        "",
+        support));
+
+    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
+        &LedController::led0_subscription,
+        &LedController::node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+        "/nucleo/led0"));
+       
+    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
+        &LedController::led1_subscription,
+        &LedController::node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+        "/nucleo/led1"));
+
+    RCL_RETURN_ON_ERROR(rclc_subscription_init_default(
+        &LedController::led2_subscription,
+        &LedController::node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
+        "/nucleo/led2"));
+
+    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
+        executor,
+        &LedController::led0_subscription,
+        &led_msg,
+        led0_callback,
+        ON_NEW_DATA
+    ));
+
+    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
+        executor,
+        &LedController::led1_subscription,
+        &led_msg,
+        led1_callback,
+        ON_NEW_DATA
+    ));
+
+    RCL_RETURN_ON_ERROR(rclc_executor_add_subscription(
+        executor,
+        &LedController::led2_subscription,
+        &led_msg,
+        led2_callback,
+        ON_NEW_DATA
+    ));
+
+    return RCL_RET_OK;
+}
+
+
+void LedController::kill() {
+    rcl_subscription_fini(&LedController::led0_subscription, &LedController::node);
+    rcl_subscription_fini(&LedController::led1_subscription, &LedController::node);
+    rcl_subscription_fini(&LedController::led2_subscription, &LedController::node);
+    rcl_node_fini(&LedController::node);
+}