chore: initial commit

2026-06-25 00:34:34 +08:00
commit 3cce1be162
295 changed files with 30971 additions and 0 deletions
@@ -0,0 +1,257 @@
+/*
+ * SPDX-FileCopyrightText: 2024-2026 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "domain.h"
+
+#include "domain_internal.h"
+
+#include "esp_log.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+static const char *TAG = "domain_cal";
+
+#define DOMAIN_CALIBRATION_TIMEOUT_MS      20000
+#define DOMAIN_CALIBRATION_STEP_TIMEOUT_MS 1500
+#define DOMAIN_CALIBRATION_RELEASE_SWEEP_DEG 45.0f
+
+static float clamp_positive_default(float value, float default_value)
+{
+    if (value > 0.0f) {
+        return value;
+    }
+    return default_value;
+}
+
+static esp_err_t prepare_calibration_switch(const domain_direction_calibration_t *calibration)
+{
+    if (calibration == NULL || !calibration->enabled) {
+        return ESP_ERR_INVALID_STATE;
+    }
+
+    return domain_runtime_calibration_switch_init();
+}
+
+static bool is_calibration_switch_active(const domain_direction_calibration_t *calibration)
+{
+    if (calibration == NULL || !calibration->enabled) {
+        return false;
+    }
+
+    return domain_runtime_calibration_switch_is_active();
+}
+
+static bool calibration_timeout_reached(TickType_t start_tick, TickType_t timeout_ticks)
+{
+    return (xTaskGetTickCount() - start_tick) >= timeout_ticks;
+}
+
+static int calibration_remaining_timeout_ms(TickType_t start_tick, TickType_t timeout_ticks)
+{
+    TickType_t elapsed_ticks = xTaskGetTickCount() - start_tick;
+    if (elapsed_ticks >= timeout_ticks) {
+        return 0;
+    }
+
+    TickType_t remaining_ticks = timeout_ticks - elapsed_ticks;
+    int remaining_ms = (int)(remaining_ticks * portTICK_PERIOD_MS);
+    return remaining_ms > 0 ? remaining_ms : 1;
+}
+
+static int calibration_step_timeout_ms(TickType_t start_tick, TickType_t timeout_ticks)
+{
+    int remaining_ms = calibration_remaining_timeout_ms(start_tick, timeout_ticks);
+    if (remaining_ms <= 0) {
+        return 0;
+    }
+    return remaining_ms < DOMAIN_CALIBRATION_STEP_TIMEOUT_MS ?
+           remaining_ms :
+           DOMAIN_CALIBRATION_STEP_TIMEOUT_MS;
+}
+
+static bool calibration_settle(const domain_direction_calibration_t *calibration,
+                               TickType_t start_tick,
+                               TickType_t timeout_ticks)
+{
+    if (calibration != NULL && calibration->settle_delay_ms > 0) {
+        int remaining_ms = calibration_remaining_timeout_ms(start_tick, timeout_ticks);
+        if (remaining_ms <= 0) {
+            return false;
+        }
+
+        int delay_ms = calibration->settle_delay_ms < remaining_ms ?
+                       calibration->settle_delay_ms :
+                       remaining_ms;
+        TickType_t ticks = pdMS_TO_TICKS(delay_ms);
+        vTaskDelay(ticks > 0 ? ticks : 1);
+    }
+    return !calibration_timeout_reached(start_tick, timeout_ticks);
+}
+
+static esp_err_t calibration_rotate_and_track(float delta, int rotate_speed_us, int timeout_ms)
+{
+    if (timeout_ms <= 0) {
+        return ESP_ERR_TIMEOUT;
+    }
+
+    esp_err_t err = domain_runtime_stepper_rotate_angle_with_accel_timeout(
+            delta,
+            rotate_speed_us,
+            timeout_ms);
+    if (err != ESP_OK) {
+        return err;
+    }
+
+    return domain_apply_rotation_delta(delta);
+}
+
+static esp_err_t calibration_release_active_switch(const domain_direction_calibration_t *calibration,
+                                                   float step_angle,
+                                                   float search_direction,
+                                                   int rotate_speed_us,
+                                                   TickType_t start_tick,
+                                                   TickType_t timeout_ticks)
+{
+    float release_sweep_limit = clamp_positive_default(DOMAIN_CALIBRATION_RELEASE_SWEEP_DEG,
+                                                       step_angle);
+    float released_sweep = 0.0f;
+    float release_delta = -search_direction * step_angle;
+
+    while (released_sweep < release_sweep_limit) {
+        if (!is_calibration_switch_active(calibration)) {
+            ESP_LOGI(TAG, "Calibration switch released after %.1f deg", released_sweep);
+            return ESP_OK;
+        }
+
+        if (calibration_timeout_reached(start_tick, timeout_ticks)) {
+            return ESP_ERR_TIMEOUT;
+        }
+
+        int step_timeout_ms = calibration_step_timeout_ms(start_tick, timeout_ticks);
+        esp_err_t err = calibration_rotate_and_track(release_delta, rotate_speed_us, step_timeout_ms);
+        if (err != ESP_OK) {
+            return err;
+        }
+
+        released_sweep += step_angle;
+        if (!calibration_settle(calibration, start_tick, timeout_ticks)) {
+            return ESP_ERR_TIMEOUT;
+        }
+    }
+
+    if (is_calibration_switch_active(calibration)) {
+        ESP_LOGW(TAG,
+                 "Calibration switch stayed active after %.1f deg release sweep",
+                 released_sweep);
+        return ESP_ERR_INVALID_STATE;
+    }
+
+    ESP_LOGI(TAG, "Calibration switch released after %.1f deg", released_sweep);
+    return ESP_OK;
+}
+
+esp_err_t domain_calibrate_with_timeout(const domain_direction_calibration_t *config, int timeout_ms)
+{
+    int effective_timeout_ms = timeout_ms > 0 ? timeout_ms : DOMAIN_CALIBRATION_TIMEOUT_MS;
+    TickType_t timeout_ticks = pdMS_TO_TICKS(effective_timeout_ms);
+    if (timeout_ticks == 0) {
+        timeout_ticks = 1;
+    }
+    TickType_t start_tick = xTaskGetTickCount();
+
+    if (!domain_direction_lock_state(effective_timeout_ms)) {
+        return ESP_ERR_TIMEOUT;
+    }
+    if (config != NULL) {
+        domain_set_calibration(config);
+    }
+
+    domain_direction_calibration_t calibration = {0};
+    if (!domain_get_calibration(&calibration)) {
+        ESP_LOGW(TAG, "Calibration not configured");
+        domain_direction_unlock_state();
+        return ESP_ERR_INVALID_STATE;
+    }
+
+    if (!calibration.enabled) {
+        ESP_LOGW(TAG, "Calibration disabled");
+        domain_direction_unlock_state();
+        return ESP_ERR_INVALID_STATE;
+    }
+
+    esp_err_t err = prepare_calibration_switch(&calibration);
+    if (err != ESP_OK) {
+        ESP_LOGE(TAG, "Prepare calibration switch failed");
+        domain_direction_unlock_state();
+        return err;
+    }
+
+    float step_angle = clamp_positive_default(calibration.step_angle_deg, 5.0f);
+    float max_sweep = clamp_positive_default(calibration.max_sweep_deg, 360.0f);
+    float total_sweep = 0.0f;
+    float direction = calibration.search_clockwise ? 1.0f : -1.0f;
+    int rotate_speed_us = domain_get_rotate_speed_us();
+
+    ESP_LOGI(TAG,
+             "Calibration start step=%.1f sweep=%.1f search=%s",
+             step_angle,
+             max_sweep,
+             calibration.search_clockwise ? "clockwise" : "counterclockwise");
+
+    if (is_calibration_switch_active(&calibration)) {
+        ESP_LOGI(TAG, "Calibration switch active at start, releasing before homing");
+        err = calibration_release_active_switch(&calibration,
+                                                step_angle,
+                                                direction,
+                                                rotate_speed_us,
+                                                start_tick,
+                                                timeout_ticks);
+        if (err != ESP_OK) {
+            domain_runtime_stepper_power_off();
+            domain_direction_unlock_state();
+            return err;
+        }
+    }
+
+    while (total_sweep < max_sweep) {
+        if (calibration_timeout_reached(start_tick, timeout_ticks)) {
+            domain_runtime_stepper_power_off();
+            domain_direction_unlock_state();
+            ESP_LOGW(TAG, "Calibration timed out after %.1f deg search", total_sweep);
+            return ESP_ERR_TIMEOUT;
+        }
+
+        float delta = direction * step_angle;
+        int step_timeout_ms = calibration_step_timeout_ms(start_tick, timeout_ticks);
+        err = calibration_rotate_and_track(delta, rotate_speed_us, step_timeout_ms);
+        if (err != ESP_OK) {
+            domain_runtime_stepper_power_off();
+            domain_direction_unlock_state();
+            return err;
+        }
+
+        total_sweep += step_angle;
+        if (!calibration_settle(&calibration, start_tick, timeout_ticks)) {
+            domain_runtime_stepper_power_off();
+            domain_direction_unlock_state();
+            ESP_LOGW(TAG, "Calibration timed out during settle after %.1f deg search", total_sweep);
+            return ESP_ERR_TIMEOUT;
+        }
+
+        if (is_calibration_switch_active(&calibration)) {
+            domain_set_current_angle(calibration.calibration_angle);
+            domain_runtime_stepper_power_off();
+            domain_direction_unlock_state();
+            ESP_LOGI(TAG, "Calibration switch detected after %.1f deg", total_sweep);
+            return ESP_OK;
+        }
+    }
+
+    domain_runtime_stepper_power_off();
+    domain_direction_unlock_state();
+    ESP_LOGW(TAG, "Calibration sweep exhausted after %.1f deg", total_sweep);
+    return ESP_ERR_TIMEOUT;
+}
@@ -0,0 +1,43 @@
+/*
+ * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "domain_internal.h"
+
+#include <string.h>
+#include "freertos/FreeRTOS.h"
+#include "freertos/portmacro.h"
+
+static domain_direction_calibration_t s_calibration = {0};
+static bool s_calibration_configured = false;
+static portMUX_TYPE s_calibration_lock = portMUX_INITIALIZER_UNLOCKED;
+
+void domain_calibration_store_set(const domain_direction_calibration_t *config)
+{
+    portENTER_CRITICAL(&s_calibration_lock);
+    if (config == NULL) {
+        memset(&s_calibration, 0, sizeof(s_calibration));
+        s_calibration_configured = false;
+        portEXIT_CRITICAL(&s_calibration_lock);
+        return;
+    }
+
+    s_calibration = *config;
+    s_calibration_configured = true;
+    portEXIT_CRITICAL(&s_calibration_lock);
+}
+
+bool domain_calibration_store_get(domain_direction_calibration_t *out_config)
+{
+    portENTER_CRITICAL(&s_calibration_lock);
+    if (!s_calibration_configured || out_config == NULL) {
+        portEXIT_CRITICAL(&s_calibration_lock);
+        return false;
+    }
+
+    *out_config = s_calibration;
+    portEXIT_CRITICAL(&s_calibration_lock);
+    return true;
+}