added retained calibration

.vscode/launch.json

@@ -9,7 +9,7 @@
             "showDevDebugOutput": "none",
             "servertype": "external",
             "gdbTarget": "localhost:3333",
-            "targetId": "nrf54l",
+            "targetId": "stm32h5",
         }
     ]
 }

boards/nucleo_h563zi.overlay

@@ -4,10 +4,10 @@
     };
 
     chosen {
+        uros,status_led= &green_led_1;
+        uros,error_led = &red_led_1;
 		pid,counter = &counter5;
-        pid,active_led = &green_led_1;
         pid,offset_led = &yellow_led_1;
-        pid,calibration_led = &red_led_1;
         pid,rotenc = &as5600;
         pid,stepper = &stepper0;
 	};
@@ -39,6 +39,15 @@
     status = "okay";
 };
 
+&timers5 {
+    status = "okay";
+    st,prescaler = <7>;
+
+    counter5: counter {
+        status = "okay";
+    };
+};
+
 &timers6 {
     status = "okay";
     st,prescaler = <25>;
@@ -48,11 +57,11 @@
     };
 };
 
-&timers5 {
+&timers7 {
     status = "okay";
-    st,prescaler = <7>;
+    st,prescaler = <25>;
 
-    counter5: counter {
+    counter7: counter {
         status = "okay";
     };
 };

prj.conf

@@ -35,8 +35,12 @@ CONFIG_STEPPER=y
 CONFIG_STEPPER_ADI_TMC2209=y
 CONFIG_STEPPER_ADI_TMC=y
 
+# reboot
+CONFIG_REBOOT=y
+
 # builld
 CONFIG_NO_OPTIMIZATIONS=n
 CONFIG_SIZE_OPTIMIZATIONS=n
-CONFIG_SPEED_OPTIMIZATIONS=y
+CONFIG_SPEED_OPTIMIZATIONS=n
-CONFIG_DEBUG_OPTIMIZATIONS=n
+CONFIG_DEBUG_OPTIMIZATIONS=y
+CONFIG_DEBUG_THREAD_INFO=y

src/devices.cpp

@@ -4,19 +4,19 @@
 bool verify_are_devices_available(void) {
     bool devices_ready = true;
 
-    if (!device_is_ready(status_led.port)) {
+    if (!device_is_ready(uros_connection_status_led.port)) {
-        printk("Status LED not ready\n");
+        printk("Microros connection status LED not ready\n");
         devices_ready = false;
     }
 
-    if (!device_is_ready(work_led.port)) {
+    if (!device_is_ready(pid_offset_led.port)) {
-        printk("LED0 not ready\n");
+        printk("PID offset LED not ready\n");
         devices_ready = false;
     }
 
 
-    if (!device_is_ready(calibration_led.port)) {
+    if (!device_is_ready(uros_error_led.port)) {
-        printk("Calibration LED not ready\n");
+        printk("Microros error LED not ready\n");
         devices_ready = false;
     }
 

src/devices.hpp

@@ -12,9 +12,9 @@
 #include <devicetree.h>
 #endif
 
-static const struct gpio_dt_spec status_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_active_led), gpios);
+static const struct gpio_dt_spec uros_connection_status_led = GPIO_DT_SPEC_GET(DT_CHOSEN(uros_status_led), gpios);
-static const struct gpio_dt_spec work_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_offset_led), gpios);
+static const struct gpio_dt_spec pid_offset_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_offset_led), gpios);
-static const struct gpio_dt_spec calibration_led = GPIO_DT_SPEC_GET(DT_CHOSEN(pid_calibration_led), gpios);
+static const struct gpio_dt_spec uros_error_led = GPIO_DT_SPEC_GET(DT_CHOSEN(uros_error_led), gpios);
 
 static const struct device *rotenc0 = DEVICE_DT_GET(DT_CHOSEN(pid_rotenc));
 static const struct device *stepper0 = DEVICE_DT_GET(DT_CHOSEN(pid_stepper));

src/main.cpp

@@ -34,6 +34,8 @@ int main(void)
         zephyr_transport_read
     );
 
+    gpio_pin_configure_dt(&uros_connection_status_led, GPIO_OUTPUT_INACTIVE);
+
     while (1) {
         micro_ros_state_machine();
     }

src/microros/microros_state_machine.cpp

@@ -1,8 +1,10 @@
 #include "microros_state_machine.hpp"
 
+#include "devices.hpp"
 #include "stepper_controller.hpp"
 
 #include <zephyr/kernel.h>
+#include <zephyr/sys/reboot.h>
 #include <rcl/rcl.h>
 #include <rclc/rclc.h>
 #include <rclc/executor.h>
@@ -26,6 +28,7 @@ void micro_ros_state_machine() {
     static rclc_executor_t executor;
     switch (micro_ros_state) {
         case ROS_AGENT_CONNECTION_UNINITIALIZED:
+            gpio_pin_configure_dt(&uros_connection_status_led, GPIO_OUTPUT_INACTIVE);
             allocator = rcl_get_default_allocator();
             printk("allocator initialized!\r\n");
             micro_ros_state = ROS_AGENT_CONNECTION_DISCONNECTED;
@@ -34,9 +37,10 @@ void micro_ros_state_machine() {
             StepperController::kill();
             rclc_executor_fini(&executor);
             rclc_support_fini(&support);
+
             if (rclc_support_init(&support, 0, NULL, &allocator) != RCL_RET_OK) {
                 printk("Can't init rcl support!\r\n");
-                k_msleep(1000);
+                k_msleep(200);
                 break;
             }
             if (rclc_executor_init(&executor, &support.context, 8, &allocator) != RCL_RET_OK){
@@ -55,12 +59,14 @@ void micro_ros_state_machine() {
             micro_ros_state = ROS_AGENT_CONNECTION_CONNECTED;
 
         case ROS_AGENT_CONNECTION_UNKNOWN:
-            if (rmw_uros_ping_agent(150, 3) != RCL_RET_OK) {
+            if (rmw_uros_ping_agent(100, 1) != RCL_RET_OK) {
-                micro_ros_state = ROS_AGENT_CONNECTION_DISCONNECTED;
+                micro_ros_state = ROS_AGENT_CONNECTION_ERROR;
+                gpio_pin_configure_dt(&uros_connection_status_led, GPIO_OUTPUT_INACTIVE);
                 printk("micro ros agent not reachable!\r\n");
                 break;
             }
             micro_ros_state = ROS_AGENT_CONNECTION_CONNECTED;
+            gpio_pin_configure_dt(&uros_connection_status_led, GPIO_OUTPUT_ACTIVE);
             printk("micro ros agent reachable!\r\n");
             rmw_uros_sync_session(100);
 
@@ -83,5 +89,10 @@ void micro_ros_state_machine() {
         case ROS_AGENT_CONNECTION_ERROR:
         default:
             micro_ros_state = ROS_AGENT_CONNECTION_ERROR;
+            gpio_pin_configure_dt(&uros_error_led, GPIO_OUTPUT_ACTIVE);
+
+            k_sleep(K_MSEC(3000));
+
+            sys_reboot(SYS_REBOOT_WARM);
     }
 }

src/stepper_controller/stepper_controller.cpp

@@ -22,6 +22,8 @@
 
 rcl_node_t StepperController::node;
 
+__attribute__((section(".noinit"))) float retained_calibration = 0.0f;
+
 // target pos
 rcl_subscription_t StepperController::stepper0_sub_pos;
 float StepperController::current_angle;
@@ -102,38 +104,45 @@ void StepperController::init() {
 }
 
 void StepperController::calibrate() {
-    gpio_pin_configure_dt(&status_led, GPIO_OUTPUT_INACTIVE);
+    printk("Calibrating stepper motor ...\n");
-    gpio_pin_configure_dt(&calibration_led, GPIO_OUTPUT_ACTIVE);
+
-    set_motor_velocity(0.02);
+    if (fabs(retained_calibration) == 1.0) { // NaN check
-    k_sleep(K_MSEC(1000));
+        direction_factor = retained_calibration;
+        printk("Cached calibration -> %c\n" , direction_factor > 0 ? '+' : '-');
+        return;
+    }
+
+    
+    set_motor_velocity(0.05);
+    k_sleep(K_MSEC(500));
 
     sample_angle();
     float left_angle = current_angle;
 
-    set_motor_velocity(-0.02);
+    set_motor_velocity(-0.05);
-    k_sleep(K_MSEC(2000));
+    k_sleep(K_MSEC(1000));
 
     sample_angle();
     float right_angle = current_angle; 
 
-    set_motor_velocity(0.02);
+    set_motor_velocity(0.05);
-    k_sleep(K_MSEC(1000));
+    k_sleep(K_MSEC(500));
 
     if (right_angle < left_angle) {
         direction_factor = -1.0;
     }
+
+    stepper_stop(stepper0);
     
-    printk("Servo direction calibration:\n");
+    printk("Stepper direction calibration:\n");
     printk("Left  = %d.%02d\n", (int)left_angle, abs((int)(left_angle * 100) % 100));
     printk("Right = %d.%02d\n", (int)right_angle, abs((int)(right_angle * 100) % 100));
     printk("-> %c\n" , direction_factor > 0 ? '+' : '-');
     printk("----------------------------\n");
 
-    stepper_stop(stepper0);
+    retained_calibration = direction_factor;
-    k_sleep(K_MSEC(100));
 
-    gpio_pin_configure_dt(&status_led, GPIO_OUTPUT_ACTIVE);
+    k_sleep(K_MSEC(100));
-    gpio_pin_configure_dt(&calibration_led, GPIO_OUTPUT_INACTIVE);
 }
 
 rcl_ret_t StepperController::setup(rclc_support_t* support, rclc_executor_t* executor)
@@ -273,7 +282,7 @@ void StepperController::pid() {
 }
 
 void StepperController::set_motor_velocity(float rps) {
-    gpio_pin_configure_dt(&work_led, fabs(rps) > 0.05 ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
+    gpio_pin_configure_dt(&pid_offset_led, fabs(rps) >= 0.05 ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
 
     uint32_t motor_steps_hz = abs((int32_t)(rps * (float)SENSOR_STEPS_PER_REVOLUTION));