feat: add calibration switch support

components/direction_controller/direction_controller.c

@@ -6,6 +6,9 @@
 
 #include <ctype.h>
 #include <string.h>
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "driver/gpio.h"
 #include "esp_log.h"
 #include "stepper_motor.h"
 #include "direction_controller.h"
@@ -29,6 +32,8 @@ static size_t s_direction_count = sizeof(s_default_directions) / sizeof(s_defaul
 static float s_current_angle = 0.0f;
 static int s_rotate_speed_us = STEPPER_SPEED_FAST;
 static bool s_initialized = false;
+static direction_controller_calibration_t s_calibration = {0};
+static bool s_calibration_configured = false;
 
 static void trimmed_span(const char *input, const char **start_out, size_t *len_out)
 {
@@ -88,6 +93,17 @@ static bool equals_trimmed(const char *input, const char *token, bool case_insen
     return true;
 }
 
+static float normalize_angle(float angle)
+{
+    while (angle > 180.0f) {
+        angle -= 360.0f;
+    }
+    while (angle <= -180.0f) {
+        angle += 360.0f;
+    }
+    return angle;
+}
+
 static float calculate_shortest_rotation(float current, float target)
 {
     float diff = target - current;
@@ -120,11 +136,14 @@ void direction_controller_init(const direction_controller_config_t *config)
             rotate_speed_us = config->rotate_speed_us;
         }
         init_stepper_gpio = config->init_stepper_gpio;
+        if (config->calibration != NULL) {
+            direction_controller_set_calibration(config->calibration);
+        }
     }
 
     s_directions = directions;
     s_direction_count = direction_count;
-    s_current_angle = initial_angle;
+    s_current_angle = normalize_angle(initial_angle);
     s_rotate_speed_us = rotate_speed_us;
 
     if (init_stepper_gpio) {
@@ -152,6 +171,12 @@ direction_controller_cmd_t direction_controller_parse_command(const char *input,
         return DIRECTION_CONTROLLER_CMD_POSITION;
     }
 
+    if (equals_trimmed(input, "cal", true) ||
+        equals_trimmed(input, "calibrate", true) ||
+        equals_trimmed(input, "校准", false)) {
+        return DIRECTION_CONTROLLER_CMD_CALIBRATE;
+    }
+
     const direction_controller_direction_t *direction = direction_controller_find_direction(input);
     if (direction != NULL) {
         if (out_direction != NULL) {
@@ -185,7 +210,8 @@ esp_err_t direction_controller_rotate_to_angle(float target_angle)
         direction_controller_init(NULL);
     }
 
-    float rotation = calculate_shortest_rotation(s_current_angle, target_angle);
+    float normalized_target = normalize_angle(target_angle);
+    float rotation = calculate_shortest_rotation(s_current_angle, normalized_target);
 
     if (rotation == 0.0f) {
         ESP_LOGI(TAG, "Already at target position");
@@ -193,10 +219,10 @@ esp_err_t direction_controller_rotate_to_angle(float target_angle)
     }
 
     ESP_LOGI(TAG, "Rotating from %.1f° to %.1f° (rotation: %.1f°)",
-             s_current_angle, target_angle, rotation);
+             s_current_angle, normalized_target, rotation);
 
     stepper_rotate_angle_with_accel(rotation, s_rotate_speed_us);
-    s_current_angle = target_angle;
+    s_current_angle = normalized_target;
     stepper_motor_power_off();
 
     ESP_LOGI(TAG, "Rotation complete, current position: %.1f°", s_current_angle);
@@ -221,7 +247,7 @@ float direction_controller_get_current_angle(void)
 
 void direction_controller_set_current_angle(float angle)
 {
-    s_current_angle = angle;
+    s_current_angle = normalize_angle(angle);
 }
 
 const direction_controller_direction_t *direction_controller_get_current_direction(void)
@@ -260,3 +286,125 @@ void direction_controller_set_rotate_speed_us(int rotate_speed_us)
         s_rotate_speed_us = rotate_speed_us;
     }
 }
+
+static esp_err_t configure_switch_gpio(const direction_controller_calibration_t *config)
+{
+    if (config == NULL) {
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    if (config->gpio_num < 0 || config->gpio_num >= GPIO_NUM_MAX) {
+        return ESP_ERR_INVALID_ARG;
+    }
+
+    gpio_config_t io_conf = {
+        .pin_bit_mask = (1ULL << config->gpio_num),
+        .mode = GPIO_MODE_INPUT,
+        .pull_up_en = config->pullup_en ? GPIO_PULLUP_ENABLE : GPIO_PULLUP_DISABLE,
+        .pull_down_en = config->pulldown_en ? GPIO_PULLDOWN_ENABLE : GPIO_PULLDOWN_DISABLE,
+        .intr_type = GPIO_INTR_DISABLE
+    };
+
+    return gpio_config(&io_conf);
+}
+
+static bool is_switch_active(const direction_controller_calibration_t *config)
+{
+    int level = gpio_get_level(config->gpio_num);
+    int active = (config->active_level != 0) ? 1 : 0;
+    return level == active;
+}
+
+static float clamp_positive_default(float value, float default_value)
+{
+    if (value > 0.0f) {
+        return value;
+    }
+    return default_value;
+}
+
+esp_err_t direction_controller_calibrate(const direction_controller_calibration_t *config)
+{
+    const direction_controller_calibration_t *cfg = config;
+
+    if (!s_initialized) {
+        direction_controller_init(NULL);
+    }
+
+    if (cfg == NULL) {
+        if (!s_calibration_configured) {
+            ESP_LOGW(TAG, "Calibration not configured");
+            return ESP_ERR_INVALID_STATE;
+        }
+        cfg = &s_calibration;
+    }
+
+    if (!cfg->enabled) {
+        ESP_LOGW(TAG, "Calibration disabled");
+        return ESP_ERR_INVALID_STATE;
+    }
+
+    esp_err_t err = configure_switch_gpio(cfg);
+    if (err != ESP_OK) {
+        ESP_LOGE(TAG, "Failed to configure calibration switch GPIO");
+        return err;
+    }
+
+    if (is_switch_active(cfg)) {
+        s_current_angle = normalize_angle(cfg->calibration_angle);
+        stepper_motor_power_off();
+        ESP_LOGI(TAG, "Calibration switch active, angle set to %.1f°", s_current_angle);
+        return ESP_OK;
+    }
+
+    float step_angle = clamp_positive_default(cfg->step_angle_deg, 5.0f);
+    float max_sweep = clamp_positive_default(cfg->max_sweep_deg, 360.0f);
+    float total_sweep = 0.0f;
+    float direction = cfg->search_cw ? 1.0f : -1.0f;
+    int settle_delay_ms = cfg->settle_delay_ms;
+
+    ESP_LOGI(TAG, "Calibration started (step=%.1f°, max=%.1f°, dir=%s)",
+             step_angle, max_sweep, cfg->search_cw ? "CW" : "CCW");
+
+    while (total_sweep < max_sweep) {
+        float delta = direction * step_angle;
+        stepper_rotate_angle_with_accel(delta, s_rotate_speed_us);
+        s_current_angle = normalize_angle(s_current_angle + delta);
+        total_sweep += step_angle;
+
+        if (settle_delay_ms > 0) {
+            vTaskDelay(pdMS_TO_TICKS(settle_delay_ms));
+        }
+
+        if (is_switch_active(cfg)) {
+            s_current_angle = normalize_angle(cfg->calibration_angle);
+            stepper_motor_power_off();
+            ESP_LOGI(TAG, "Calibration complete, angle set to %.1f°", s_current_angle);
+            return ESP_OK;
+        }
+    }
+
+    stepper_motor_power_off();
+    ESP_LOGW(TAG, "Calibration failed: switch not triggered within %.1f°", max_sweep);
+    return ESP_ERR_TIMEOUT;
+}
+
+void direction_controller_set_calibration(const direction_controller_calibration_t *config)
+{
+    if (config == NULL) {
+        s_calibration_configured = false;
+        memset(&s_calibration, 0, sizeof(s_calibration));
+        return;
+    }
+
+    s_calibration = *config;
+    s_calibration_configured = true;
+}
+
+const direction_controller_calibration_t *direction_controller_get_calibration(void)
+{
+    if (!s_calibration_configured) {
+        return NULL;
+    }
+    return &s_calibration;
+}

components/direction_controller/include/direction_controller.h

@@ -8,6 +8,7 @@
 
 #include <stdbool.h>
 #include <stddef.h>
+#include "driver/gpio.h"
 #include "esp_err.h"
 
 #ifdef __cplusplus
@@ -33,8 +34,25 @@ typedef struct {
     float initial_angle;                                /* Initial angle relative to South */
     int rotate_speed_us;                                /* Rotation speed (microseconds per step) */
     bool init_stepper_gpio;                             /* Initialize stepper GPIO in init */
+    const struct direction_controller_calibration_t *calibration; /* Optional calibration config */
 } direction_controller_config_t;
 
+/**
+ * @brief Calibration switch configuration
+ */
+typedef struct direction_controller_calibration_t {
+    bool enabled;                   /* Enable calibration */
+    gpio_num_t gpio_num;            /* Switch GPIO */
+    int active_level;               /* Active logic level (0 or 1) */
+    bool pullup_en;                 /* Enable internal pull-up (if supported) */
+    bool pulldown_en;               /* Enable internal pull-down (if supported) */
+    float calibration_angle;        /* Angle to set when switch is active */
+    float step_angle_deg;           /* Step angle per search iteration */
+    float max_sweep_deg;            /* Maximum total sweep angle */
+    bool search_cw;                 /* Search direction: true=clockwise, false=counterclockwise */
+    int settle_delay_ms;            /* Delay after each step to allow switch settle */
+} direction_controller_calibration_t;
+
 /**
  * @brief Parsed command types
  */
@@ -42,7 +60,8 @@ typedef enum {
     DIRECTION_CONTROLLER_CMD_UNKNOWN = 0,
     DIRECTION_CONTROLLER_CMD_HELP,
     DIRECTION_CONTROLLER_CMD_POSITION,
-    DIRECTION_CONTROLLER_CMD_DIRECTION
+    DIRECTION_CONTROLLER_CMD_DIRECTION,
+    DIRECTION_CONTROLLER_CMD_CALIBRATE
 } direction_controller_cmd_t;
 
 /**
@@ -86,6 +105,14 @@ esp_err_t direction_controller_rotate_to_angle(float target_angle);
  */
 esp_err_t direction_controller_rotate_to_direction(const char *input);
 
+/**
+ * @brief Calibrate direction using a switch
+ *
+ * @param config Optional calibration configuration, NULL uses config set in init
+ * @return ESP_OK on success
+ */
+esp_err_t direction_controller_calibrate(const direction_controller_calibration_t *config);
+
 /**
  * @brief Get current angle
  */
@@ -121,6 +148,16 @@ int direction_controller_get_rotate_speed_us(void);
  */
 void direction_controller_set_rotate_speed_us(int rotate_speed_us);
 
+/**
+ * @brief Set calibration configuration
+ */
+void direction_controller_set_calibration(const direction_controller_calibration_t *config);
+
+/**
+ * @brief Get calibration configuration (NULL if not configured)
+ */
+const direction_controller_calibration_t *direction_controller_get_calibration(void);
+
 #ifdef __cplusplus
 }
 #endif

main/main.c

@@ -48,6 +48,7 @@ static void print_help(void)
     printf("命令 (Commands):\n");
     printf("  help - 显示帮助信息\n");
     printf("  pos  - 显示当前位置\n");
+    printf("  cal  - 校准正北位置\n");
     printf("========================================\n\n");
 }
 
@@ -119,6 +120,21 @@ static void direction_control_task(void *arg)
                         case DIRECTION_CONTROLLER_CMD_POSITION:
                             print_current_position();
                             break;
+                        case DIRECTION_CONTROLLER_CMD_CALIBRATE: {
+                            printf("开始校准...\n");
+                            esp_err_t err = direction_controller_calibrate(NULL);
+                            if (err == ESP_OK) {
+                                printf("校准完成\n");
+                                print_current_position();
+                            } else if (err == ESP_ERR_TIMEOUT) {
+                                printf("校准失败: 未检测到微动开关\n");
+                            } else if (err == ESP_ERR_INVALID_STATE) {
+                                printf("校准失败: 未配置校准开关\n");
+                            } else {
+                                printf("校准失败: 错误码 %d\n", err);
+                            }
+                            break;
+                        }
                         case DIRECTION_CONTROLLER_CMD_DIRECTION:
                             if (dir != NULL) {
                                 printf("目标方向: %s (%s, %.1f°)\n",
@@ -181,7 +197,27 @@ void app_main(void)
     ESP_LOGI(TAG, "========================================");
     
     /* Initialize peripherals */
-    direction_controller_init(NULL);
+    direction_controller_calibration_t calibration = {
+        .enabled = true,
+        .gpio_num = GPIO_NUM_39,
+        .active_level = 0,
+        .pullup_en = false,
+        .pulldown_en = false,
+        .calibration_angle = 180.0f,
+        .step_angle_deg = 5.0f,
+        .max_sweep_deg = 360.0f,
+        .search_cw = true,
+        .settle_delay_ms = 10
+    };
+    direction_controller_config_t config = {
+        .directions = NULL,
+        .direction_count = 0,
+        .initial_angle = 0.0f,
+        .rotate_speed_us = 0,
+        .init_stepper_gpio = true,
+        .calibration = &calibration
+    };
+    direction_controller_init(&config);
     uart_init();
     
     ESP_LOGI(TAG, "Initial position: South (0°)");