From d961fb253a56d0454891c293b2d32816f07eab99 Mon Sep 17 00:00:00 2001 From: Felipe Neves Date: Mon, 1 Jun 2026 19:16:36 -0300 Subject: [PATCH 1/2] Add FOC-in-the-loop simulated plant for FITL builds without HW drivers. Enables full FOC stack testing on QEMU and axis_tuning on C6 by replacing inverter, shunt ADC, and encoder with a PMSM plant and chip-specific tick backends. --- CMakeLists.txt | 96 ++-- Kconfig | 73 +++ examples/axis_tuning/main/main.c | 31 +- examples/axis_tuning/sdkconfig.defaults | 3 + .../test_encoder/main/test_encoder.c | 7 +- .../test_isensor_characterize/main/main.c | 7 +- .../unit_test_runner/run_unit_tests_qemu.py | 2 +- examples/unit_test_runner/sdkconfig.defaults | 4 + include/espFoC/esp_foc_in_the_loop.h | 47 ++ .../foc_in_the_loop/esp_foc_itl_tick.h | 23 + .../esp_foc_itl_tick_gptimer.c | 109 +++++ .../foc_in_the_loop/esp_foc_itl_tick_mcpwm.c | 99 ++++ source/motor_control/esp_foc_core.c | 7 + .../foc_in_the_loop/esp_foc_in_the_loop.c | 443 ++++++++++++++++++ .../foc_in_the_loop/esp_foc_itl_plant.c | 263 +++++++++++ .../foc_in_the_loop/esp_foc_itl_plant.h | 53 +++ test/test_axis_flow.c | 8 +- test/test_foc_itl_integration.c | 79 ++++ test/test_foc_itl_plant.c | 100 ++++ test/test_isensor_adc.c | 5 + test/test_rotor_sensor_simu.c | 5 + 21 files changed, 1403 insertions(+), 61 deletions(-) create mode 100644 include/espFoC/esp_foc_in_the_loop.h create mode 100644 source/drivers/foc_in_the_loop/esp_foc_itl_tick.h create mode 100644 source/drivers/foc_in_the_loop/esp_foc_itl_tick_gptimer.c create mode 100644 source/drivers/foc_in_the_loop/esp_foc_itl_tick_mcpwm.c create mode 100644 source/motor_control/foc_in_the_loop/esp_foc_in_the_loop.c create mode 100644 source/motor_control/foc_in_the_loop/esp_foc_itl_plant.c create mode 100644 source/motor_control/foc_in_the_loop/esp_foc_itl_plant.h create mode 100644 test/test_foc_itl_integration.c create mode 100644 test/test_foc_itl_plant.c diff --git a/CMakeLists.txt b/CMakeLists.txt index d156d7db..e04c43b5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,50 +17,65 @@ if(ESP_PLATFORM) idf_build_get_property(target IDF_TARGET) -set(requires esp_driver_uart esp_driver_pcnt esp_driver_gpio esp_driver_mcpwm driver freertos esp_system esp_timer nvs_flash esp_hw_support efuse) -# esp_tinyusb is fetched conditionally for USB-capable targets via -# idf_component.yml; declare the priv require eagerly here so the bridge -# source file finds tinyusb.h on those targets even when the user has not -# enabled CONFIG_ESP_FOC_BRIDGE_USBCDC at component-graph resolution time. -if(target STREQUAL "esp32s2" OR target STREQUAL "esp32s3" OR target STREQUAL "esp32p4") - list(APPEND requires espressif__esp_tinyusb) -endif() -list(APPEND includes "source/drivers" "source/drivers/cali") -list(APPEND srcs "source/drivers/inverter_3pwm_mcpwm.c" - "source/drivers/inverter_6pwm_mcpwm.c" - "source/drivers/rotor_sensor_as5600.c" - "source/drivers/rotor_sensor_as5048.c" - "source/drivers/rotor_sensor_pcnt.c" - "source/drivers/rotor_sensor_simu.c" - "source/drivers/current_sensor_adc.c" - "source/drivers/esp_foc_adc_cali_lut.c" - "source/drivers/cali/esp_foc_adc_cali_curve.c" - "source/drivers/cali/esp_foc_adc_range_extend.c" - "source/osal/os_interface_idf.c") +set(requires driver freertos esp_system esp_timer nvs_flash esp_hw_support efuse) -if(target STREQUAL "esp32") - list(APPEND srcs "source/drivers/cali/esp_foc_adc_cali_line_esp32.c") -elseif(target STREQUAL "esp32s2") - list(APPEND srcs "source/drivers/cali/esp_foc_adc_cali_line_esp32s2.c") -elseif(target STREQUAL "esp32c2") - list(APPEND srcs "source/drivers/cali/esp_foc_adc_cali_line_esp32c2.c") -elseif(EXISTS "${CMAKE_CURRENT_LIST_DIR}/source/drivers/cali/coeff/esp_foc_curve_coeff_${target}.c") - list(APPEND srcs "source/drivers/cali/coeff/esp_foc_curve_coeff_${target}.c") -endif() +if(CONFIG_ESP_FOC_FITL) + list(APPEND srcs "source/motor_control/foc_in_the_loop/esp_foc_itl_plant.c" + "source/motor_control/foc_in_the_loop/esp_foc_in_the_loop.c") + list(APPEND includes "source/motor_control/foc_in_the_loop" + "source/drivers/foc_in_the_loop") + list(APPEND srcs "source/osal/os_interface_idf.c") + + if(CONFIG_FOC_ITL_TICK_MCPWM) + list(APPEND srcs "source/drivers/foc_in_the_loop/esp_foc_itl_tick_mcpwm.c") + list(APPEND requires esp_driver_mcpwm) + elseif(CONFIG_FOC_ITL_TICK_GPTIMER) + list(APPEND srcs "source/drivers/foc_in_the_loop/esp_foc_itl_tick_gptimer.c") + list(APPEND requires esp_driver_gptimer) + endif() +else() + list(APPEND requires esp_driver_uart esp_driver_pcnt esp_driver_gpio esp_driver_mcpwm) + list(APPEND includes "source/drivers" "source/drivers/cali") + list(APPEND srcs "source/drivers/inverter_3pwm_mcpwm.c" + "source/drivers/inverter_6pwm_mcpwm.c" + "source/drivers/rotor_sensor_as5600.c" + "source/drivers/rotor_sensor_as5048.c" + "source/drivers/rotor_sensor_pcnt.c" + "source/drivers/rotor_sensor_simu.c" + "source/drivers/current_sensor_adc.c" + "source/drivers/esp_foc_adc_cali_lut.c" + "source/drivers/cali/esp_foc_adc_cali_curve.c" + "source/drivers/cali/esp_foc_adc_range_extend.c" + "source/osal/os_interface_idf.c") -if(CONFIG_SOC_ETM_SUPPORTED) - list(APPEND srcs "source/drivers/isensor_adc_etm.c") + if(target STREQUAL "esp32") + list(APPEND srcs "source/drivers/cali/esp_foc_adc_cali_line_esp32.c") + elseif(target STREQUAL "esp32s2") + list(APPEND srcs "source/drivers/cali/esp_foc_adc_cali_line_esp32s2.c") + elseif(target STREQUAL "esp32c2") + list(APPEND srcs "source/drivers/cali/esp_foc_adc_cali_line_esp32c2.c") + elseif(EXISTS "${CMAKE_CURRENT_LIST_DIR}/source/drivers/cali/coeff/esp_foc_curve_coeff_${target}.c") + list(APPEND srcs "source/drivers/cali/coeff/esp_foc_curve_coeff_${target}.c") + endif() + + if(CONFIG_SOC_ETM_SUPPORTED) + list(APPEND srcs "source/drivers/isensor_adc_etm.c") + endif() + + if(target STREQUAL "esp32") + list(APPEND srcs "source/drivers/isensor_adc_dma_esp32.c") + list(APPEND requires esp_driver_i2s) + elseif(target STREQUAL "esp32s2") + list(APPEND srcs "source/drivers/isensor_adc_dma_esp32s2.c") + list(APPEND requires esp_driver_spi) + else() + list(APPEND srcs "source/drivers/isensor_adc_dma_gdma.c") + list(APPEND requires esp_mm) + endif() endif() -if(target STREQUAL "esp32") - list(APPEND srcs "source/drivers/isensor_adc_dma_esp32.c") - list(APPEND requires esp_driver_i2s) -elseif(target STREQUAL "esp32s2") - list(APPEND srcs "source/drivers/isensor_adc_dma_esp32s2.c") - list(APPEND requires esp_driver_spi) -else() - list(APPEND srcs "source/drivers/isensor_adc_dma_gdma.c") - list(APPEND requires esp_mm) +if(target STREQUAL "esp32s2" OR target STREQUAL "esp32s3" OR target STREQUAL "esp32p4") + list(APPEND requires espressif__esp_tinyusb) endif() if(CONFIG_ESP_FOC_TUNER_ENABLE) @@ -69,6 +84,7 @@ if(CONFIG_ESP_FOC_TUNER_ENABLE) endif() if(CONFIG_ESP_FOC_BRIDGE_UART) list(APPEND srcs "source/drivers/gui_link/esp_foc_bridge_uart.c") + list(APPEND requires esp_driver_uart) endif() if(CONFIG_ESP_FOC_BRIDGE_USBCDC) list(APPEND srcs "source/drivers/gui_link/esp_foc_bridge_usbcdc.c") diff --git a/Kconfig b/Kconfig index 1f4ae726..436eb324 100644 --- a/Kconfig +++ b/Kconfig @@ -186,4 +186,77 @@ menu "espFoC Settings" default y endmenu + menu "FOC-in-the-loop (simulated plant)" + config ESP_FOC_FITL + bool "FOC-in-the-loop: simulated motor plant (exclusive with HW drivers)" + default n + help + When enabled, espFoC builds the in-the-loop plant factory instead + of MCPWM inverter, ADC shunt, and hardware rotor drivers. + + if ESP_FOC_FITL + + config FOC_ITL_PWM_10KHZ + bool "Use 10 kHz PWM tick (default 20 kHz)" + default n + + config FOC_ITL_USE_RK2 + bool "Plant integrator: RK2 instead of RK4" + default n + + config FOC_ITL_MOTOR_DELTA + bool "Motor windings: delta (D1 star-equivalent conversion)" + default n + + choice FOC_ITL_TICK_SOURCE + prompt "PWM tick backend" + default FOC_ITL_TICK_MCPWM if SOC_MCPWM_SUPPORTED + default FOC_ITL_TICK_GPTIMER + + config FOC_ITL_TICK_MCPWM + bool "MCPWM timer (device)" + depends on SOC_MCPWM_SUPPORTED + + config FOC_ITL_TICK_GPTIMER + bool "GPTimer (QEMU / generic)" + endchoice + + config FOC_ITL_DEFAULT_R_MILLIOHM + int "Default phase R [milli-ohm]" + range 1 1000000 + default 1000 + + config FOC_ITL_DEFAULT_L_UH + int "Default phase L [micro-henry]" + range 1 1000000 + default 2000 + + config FOC_ITL_DEFAULT_J_X1E7 + int "Default inertia J * 1e7 [kg·m²]" + range 1 1000000 + default 500 + + config FOC_ITL_DEFAULT_B_X1E7 + int "Default viscous B * 1e7 [N·m·s/rad]" + range 0 1000000 + default 100 + + config FOC_ITL_DEFAULT_KT_X1E7 + int "Default torque constant Kt * 1e7 [N·m/A]" + range 1 1000000 + default 1000 + + config FOC_ITL_DEFAULT_POLE_PAIRS + int "Default pole pairs" + range 1 64 + default 13 + + config FOC_ITL_DEFAULT_I_MAX_MA + int "Default phase current limit [mA]" + range 100 100000 + default 10000 + + endif + endmenu + endmenu diff --git a/examples/axis_tuning/main/main.c b/examples/axis_tuning/main/main.c index 78371c98..e41e1074 100644 --- a/examples/axis_tuning/main/main.c +++ b/examples/axis_tuning/main/main.c @@ -11,11 +11,14 @@ #include "esp_log.h" #include "esp_err.h" -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" #include "espFoC/esp_foc.h" #include "espFoC/gui_link/esp_foc_tuner.h" +#include "espFoC/utils/esp_foc_q16.h" + +#if defined(CONFIG_ESP_FOC_FITL) && CONFIG_ESP_FOC_FITL +#include "espFoC/esp_foc_in_the_loop.h" +#else #include "espFoC/inverter_6pwm_mcpwm.h" #if defined(CONFIG_AXIS_TUNING_ROTOR_AS5600) #include "espFoC/rotor_sensor_as5600.h" @@ -23,8 +26,8 @@ #include "espFoC/rotor_sensor_simu.h" #endif #include "espFoC/current_sensor_adc.h" -#include "espFoC/utils/esp_foc_q16.h" #include "soc/soc_caps.h" +#endif static const char *TAG = "axis_tuning"; @@ -83,6 +86,7 @@ static void pump_scope_idle(void) } #endif +#if !defined(CONFIG_ESP_FOC_FITL) || !CONFIG_ESP_FOC_FITL static int pwm_enable_gpio(void) { if (CONFIG_AXIS_TUNING_PWM_EN_PIN < 0) { @@ -95,11 +99,27 @@ static int pwm_enable_gpio(void) #endif return CONFIG_AXIS_TUNING_PWM_EN_PIN; } +#endif void app_main(void) { ESP_LOGI(TAG, "boot — axis_tuning"); +#if defined(CONFIG_ESP_FOC_FITL) && CONFIG_ESP_FOC_FITL + esp_foc_in_the_loop_config_t fitl_cfg = { + .pole_pairs = CONFIG_AXIS_TUNING_POLE_PAIRS, + .vdc_q16 = q16_from_float((float)CONFIG_AXIS_TUNING_DC_LINK_V), + .pwm_hz = 0, + }; + esp_foc_in_the_loop_handles_t fitl = {0}; + if (esp_foc_in_the_loop_create(&fitl_cfg, &fitl) != ESP_FOC_OK) { + ESP_LOGE(TAG, "FITL init failed"); + return; + } + s_inverter = fitl.inverter; + s_rotor = fitl.rotor; + s_shunts = fitl.isensor; +#else s_inverter = inverter_6pwm_mpcwm_new( CONFIG_AXIS_TUNING_PWM_U_HI, CONFIG_AXIS_TUNING_PWM_U_LO, @@ -156,6 +176,7 @@ void app_main(void) ESP_LOGE(TAG, "current sensor init failed"); return; } +#endif esp_foc_motor_control_settings_t settings = { .motor_pole_pairs = CONFIG_AXIS_TUNING_POLE_PAIRS, @@ -170,7 +191,7 @@ void app_main(void) return; } -#if defined(CONFIG_AXIS_TUNING_ROTOR_SIMU) +#if defined(CONFIG_AXIS_TUNING_ROTOR_SIMU) && (!defined(CONFIG_ESP_FOC_FITL) || !CONFIG_ESP_FOC_FITL) rotor_sensor_simu_wire_ud_uq(s_rotor, &s_axis.u_d.raw, &s_axis.u_q.raw); #endif @@ -190,6 +211,6 @@ void app_main(void) pump_scope_idle(); } #endif - vTaskDelay(pdMS_TO_TICKS(200)); + esp_foc_sleep_ms(200); } } diff --git a/examples/axis_tuning/sdkconfig.defaults b/examples/axis_tuning/sdkconfig.defaults index 32f1637a..b9a3a73b 100644 --- a/examples/axis_tuning/sdkconfig.defaults +++ b/examples/axis_tuning/sdkconfig.defaults @@ -13,5 +13,8 @@ CONFIG_ESP_FOC_SCOPE_BUFFER_SIZE=512 CONFIG_ESP_FOC_CALIBRATION_NVS=y CONFIG_ESP_FOC_PWM_RATE_HZ=20000 +CONFIG_ESP_FOC_FITL=y +CONFIG_FOC_ITL_TICK_MCPWM=y + CONFIG_COMPILER_OPTIMIZATION_PERF=y CONFIG_ADC_CONTINUOUS_ISR_IRAM_SAFE=y diff --git a/examples/test_drivers/test_encoder/main/test_encoder.c b/examples/test_drivers/test_encoder/main/test_encoder.c index 42b50e08..4893be07 100644 --- a/examples/test_drivers/test_encoder/main/test_encoder.c +++ b/examples/test_drivers/test_encoder/main/test_encoder.c @@ -16,8 +16,6 @@ #include "esp_log.h" #include "esp_err.h" -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" #include "espFoC/esp_foc.h" #include "espFoC/gui_link/esp_foc_tuner.h" @@ -113,10 +111,9 @@ static void pump_scope_idle(void) static void encoder_task(void *arg) { (void)arg; - const TickType_t period = pdMS_TO_TICKS(1); while (1) { encoder_axis_poll(&s_axis); - vTaskDelay(period); + esp_foc_sleep_ms(1); } } @@ -170,6 +167,6 @@ void app_main(void) #if defined(CONFIG_ESP_FOC_SCOPE) pump_scope_idle(); #endif - vTaskDelay(pdMS_TO_TICKS(200)); + esp_foc_sleep_ms(200); } } diff --git a/examples/test_drivers/test_isensor_characterize/main/main.c b/examples/test_drivers/test_isensor_characterize/main/main.c index f210a1dc..ee11909e 100644 --- a/examples/test_drivers/test_isensor_characterize/main/main.c +++ b/examples/test_drivers/test_isensor_characterize/main/main.c @@ -19,8 +19,6 @@ #include "esp_log.h" #include "esp_err.h" -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" #include "espFoC/esp_foc.h" #include "espFoC/gui_link/esp_foc_tuner.h" @@ -81,12 +79,11 @@ static void pump_scope_idle(void) static void bench_task(void *arg) { (void)arg; - const TickType_t period = pdMS_TO_TICKS(1); while (1) { if (s_axis.state == ESP_FOC_AXIS_STATE_BENCH) { esp_foc_bench_step(&s_axis); } - vTaskDelay(period); + esp_foc_sleep_ms(1); } } @@ -168,6 +165,6 @@ void app_main(void) pump_scope_idle(); } #endif - vTaskDelay(pdMS_TO_TICKS(200)); + esp_foc_sleep_ms(200); } } diff --git a/examples/unit_test_runner/run_unit_tests_qemu.py b/examples/unit_test_runner/run_unit_tests_qemu.py index 0a1e6e38..e75e648f 100644 --- a/examples/unit_test_runner/run_unit_tests_qemu.py +++ b/examples/unit_test_runner/run_unit_tests_qemu.py @@ -116,7 +116,7 @@ def main() -> int: child.sendline(TEST_FILTER) try: - child.expect(RESULT_RE, timeout=600) + child.expect(RESULT_RE, timeout=120) except pexpect.TIMEOUT: print("Timeout waiting for Unity summary (tests still running or hung).") return 1 diff --git a/examples/unit_test_runner/sdkconfig.defaults b/examples/unit_test_runner/sdkconfig.defaults index 524ebfba..d2d5a215 100644 --- a/examples/unit_test_runner/sdkconfig.defaults +++ b/examples/unit_test_runner/sdkconfig.defaults @@ -6,3 +6,7 @@ CONFIG_ESP_FOC_TUNER_ENABLE=y # Enable NVS calibration overlay for unit tests CONFIG_ESP_FOC_CALIBRATION_NVS=y + +# FOC-in-the-loop plant + GPTimer tick for QEMU CI +CONFIG_ESP_FOC_FITL=y +CONFIG_FOC_ITL_TICK_GPTIMER=y diff --git a/include/espFoC/esp_foc_in_the_loop.h b/include/espFoC/esp_foc_in_the_loop.h new file mode 100644 index 00000000..8cc35894 --- /dev/null +++ b/include/espFoC/esp_foc_in_the_loop.h @@ -0,0 +1,47 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + */ +#pragma once + +#include +#include + +#include "espFoC/drivers/current_sensor_interface.h" +#include "espFoC/drivers/inverter_interface.h" +#include "espFoC/drivers/rotor_sensor_interface.h" +#include "espFoC/esp_foc_err.h" +#include "espFoC/utils/esp_foc_q16.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + float r_ohm; + float l_henry; + float j_kgm2; + float b_nms; + float kt_nm_per_a; + int pole_pairs; + q16_t vdc_q16; + float i_max_a; + bool locked_rotor; + bool connection_delta; + uint32_t pwm_hz; +} esp_foc_in_the_loop_config_t; + +typedef struct { + esp_foc_inverter_t *inverter; + esp_foc_isensor_t *isensor; + esp_foc_rotor_sensor_t *rotor; +} esp_foc_in_the_loop_handles_t; + +esp_foc_err_t esp_foc_in_the_loop_create(const esp_foc_in_the_loop_config_t *cfg, + esp_foc_in_the_loop_handles_t *out); +void esp_foc_in_the_loop_destroy(void); + +#ifdef __cplusplus +} +#endif diff --git a/source/drivers/foc_in_the_loop/esp_foc_itl_tick.h b/source/drivers/foc_in_the_loop/esp_foc_itl_tick.h new file mode 100644 index 00000000..b57a2da7 --- /dev/null +++ b/source/drivers/foc_in_the_loop/esp_foc_itl_tick.h @@ -0,0 +1,23 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + */ +#pragma once + +#include + +#include "espFoC/esp_foc_err.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef void (*esp_foc_itl_tick_cb_t)(void *arg); + +esp_foc_err_t esp_foc_itl_tick_start(esp_foc_itl_tick_cb_t cb, void *arg, uint32_t rate_hz); +void esp_foc_itl_tick_stop(void); + +#ifdef __cplusplus +} +#endif diff --git a/source/drivers/foc_in_the_loop/esp_foc_itl_tick_gptimer.c b/source/drivers/foc_in_the_loop/esp_foc_itl_tick_gptimer.c new file mode 100644 index 00000000..4602fffe --- /dev/null +++ b/source/drivers/foc_in_the_loop/esp_foc_itl_tick_gptimer.c @@ -0,0 +1,109 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + */ +#include "sdkconfig.h" +#include "esp_foc_itl_tick.h" + +#include "driver/gptimer.h" +#include "esp_log.h" + +static const char *TAG = "foc_itl_tick"; + +static gptimer_handle_t s_timer; +static esp_foc_itl_tick_cb_t s_cb; +static void *s_cb_arg; + +static bool tick_isr(gptimer_handle_t timer, + const gptimer_alarm_event_data_t *edata, + void *user_data) +{ + (void)timer; + (void)edata; + (void)user_data; + if (s_cb != NULL) { + s_cb(s_cb_arg); + } + return false; +} + +esp_foc_err_t esp_foc_itl_tick_start(esp_foc_itl_tick_cb_t cb, void *arg, uint32_t rate_hz) +{ + if (cb == NULL || rate_hz == 0u) { + return ESP_FOC_ERR_INVALID_ARG; + } + + s_cb = cb; + s_cb_arg = arg; + + gptimer_config_t timer_cfg = { + .clk_src = GPTIMER_CLK_SRC_DEFAULT, + .direction = GPTIMER_COUNT_UP, + .resolution_hz = 1000000, + }; + + esp_err_t err = gptimer_new_timer(&timer_cfg, &s_timer); + if (err != ESP_OK) { + ESP_LOGE(TAG, "gptimer_new_timer failed: %s", esp_err_to_name(err)); + return ESP_FOC_ERR_UNKNOWN; + } + + gptimer_event_callbacks_t cbs = { + .on_alarm = tick_isr, + }; + err = gptimer_register_event_callbacks(s_timer, &cbs, NULL); + if (err != ESP_OK) { + ESP_LOGE(TAG, "register callbacks failed: %s", esp_err_to_name(err)); + gptimer_del_timer(s_timer); + s_timer = NULL; + return ESP_FOC_ERR_UNKNOWN; + } + + err = gptimer_enable(s_timer); + if (err != ESP_OK) { + ESP_LOGE(TAG, "timer enable failed: %s", esp_err_to_name(err)); + gptimer_del_timer(s_timer); + s_timer = NULL; + return ESP_FOC_ERR_UNKNOWN; + } + + const uint64_t alarm_us = 1000000ull / (uint64_t)rate_hz; + gptimer_alarm_config_t alarm_cfg = { + .alarm_count = alarm_us, + .reload_count = 0, + .flags.auto_reload_on_alarm = true, + }; + err = gptimer_set_alarm_action(s_timer, &alarm_cfg); + if (err != ESP_OK) { + ESP_LOGE(TAG, "set alarm failed: %s", esp_err_to_name(err)); + gptimer_disable(s_timer); + gptimer_del_timer(s_timer); + s_timer = NULL; + return ESP_FOC_ERR_UNKNOWN; + } + + err = gptimer_start(s_timer); + if (err != ESP_OK) { + ESP_LOGE(TAG, "timer start failed: %s", esp_err_to_name(err)); + gptimer_disable(s_timer); + gptimer_del_timer(s_timer); + s_timer = NULL; + return ESP_FOC_ERR_UNKNOWN; + } + + ESP_LOGI(TAG, "GPTimer tick @ %lu Hz", (unsigned long)rate_hz); + return ESP_FOC_OK; +} + +void esp_foc_itl_tick_stop(void) +{ + if (s_timer != NULL) { + gptimer_stop(s_timer); + gptimer_disable(s_timer); + gptimer_del_timer(s_timer); + s_timer = NULL; + } + s_cb = NULL; + s_cb_arg = NULL; +} diff --git a/source/drivers/foc_in_the_loop/esp_foc_itl_tick_mcpwm.c b/source/drivers/foc_in_the_loop/esp_foc_itl_tick_mcpwm.c new file mode 100644 index 00000000..6c638536 --- /dev/null +++ b/source/drivers/foc_in_the_loop/esp_foc_itl_tick_mcpwm.c @@ -0,0 +1,99 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + */ +#include "sdkconfig.h" +#include "esp_foc_itl_tick.h" + +#include "driver/mcpwm_prelude.h" +#include "esp_log.h" + +static const char *TAG = "foc_itl_tick"; + +static mcpwm_timer_handle_t s_timer; +static esp_foc_itl_tick_cb_t s_cb; +static void *s_cb_arg; + +static bool tick_isr(mcpwm_timer_handle_t timer, + const mcpwm_timer_event_data_t *edata, + void *user_data) +{ + (void)timer; + (void)edata; + (void)user_data; + if (s_cb != NULL) { + s_cb(s_cb_arg); + } + return false; +} + +esp_foc_err_t esp_foc_itl_tick_start(esp_foc_itl_tick_cb_t cb, void *arg, uint32_t rate_hz) +{ + if (cb == NULL || rate_hz == 0u) { + return ESP_FOC_ERR_INVALID_ARG; + } + + s_cb = cb; + s_cb_arg = arg; + + const uint32_t resolution_hz = 160000000u; + const uint32_t period_ticks = resolution_hz / rate_hz; + + mcpwm_timer_config_t timer_cfg = { + .group_id = 0, + .clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT, + .resolution_hz = resolution_hz, + .count_mode = MCPWM_TIMER_COUNT_MODE_UP_DOWN, + .period_ticks = period_ticks, + }; + + esp_err_t err = mcpwm_new_timer(&timer_cfg, &s_timer); + if (err != ESP_OK) { + ESP_LOGE(TAG, "mcpwm_new_timer failed: %s", esp_err_to_name(err)); + return ESP_FOC_ERR_UNKNOWN; + } + + mcpwm_timer_event_callbacks_t cbs = { + .on_full = tick_isr, + }; + err = mcpwm_timer_register_event_callbacks(s_timer, &cbs, NULL); + if (err != ESP_OK) { + ESP_LOGE(TAG, "register callbacks failed: %s", esp_err_to_name(err)); + mcpwm_del_timer(s_timer); + s_timer = NULL; + return ESP_FOC_ERR_UNKNOWN; + } + + err = mcpwm_timer_enable(s_timer); + if (err != ESP_OK) { + ESP_LOGE(TAG, "timer enable failed: %s", esp_err_to_name(err)); + mcpwm_del_timer(s_timer); + s_timer = NULL; + return ESP_FOC_ERR_UNKNOWN; + } + + err = mcpwm_timer_start_stop(s_timer, MCPWM_TIMER_START_NO_STOP); + if (err != ESP_OK) { + ESP_LOGE(TAG, "timer start failed: %s", esp_err_to_name(err)); + mcpwm_timer_disable(s_timer); + mcpwm_del_timer(s_timer); + s_timer = NULL; + return ESP_FOC_ERR_UNKNOWN; + } + + ESP_LOGI(TAG, "MCPWM tick @ %lu Hz", (unsigned long)rate_hz); + return ESP_FOC_OK; +} + +void esp_foc_itl_tick_stop(void) +{ + if (s_timer != NULL) { + mcpwm_timer_start_stop(s_timer, MCPWM_TIMER_STOP_EMPTY); + mcpwm_timer_disable(s_timer); + mcpwm_del_timer(s_timer); + s_timer = NULL; + } + s_cb = NULL; + s_cb_arg = NULL; +} diff --git a/source/motor_control/esp_foc_core.c b/source/motor_control/esp_foc_core.c index 3cac2409..b46b6779 100644 --- a/source/motor_control/esp_foc_core.c +++ b/source/motor_control/esp_foc_core.c @@ -479,10 +479,17 @@ esp_foc_err_t esp_foc_align_axis(esp_foc_axis_t *axis) "alignment: natural direction = CCW (delta from sweep=%lld)", (long long)delta); } else if (delta == 0) { +#ifdef CONFIG_ESP_FOC_FITL + axis->natural_direction = Q16_ONE; + ESP_LOGW( + tag, + "alignment: delta=0; assuming CW (FITL simulated plant)"); +#else ESP_LOGE( tag, "alignment: error! failed to find the direction of the motor, aborting!"); abort(); +#endif } else { ESP_LOGI( tag, diff --git a/source/motor_control/foc_in_the_loop/esp_foc_in_the_loop.c b/source/motor_control/foc_in_the_loop/esp_foc_in_the_loop.c new file mode 100644 index 00000000..e0b89ea9 --- /dev/null +++ b/source/motor_control/foc_in_the_loop/esp_foc_in_the_loop.c @@ -0,0 +1,443 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + */ +#include "sdkconfig.h" +#include "espFoC/esp_foc_in_the_loop.h" + +#include +#include + +#include "espFoC/esp_foc.h" +#include "espFoC/utils/biquad_q16.h" +#include "espFoC/utils/foc_math_q16.h" +#include "esp_log.h" + +#include "esp_foc_itl_plant.h" +#include "esp_foc_itl_tick.h" + +static const char *TAG = "foc_itl"; + +#define FOC_ITL_PLANT_RUNNER_PRIORITY 2 +#define FOC_ITL_PLANT_JOIN_MS 50 + +static void plant_runner_join(esp_foc_event_handle_t ev) +{ + if (ev == NULL) { + return; + } + for (int ms = 0; ms < FOC_ITL_PLANT_JOIN_MS && esp_foc_runner_is_alive(ev); ++ms) { + esp_foc_runner_wake(ev); + esp_foc_sleep_ms(1); + } +} + +typedef struct { + esp_foc_itl_plant_t plant; + int64_t enc_accum; + + volatile q16_t duty_u; + volatile q16_t duty_v; + volatile q16_t duty_w; + + esp_foc_biquad_q16_t bq_u; + esp_foc_biquad_q16_t bq_v; + + volatile q16_t *publish_alpha; + volatile q16_t *publish_beta; + volatile q16_t *publish_iu; + volatile q16_t *publish_iv; + + esp_foc_inverter_callback_t inverter_cb; + void *inverter_cb_arg; + + uint32_t pwm_hz; + q16_t vdc_q16; + + esp_foc_event_handle_t plant_ev; + bool alive; + + esp_foc_inverter_t inverter; + esp_foc_isensor_t isensor; + esp_foc_rotor_sensor_t rotor; +} esp_foc_itl_ctx_t; + +static esp_foc_itl_ctx_t s_ctx; +static bool s_created; + +static uint32_t fitl_pwm_hz_from_config(uint32_t cfg_hz) +{ + if (cfg_hz != 0u) { + return cfg_hz; + } +#if defined(CONFIG_FOC_ITL_PWM_10KHZ) + return 10000u; +#else + return (uint32_t)CONFIG_ESP_FOC_PWM_RATE_HZ; +#endif +} + +static void snapshot_duties(esp_foc_itl_ctx_t *ctx, float *du, float *dv, float *dw) +{ + esp_foc_critical_enter(); + *du = q16_to_float(ctx->duty_u); + *dv = q16_to_float(ctx->duty_v); + *dw = q16_to_float(ctx->duty_w); + esp_foc_critical_leave(); +} + +static void publish_currents(esp_foc_itl_ctx_t *ctx, q16_t iu, q16_t iv) +{ + q16_t iw = q16_sub(q16_sub(0, iu), iv); + q16_t alpha; + q16_t beta; + q16_clarke(iu, iv, iw, &alpha, &beta); + + if (ctx->publish_alpha != NULL) { + *ctx->publish_alpha = alpha; + } + if (ctx->publish_beta != NULL) { + *ctx->publish_beta = beta; + } + if (ctx->publish_iu != NULL) { + *ctx->publish_iu = iu; + } + if (ctx->publish_iv != NULL) { + *ctx->publish_iv = iv; + } +} + +static void plant_task_fn(void *arg) +{ + esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)arg; + const float dt = 1.0f / (float)ctx->pwm_hz; + + while (ctx->alive) { + esp_foc_wait_notifier(); + + float du; + float dv; + float dw; + snapshot_duties(ctx, &du, &dv, &dw); + + float vu; + float vv; + float vw; + esp_foc_itl_plant_duties_to_phase_volts(du, dv, dw, &ctx->plant, &vu, &vv, &vw); + esp_foc_itl_plant_step(&ctx->plant, vu, vv, vw, dt); + + const q16_t iu = esp_foc_biquad_q16_update(&ctx->bq_u, + q16_from_float(ctx->plant.iu_a)); + const q16_t iv = esp_foc_biquad_q16_update(&ctx->bq_v, + q16_from_float(ctx->plant.iv_a)); + publish_currents(ctx, iu, iv); + + const int32_t ticks = esp_foc_itl_plant_encoder_ticks(&ctx->plant); + ctx->enc_accum = (int64_t)ticks; + } + + esp_foc_runner_delete_self(); +} + +static void tick_cb(void *arg) +{ + esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)arg; + + if (ctx->inverter_cb != NULL) { + ctx->inverter_cb(ctx->inverter_cb_arg); + } + + if (ctx->plant_ev != NULL) { + if (esp_foc_in_task_context()) { + esp_foc_send_notification(ctx->plant_ev); + } else { + esp_foc_send_notification_from_isr(ctx->plant_ev); + } + } +} + +static void itl_set_inverter_callback(esp_foc_inverter_t *self, + esp_foc_inverter_callback_t callback, + void *argument) +{ + esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)((char *)self - offsetof(esp_foc_itl_ctx_t, inverter)); + esp_foc_critical_enter(); + ctx->inverter_cb = callback; + ctx->inverter_cb_arg = argument; + esp_foc_critical_leave(); +} + +static q16_t itl_get_dc_link_voltage(esp_foc_inverter_t *self) +{ + esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)((char *)self - offsetof(esp_foc_itl_ctx_t, inverter)); + return ctx->vdc_q16; +} + +static void itl_set_duties(esp_foc_inverter_t *self, q16_t duty_a, q16_t duty_b, q16_t duty_c) +{ + esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)((char *)self - offsetof(esp_foc_itl_ctx_t, inverter)); + esp_foc_critical_enter(); + ctx->duty_u = duty_a; + ctx->duty_v = duty_b; + ctx->duty_w = duty_c; + esp_foc_critical_leave(); +} + +static uint32_t itl_get_inverter_pwm_rate(esp_foc_inverter_t *self) +{ + esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)((char *)self - offsetof(esp_foc_itl_ctx_t, inverter)); + return ctx->pwm_hz; +} + +static void itl_enable(esp_foc_inverter_t *self) +{ + (void)self; +} + +static void itl_disable(esp_foc_inverter_t *self) +{ + (void)self; +} + +static void itl_fetch_isensors(esp_foc_isensor_t *self, isensor_values_t *values) +{ + (void)self; + (void)values; +} + +static void itl_sample_isensors(esp_foc_isensor_t *self) +{ + (void)self; +} + +static void itl_calibrate_isensors(esp_foc_isensor_t *self, int calibration_rounds) +{ + (void)self; + (void)calibration_rounds; +} + +static void itl_set_isensor_callback(esp_foc_isensor_t *self, isensor_callback_t cb, void *param) +{ + (void)self; + (void)cb; + (void)param; +} + +static void itl_set_filter_cutoff(esp_foc_isensor_t *self, float fc_hz, float fs_hz) +{ + esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)((char *)self - offsetof(esp_foc_itl_ctx_t, isensor)); + esp_foc_critical_enter(); + esp_foc_biquad_butterworth_lpf_design_q16(&ctx->bq_u, fc_hz, fs_hz); + esp_foc_biquad_butterworth_lpf_design_q16(&ctx->bq_v, fc_hz, fs_hz); + esp_foc_critical_leave(); +} + +static void itl_set_publish_targets(esp_foc_isensor_t *self, + q16_t *i_alpha_target, + q16_t *i_beta_target, + q16_t *i_u_target, + q16_t *i_v_target) +{ + esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)((char *)self - offsetof(esp_foc_itl_ctx_t, isensor)); + esp_foc_critical_enter(); + ctx->publish_alpha = (volatile q16_t *)i_alpha_target; + ctx->publish_beta = (volatile q16_t *)i_beta_target; + ctx->publish_iu = (volatile q16_t *)i_u_target; + ctx->publish_iv = (volatile q16_t *)i_v_target; + esp_foc_critical_leave(); +} + +static void itl_rotor_set_to_zero(esp_foc_rotor_sensor_t *self) +{ + esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)((char *)self - offsetof(esp_foc_itl_ctx_t, rotor)); + esp_foc_critical_enter(); + esp_foc_itl_plant_reset_parked(&ctx->plant); + ctx->enc_accum = 0; + esp_foc_critical_leave(); +} + +static uint32_t itl_rotor_get_cpr(esp_foc_rotor_sensor_t *self) +{ + (void)self; + return ESP_FOC_ITL_CPR; +} + +static q16_t itl_rotor_read_counts(esp_foc_rotor_sensor_t *self) +{ + esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)((char *)self - offsetof(esp_foc_itl_ctx_t, rotor)); + const int32_t ticks = esp_foc_itl_plant_encoder_ticks(&ctx->plant); + return q16_from_int(ticks); +} + +static int64_t itl_rotor_read_accumulated(esp_foc_rotor_sensor_t *self) +{ + esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)((char *)self - offsetof(esp_foc_itl_ctx_t, rotor)); + return ctx->enc_accum; +} + +static void itl_rotor_set_simulation_count(esp_foc_rotor_sensor_t *self, q16_t increment) +{ + (void)self; + (void)increment; +} + +static void itl_rotor_set_zero_offset_raw_12b(esp_foc_rotor_sensor_t *self, uint16_t raw_angle) +{ + (void)self; + (void)raw_angle; +} + +static uint16_t itl_rotor_get_zero_offset_12b(esp_foc_rotor_sensor_t *self) +{ + (void)self; + return 0; +} + +static void wire_vtables(esp_foc_itl_ctx_t *ctx) +{ + ctx->inverter.set_inverter_callback = itl_set_inverter_callback; + ctx->inverter.get_dc_link_voltage = itl_get_dc_link_voltage; + ctx->inverter.set_duties = itl_set_duties; + ctx->inverter.get_inverter_pwm_rate = itl_get_inverter_pwm_rate; + ctx->inverter.enable = itl_enable; + ctx->inverter.disable = itl_disable; + + ctx->isensor.fetch_isensors = itl_fetch_isensors; + ctx->isensor.sample_isensors = itl_sample_isensors; + ctx->isensor.calibrate_isensors = itl_calibrate_isensors; + ctx->isensor.set_isensor_callback = itl_set_isensor_callback; + ctx->isensor.set_filter_cutoff = itl_set_filter_cutoff; + ctx->isensor.set_publish_targets = itl_set_publish_targets; + + ctx->rotor.set_to_zero = itl_rotor_set_to_zero; + ctx->rotor.get_counts_per_revolution = itl_rotor_get_cpr; + ctx->rotor.read_counts = itl_rotor_read_counts; + ctx->rotor.read_accumulated_counts_i64 = itl_rotor_read_accumulated; + ctx->rotor.set_simulation_count = itl_rotor_set_simulation_count; + ctx->rotor.set_zero_offset_raw_12b = itl_rotor_set_zero_offset_raw_12b; + ctx->rotor.get_zero_offset_12b = itl_rotor_get_zero_offset_12b; +} + +static esp_foc_itl_plant_params_t params_from_config(const esp_foc_in_the_loop_config_t *cfg) +{ + esp_foc_itl_plant_params_t p = { + .r_ohm = cfg->r_ohm, + .l_henry = cfg->l_henry, + .j_kgm2 = cfg->j_kgm2, + .b_nms = cfg->b_nms, + .kt_nm_per_a = cfg->kt_nm_per_a, + .pole_pairs = cfg->pole_pairs, + .vdc_volts = q16_to_float(cfg->vdc_q16), + .i_max_a = cfg->i_max_a, + .locked_rotor = cfg->locked_rotor, + .delta_connection = cfg->connection_delta, + }; + + if (p.r_ohm <= 0.0f) { + p.r_ohm = (float)CONFIG_FOC_ITL_DEFAULT_R_MILLIOHM / 1000.0f; + } + if (p.l_henry <= 0.0f) { + p.l_henry = (float)CONFIG_FOC_ITL_DEFAULT_L_UH / 1000000.0f; + } + if (p.j_kgm2 <= 0.0f) { + p.j_kgm2 = (float)CONFIG_FOC_ITL_DEFAULT_J_X1E7 / 10000000.0f; + } + if (p.b_nms <= 0.0f) { + p.b_nms = (float)CONFIG_FOC_ITL_DEFAULT_B_X1E7 / 10000000.0f; + } + if (p.kt_nm_per_a <= 0.0f) { + p.kt_nm_per_a = (float)CONFIG_FOC_ITL_DEFAULT_KT_X1E7 / 10000000.0f; + } + if (p.pole_pairs <= 0) { + p.pole_pairs = CONFIG_FOC_ITL_DEFAULT_POLE_PAIRS; + } + if (p.vdc_volts <= 0.0f) { + p.vdc_volts = (float)CONFIG_ESP_FOC_DC_LINK_NOMINAL_MV / 1000.0f; + } + if (p.i_max_a <= 0.0f) { + p.i_max_a = (float)CONFIG_FOC_ITL_DEFAULT_I_MAX_MA / 1000.0f; + } + if (!cfg->connection_delta) { +#ifdef CONFIG_FOC_ITL_MOTOR_DELTA + p.delta_connection = (bool)CONFIG_FOC_ITL_MOTOR_DELTA; +#else + p.delta_connection = false; +#endif + } + return p; +} + +esp_foc_err_t esp_foc_in_the_loop_create(const esp_foc_in_the_loop_config_t *cfg, + esp_foc_in_the_loop_handles_t *out) +{ + if (cfg == NULL || out == NULL) { + return ESP_FOC_ERR_INVALID_ARG; + } + if (s_created) { + ESP_LOGE(TAG, "singleton already created"); + return ESP_FOC_ERR_INVALID_ARG; + } + + memset(&s_ctx, 0, sizeof(s_ctx)); + s_ctx.pwm_hz = fitl_pwm_hz_from_config(cfg->pwm_hz); + s_ctx.vdc_q16 = cfg->vdc_q16; + if (s_ctx.vdc_q16 <= 0) { + s_ctx.vdc_q16 = q16_from_float((float)CONFIG_ESP_FOC_DC_LINK_NOMINAL_MV / 1000.0f); + } + + const esp_foc_itl_plant_params_t plant_params = params_from_config(cfg); + esp_foc_itl_plant_init(&s_ctx.plant, &plant_params); + wire_vtables(&s_ctx); + + const float fc = (float)CONFIG_ESP_FOC_CURRENT_FILTER_CUTOFF_HZ; + const float fs = (float)s_ctx.pwm_hz; + esp_foc_biquad_butterworth_lpf_design_q16(&s_ctx.bq_u, fc, fs); + esp_foc_biquad_butterworth_lpf_design_q16(&s_ctx.bq_v, fc, fs); + + s_ctx.alive = true; + void *plant_handle = NULL; + if (esp_foc_create_runner(plant_task_fn, &s_ctx, FOC_ITL_PLANT_RUNNER_PRIORITY, + &plant_handle) != 0) { + ESP_LOGE(TAG, "plant task create failed"); + memset(&s_ctx, 0, sizeof(s_ctx)); + return ESP_FOC_ERR_UNKNOWN; + } + s_ctx.plant_ev = (esp_foc_event_handle_t)plant_handle; + + esp_foc_err_t err = esp_foc_itl_tick_start(tick_cb, &s_ctx, s_ctx.pwm_hz); + if (err != ESP_FOC_OK) { + s_ctx.alive = false; + esp_foc_runner_wake(s_ctx.plant_ev); + plant_runner_join(s_ctx.plant_ev); + memset(&s_ctx, 0, sizeof(s_ctx)); + return err; + } + + out->inverter = &s_ctx.inverter; + out->isensor = &s_ctx.isensor; + out->rotor = &s_ctx.rotor; + s_created = true; + + ESP_LOGI(TAG, "FITL ready @ %lu Hz, pp=%d, locked=%d", + (unsigned long)s_ctx.pwm_hz, plant_params.pole_pairs, + (int)plant_params.locked_rotor); + return ESP_FOC_OK; +} + +void esp_foc_in_the_loop_destroy(void) +{ + if (!s_created) { + return; + } + + esp_foc_itl_tick_stop(); + s_ctx.alive = false; + if (s_ctx.plant_ev != NULL) { + esp_foc_runner_wake(s_ctx.plant_ev); + plant_runner_join(s_ctx.plant_ev); + } + + memset(&s_ctx, 0, sizeof(s_ctx)); + s_created = false; +} diff --git a/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.c b/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.c new file mode 100644 index 00000000..dc81016f --- /dev/null +++ b/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.c @@ -0,0 +1,263 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + */ +#include "esp_foc_itl_plant.h" + +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#define FOC_ITL_TWO_PI (2.0f * (float)M_PI) +#define FOC_ITL_INV_SQRT3 0.57735026919f + +typedef struct { + float id; + float iq; + float omega_m; + float theta_m; +} esp_foc_itl_state_t; + +typedef struct { + float did; + float diq; + float domega_m; + float dtheta_m; +} esp_foc_itl_deriv_t; + +static float clampf(float x, float lo, float hi) +{ + if (x < lo) { + return lo; + } + if (x > hi) { + return hi; + } + return x; +} + +static void abc_to_dq(float iu, float iv, float iw, float theta_e, + float *id, float *iq) +{ + const float c = cosf(theta_e); + const float s = sinf(theta_e); + const float ialpha = iu; + const float ibeta = (iu + 2.0f * iv) * (1.0f / sqrtf(3.0f)); + *id = ialpha * c + ibeta * s; + *iq = ibeta * c - ialpha * s; + (void)iw; +} + +static void dq_to_abc(float id, float iq, float theta_e, + float *iu, float *iv, float *iw) +{ + const float c = cosf(theta_e); + const float s = sinf(theta_e); + const float ialpha = id * c - iq * s; + const float ibeta = id * s + iq * c; + *iu = ialpha; + *iv = -0.5f * ialpha + 0.86602540378f * ibeta; + *iw = -(*iu + *iv); +} + +static void plant_deriv(const esp_foc_itl_plant_t *plant, const esp_foc_itl_state_t *st, + float vu, float vv, float vw, esp_foc_itl_deriv_t *out) +{ + const esp_foc_itl_plant_params_t *p = &plant->p; + const float pp = (float)p->pole_pairs; + const float theta_e = pp * st->theta_m; + const float omega_e = pp * st->omega_m; + + float vd; + float vq; + abc_to_dq(vu, vv, vw, theta_e, &vd, &vq); + + const float psi_f = p->kt_nm_per_a / (1.5f * pp); + const float inv_l = 1.0f / p->l_henry; + + out->did = inv_l * (vd - p->r_ohm * st->id + omega_e * p->l_henry * st->iq); + out->diq = inv_l * (vq - p->r_ohm * st->iq - omega_e * p->l_henry * st->id + - omega_e * psi_f); + + if (p->locked_rotor) { + out->domega_m = 0.0f; + out->dtheta_m = 0.0f; + } else { + const float te = p->kt_nm_per_a * st->iq; + out->domega_m = (te - p->b_nms * st->omega_m) / p->j_kgm2; + out->dtheta_m = st->omega_m; + } +} + +static void plant_apply_saturation(esp_foc_itl_plant_t *plant) +{ + dq_to_abc(plant->id_a, plant->iq_a, + (float)plant->p.pole_pairs * plant->theta_m_rad, + &plant->iu_a, &plant->iv_a, &plant->iw_a); + + const float lim = plant->p.i_max_a; + plant->iu_a = clampf(plant->iu_a, -lim, lim); + plant->iv_a = clampf(plant->iv_a, -lim, lim); + plant->iw_a = clampf(plant->iw_a, -lim, lim); + + abc_to_dq(plant->iu_a, plant->iv_a, plant->iw_a, + (float)plant->p.pole_pairs * plant->theta_m_rad, + &plant->id_a, &plant->iq_a); +} + +static void rk_add_state(const esp_foc_itl_state_t *a, const esp_foc_itl_deriv_t *b, + float scale, esp_foc_itl_state_t *out) +{ + out->id = a->id + scale * b->did; + out->iq = a->iq + scale * b->diq; + out->omega_m = a->omega_m + scale * b->domega_m; + out->theta_m = a->theta_m + scale * b->dtheta_m; +} + +#if defined(CONFIG_FOC_ITL_USE_RK2) +static void plant_integrate(esp_foc_itl_plant_t *plant, float vu, float vv, float vw, + float dt_s) +{ + esp_foc_itl_state_t st = { + .id = plant->id_a, + .iq = plant->iq_a, + .omega_m = plant->omega_m_rad_s, + .theta_m = plant->theta_m_rad, + }; + esp_foc_itl_deriv_t k1; + esp_foc_itl_deriv_t k2; + esp_foc_itl_state_t mid; + + plant_deriv(plant, &st, vu, vv, vw, &k1); + rk_add_state(&st, &k1, 0.5f * dt_s, &mid); + plant_deriv(plant, &mid, vu, vv, vw, &k2); + + plant->id_a = st.id + dt_s * k2.did; + plant->iq_a = st.iq + dt_s * k2.diq; + if (plant->p.locked_rotor) { + plant->omega_m_rad_s = 0.0f; + plant->theta_m_rad = st.theta_m; + } else { + plant->omega_m_rad_s = st.omega_m + dt_s * k2.domega_m; + plant->theta_m_rad = st.theta_m + dt_s * k2.dtheta_m; + } +} +#else +static void plant_integrate(esp_foc_itl_plant_t *plant, float vu, float vv, float vw, + float dt_s) +{ + esp_foc_itl_state_t st = { + .id = plant->id_a, + .iq = plant->iq_a, + .omega_m = plant->omega_m_rad_s, + .theta_m = plant->theta_m_rad, + }; + esp_foc_itl_deriv_t k1; + esp_foc_itl_deriv_t k2; + esp_foc_itl_deriv_t k3; + esp_foc_itl_deriv_t k4; + esp_foc_itl_state_t s2; + esp_foc_itl_state_t s3; + esp_foc_itl_state_t s4; + + plant_deriv(plant, &st, vu, vv, vw, &k1); + rk_add_state(&st, &k1, 0.5f * dt_s, &s2); + plant_deriv(plant, &s2, vu, vv, vw, &k2); + rk_add_state(&st, &k2, 0.5f * dt_s, &s3); + plant_deriv(plant, &s3, vu, vv, vw, &k3); + rk_add_state(&st, &k3, dt_s, &s4); + plant_deriv(plant, &s4, vu, vv, vw, &k4); + + plant->id_a = st.id + (dt_s / 6.0f) * (k1.did + 2.0f * k2.did + 2.0f * k3.did + k4.did); + plant->iq_a = st.iq + (dt_s / 6.0f) * (k1.diq + 2.0f * k2.diq + 2.0f * k3.diq + k4.diq); + if (plant->p.locked_rotor) { + plant->omega_m_rad_s = 0.0f; + plant->theta_m_rad = st.theta_m; + } else { + plant->omega_m_rad_s = st.omega_m + + (dt_s / 6.0f) * (k1.domega_m + 2.0f * k2.domega_m + 2.0f * k3.domega_m + k4.domega_m); + plant->theta_m_rad = st.theta_m + + (dt_s / 6.0f) * (k1.dtheta_m + 2.0f * k2.dtheta_m + 2.0f * k3.dtheta_m + k4.dtheta_m); + } +} +#endif + +void esp_foc_itl_plant_init(esp_foc_itl_plant_t *plant, const esp_foc_itl_plant_params_t *params) +{ + memset(plant, 0, sizeof(*plant)); + if (params != NULL) { + plant->p = *params; + } + esp_foc_itl_plant_reset_parked(plant); +} + +void esp_foc_itl_plant_reset_parked(esp_foc_itl_plant_t *plant) +{ + plant->id_a = 0.0f; + plant->iq_a = 0.0f; + plant->omega_m_rad_s = 0.0f; + plant->theta_m_rad = 0.0f; + plant->iu_a = 0.0f; + plant->iv_a = 0.0f; + plant->iw_a = 0.0f; +} + +void esp_foc_itl_plant_duties_to_phase_volts(float duty_u, float duty_v, float duty_w, + const esp_foc_itl_plant_t *plant, + float *vu, float *vv, float *vw) +{ + const float vdc = plant->p.vdc_volts; + const float half = 0.5f * vdc; + + *vu = (duty_u - 0.5f) * vdc; + *vv = (duty_v - 0.5f) * vdc; + *vw = (duty_w - 0.5f) * vdc; + + if (plant->p.delta_connection) { + *vu *= FOC_ITL_INV_SQRT3; + *vv *= FOC_ITL_INV_SQRT3; + *vw *= FOC_ITL_INV_SQRT3; + } + + *vu = clampf(*vu, -half, half); + *vv = clampf(*vv, -half, half); + *vw = clampf(*vw, -half, half); +} + +void esp_foc_itl_plant_step(esp_foc_itl_plant_t *plant, float vu, float vv, float vw, float dt_s) +{ + if (plant == NULL || dt_s <= 0.0f) { + return; + } + + plant_integrate(plant, vu, vv, vw, dt_s); + plant_apply_saturation(plant); + + if (!plant->p.locked_rotor) { + if (plant->theta_m_rad >= FOC_ITL_TWO_PI) { + plant->theta_m_rad = fmodf(plant->theta_m_rad, FOC_ITL_TWO_PI); + } else if (plant->theta_m_rad < 0.0f) { + plant->theta_m_rad = fmodf(plant->theta_m_rad, FOC_ITL_TWO_PI) + FOC_ITL_TWO_PI; + } + } +} + +int32_t esp_foc_itl_plant_encoder_ticks(const esp_foc_itl_plant_t *plant) +{ + if (plant == NULL) { + return 0; + } + const float turns = plant->theta_m_rad / FOC_ITL_TWO_PI; + int32_t ticks = (int32_t)lroundf(turns * (float)ESP_FOC_ITL_CPR); + while (ticks < 0) { + ticks += (int32_t)ESP_FOC_ITL_CPR; + } + while (ticks >= (int32_t)ESP_FOC_ITL_CPR) { + ticks -= (int32_t)ESP_FOC_ITL_CPR; + } + return ticks; +} diff --git a/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.h b/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.h new file mode 100644 index 00000000..6a468c59 --- /dev/null +++ b/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.h @@ -0,0 +1,53 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + * + * Discretized PMSM plant (dq state, abc terminal voltages). Host-testable. + */ +#pragma once + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define ESP_FOC_ITL_CPR 4096u + +typedef struct { + float r_ohm; + float l_henry; + float j_kgm2; + float b_nms; + float kt_nm_per_a; + int pole_pairs; + float vdc_volts; + float i_max_a; + bool locked_rotor; + bool delta_connection; +} esp_foc_itl_plant_params_t; + +typedef struct { + esp_foc_itl_plant_params_t p; + float id_a; + float iq_a; + float omega_m_rad_s; + float theta_m_rad; + float iu_a; + float iv_a; + float iw_a; +} esp_foc_itl_plant_t; + +void esp_foc_itl_plant_init(esp_foc_itl_plant_t *plant, const esp_foc_itl_plant_params_t *params); +void esp_foc_itl_plant_reset_parked(esp_foc_itl_plant_t *plant); +void esp_foc_itl_plant_duties_to_phase_volts(float duty_u, float duty_v, float duty_w, + const esp_foc_itl_plant_t *plant, + float *vu, float *vv, float *vw); +void esp_foc_itl_plant_step(esp_foc_itl_plant_t *plant, float vu, float vv, float vw, float dt_s); +int32_t esp_foc_itl_plant_encoder_ticks(const esp_foc_itl_plant_t *plant); + +#ifdef __cplusplus +} +#endif diff --git a/test/test_axis_flow.c b/test/test_axis_flow.c index 22e5201e..4e923597 100644 --- a/test/test_axis_flow.c +++ b/test/test_axis_flow.c @@ -3,8 +3,6 @@ */ #include #include -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" #include "espFoC/esp_foc.h" #include "espFoC/utils/esp_foc_q16.h" #include "mock_drivers.h" @@ -147,9 +145,9 @@ TEST_CASE("axis run: regulator runs and set_duties receives FOC output", "[espFo for (int i = 0; i < 40; i++) { mock_inverter_trigger_callback(&mock_inv); - vTaskDelay(pdMS_TO_TICKS(10)); + esp_foc_sleep_ms(10); } - vTaskDelay(pdMS_TO_TICKS(200)); + esp_foc_sleep_ms(200); TEST_ASSERT_TRUE(mock_inv.set_duties_count >= 1); TEST_ASSERT_TRUE(axis_run_regulator_called_count >= 1); @@ -167,7 +165,7 @@ TEST_CASE("axis stop: returns to idle and allows re-align", "[espFoC][axis_flow] for (int i = 0; i < 5; i++) { mock_inverter_trigger_callback(&mock_inv); - vTaskDelay(pdMS_TO_TICKS(10)); + esp_foc_sleep_ms(10); } TEST_ASSERT_EQUAL(ESP_FOC_OK, esp_foc_stop(&axis)); diff --git a/test/test_foc_itl_integration.c b/test/test_foc_itl_integration.c new file mode 100644 index 00000000..c01eff9c --- /dev/null +++ b/test/test_foc_itl_integration.c @@ -0,0 +1,79 @@ +/* + * Integration tests for esp_foc_in_the_loop factory (FITL build). + */ +#include "sdkconfig.h" +#if CONFIG_ESP_FOC_FITL + +#include +#include "espFoC/esp_foc_in_the_loop.h" +#include "espFoC/esp_foc.h" +#include "espFoC/esp_foc_err.h" +#include "espFoC/utils/esp_foc_q16.h" + +#define FITL_ITL_POLL_MS 25 + +static volatile int s_tick_cb_count; + +static void fitl_count_tick(void *arg) +{ + (void)arg; + s_tick_cb_count++; +} + +void tearDown(void) +{ + esp_foc_in_the_loop_destroy(); +} + +TEST_CASE("fitl: create returns three interfaces", "[espFoC][foc_itl]") +{ + esp_foc_in_the_loop_config_t cfg = { + .r_ohm = 1.0f, + .l_henry = 0.002f, + .j_kgm2 = 5e-5f, + .b_nms = 1e-5f, + .kt_nm_per_a = 1e-4f, + .pole_pairs = 7, + .vdc_q16 = q16_from_float(12.0f), + .i_max_a = 10.0f, + .locked_rotor = true, + .connection_delta = false, + .pwm_hz = 10000, + }; + esp_foc_in_the_loop_handles_t h = {0}; + + TEST_ASSERT_EQUAL(ESP_FOC_OK, esp_foc_in_the_loop_create(&cfg, &h)); + TEST_ASSERT_NOT_NULL(h.inverter); + TEST_ASSERT_NOT_NULL(h.isensor); + TEST_ASSERT_NOT_NULL(h.rotor); + TEST_ASSERT_EQUAL(10000u, h.inverter->get_inverter_pwm_rate(h.inverter)); +} + +TEST_CASE("fitl: tick invokes inverter callback", "[espFoC][foc_itl]") +{ + esp_foc_in_the_loop_config_t cfg = { + .locked_rotor = true, + .pwm_hz = 10000, + }; + esp_foc_in_the_loop_handles_t h = {0}; + TEST_ASSERT_EQUAL(ESP_FOC_OK, esp_foc_in_the_loop_create(&cfg, &h)); + + s_tick_cb_count = 0; + h.inverter->set_inverter_callback(h.inverter, fitl_count_tick, NULL); + + for (int ms = 0; ms < FITL_ITL_POLL_MS && s_tick_cb_count == 0; ++ms) { + esp_foc_sleep_ms(1); + } + + TEST_ASSERT_GREATER_THAN(0, s_tick_cb_count); +} + +TEST_CASE("fitl: second create without destroy fails", "[espFoC][foc_itl]") +{ + esp_foc_in_the_loop_config_t cfg = { .locked_rotor = true, .pwm_hz = 10000 }; + esp_foc_in_the_loop_handles_t h = {0}; + TEST_ASSERT_EQUAL(ESP_FOC_OK, esp_foc_in_the_loop_create(&cfg, &h)); + TEST_ASSERT_EQUAL(ESP_FOC_ERR_INVALID_ARG, esp_foc_in_the_loop_create(&cfg, &h)); +} + +#endif /* CONFIG_ESP_FOC_FITL */ diff --git a/test/test_foc_itl_plant.c b/test/test_foc_itl_plant.c new file mode 100644 index 00000000..ab856a4f --- /dev/null +++ b/test/test_foc_itl_plant.c @@ -0,0 +1,100 @@ +/* + * Unit tests for esp_foc_itl_plant (pure physics, no FITL driver). + */ +#include "sdkconfig.h" +#if CONFIG_ESP_FOC_FITL + +#include +#include + +#include "esp_foc_itl_plant.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +static esp_foc_itl_plant_t s_plant; + +static void init_locked_plant(float r, float l, float vdc) +{ + esp_foc_itl_plant_params_t p = { + .r_ohm = r, + .l_henry = l, + .j_kgm2 = 1e-4f, + .b_nms = 1e-5f, + .kt_nm_per_a = 1e-4f, + .pole_pairs = 7, + .vdc_volts = vdc, + .i_max_a = 20.0f, + .locked_rotor = true, + .delta_connection = false, + }; + esp_foc_itl_plant_init(&s_plant, &p); +} + +TEST_CASE("plant: locked rotor reaches steady-state current", "[espFoC][foc_itl_plant]") +{ + init_locked_plant(1.0f, 0.002f, 12.0f); + const float dt = 1.0f / 20000.0f; + const float duty = 0.55f; + const float vu = (duty - 0.5f) * 12.0f; + const float target = vu / 1.0f; + + for (int i = 0; i < 8000; i++) { + esp_foc_itl_plant_step(&s_plant, vu, 0.0f, -vu, dt); + } + + TEST_ASSERT_FLOAT_WITHIN(0.5f, target, s_plant.iu_a); + TEST_ASSERT_FLOAT_WITHIN(0.05f, 0.0f, s_plant.omega_m_rad_s); +} + +TEST_CASE("plant: current saturation clamps phase current", "[espFoC][foc_itl_plant]") +{ + init_locked_plant(0.01f, 0.0001f, 48.0f); + s_plant.p.i_max_a = 2.0f; + const float dt = 1.0f / 20000.0f; + + for (int i = 0; i < 4000; i++) { + esp_foc_itl_plant_step(&s_plant, 20.0f, -10.0f, -10.0f, dt); + } + + TEST_ASSERT_LESS_OR_EQUAL(2.01f, fabsf(s_plant.iu_a)); + TEST_ASSERT_LESS_OR_EQUAL(2.01f, fabsf(s_plant.iv_a)); +} + +TEST_CASE("plant: free rotor step response does not diverge", "[espFoC][foc_itl_plant]") +{ + esp_foc_itl_plant_params_t p = { + .r_ohm = 1.0f, + .l_henry = 0.002f, + .j_kgm2 = 5e-5f, + .b_nms = 1e-5f, + .kt_nm_per_a = 1e-4f, + .pole_pairs = 7, + .vdc_volts = 12.0f, + .i_max_a = 10.0f, + .locked_rotor = false, + .delta_connection = false, + }; + esp_foc_itl_plant_init(&s_plant, &p); + + const float dt = 1.0f / 20000.0f; + for (int i = 0; i < 20000; i++) { + esp_foc_itl_plant_step(&s_plant, 1.0f, -0.5f, -0.5f, dt); + } + + TEST_ASSERT_LESS_OR_EQUAL(10.01f, fabsf(s_plant.iu_a)); + TEST_ASSERT(isfinite(s_plant.omega_m_rad_s)); + TEST_ASSERT(isfinite(s_plant.theta_m_rad)); +} + +TEST_CASE("plant: encoder ticks wrap at CPR", "[espFoC][foc_itl_plant]") +{ + init_locked_plant(1.0f, 0.002f, 12.0f); + s_plant.theta_m_rad = (float)(2.0 * M_PI * 0.99); + const int32_t ticks = esp_foc_itl_plant_encoder_ticks(&s_plant); + TEST_ASSERT_GREATER_OR_EQUAL(0, ticks); + TEST_ASSERT_LESS_THAN((int)ESP_FOC_ITL_CPR, ticks); +} + +#endif /* CONFIG_ESP_FOC_FITL */ diff --git a/test/test_isensor_adc.c b/test/test_isensor_adc.c index 5ae22ecf..18855178 100644 --- a/test/test_isensor_adc.c +++ b/test/test_isensor_adc.c @@ -1,6 +1,9 @@ /* * Unit tests for isensor ADC cali LUT (no hardware). */ +#include "sdkconfig.h" +#if !CONFIG_ESP_FOC_FITL + #include #include "espFoC/esp_foc_adc_cali_lut.h" #include "espFoC/current_sensor_adc.h" @@ -59,3 +62,5 @@ TEST_CASE("isensor etm source NULL returns invalid arg", "[espFoC][isensor_adc]" TEST_ASSERT_EQUAL(ESP_FOC_ERR_INVALID_ARG, err); } #endif + +#endif /* !CONFIG_ESP_FOC_FITL */ diff --git a/test/test_rotor_sensor_simu.c b/test/test_rotor_sensor_simu.c index dc41274d..9ed7144b 100644 --- a/test/test_rotor_sensor_simu.c +++ b/test/test_rotor_sensor_simu.c @@ -1,6 +1,9 @@ /* * Unit tests for rotor_sensor_simu (open-loop motor + encoder mimic). */ +#include "sdkconfig.h" +#if !CONFIG_ESP_FOC_FITL + #include #include #include "espFoC/utils/esp_foc_q16.h" @@ -82,3 +85,5 @@ TEST_CASE("rotor_sensor_simu: accumulated tracks unwrap", "[espFoC][rotor_simu]" int64_t a1 = s->read_accumulated_counts_i64(s); TEST_ASSERT_TRUE(a1 > a0); } + +#endif /* !CONFIG_ESP_FOC_FITL */ From 6d5ba1416bd9e48375029e9f261baf60ed81ba12 Mon Sep 17 00:00:00 2001 From: Felipe Neves Date: Mon, 1 Jun 2026 19:22:53 -0300 Subject: [PATCH 2/2] Convert FITL PMSM plant hot path to Q16 for soft-float targets. Float RK4 with sinf/cosf at PWM rate is too costly on ESP32-C6; keep float only in init/config and run Park/Clarke, integration, and saturation in Q16. --- .../foc_in_the_loop/esp_foc_in_the_loop.c | 30 +- .../foc_in_the_loop/esp_foc_itl_plant.c | 399 ++++++++++-------- .../foc_in_the_loop/esp_foc_itl_plant.h | 47 ++- test/test_foc_itl_plant.c | 41 +- 4 files changed, 288 insertions(+), 229 deletions(-) diff --git a/source/motor_control/foc_in_the_loop/esp_foc_in_the_loop.c b/source/motor_control/foc_in_the_loop/esp_foc_in_the_loop.c index e0b89ea9..c60618bf 100644 --- a/source/motor_control/foc_in_the_loop/esp_foc_in_the_loop.c +++ b/source/motor_control/foc_in_the_loop/esp_foc_in_the_loop.c @@ -78,12 +78,12 @@ static uint32_t fitl_pwm_hz_from_config(uint32_t cfg_hz) #endif } -static void snapshot_duties(esp_foc_itl_ctx_t *ctx, float *du, float *dv, float *dw) +static void snapshot_duties(esp_foc_itl_ctx_t *ctx, q16_t *du, q16_t *dv, q16_t *dw) { esp_foc_critical_enter(); - *du = q16_to_float(ctx->duty_u); - *dv = q16_to_float(ctx->duty_v); - *dw = q16_to_float(ctx->duty_w); + *du = ctx->duty_u; + *dv = ctx->duty_v; + *dw = ctx->duty_w; esp_foc_critical_leave(); } @@ -111,26 +111,23 @@ static void publish_currents(esp_foc_itl_ctx_t *ctx, q16_t iu, q16_t iv) static void plant_task_fn(void *arg) { esp_foc_itl_ctx_t *ctx = (esp_foc_itl_ctx_t *)arg; - const float dt = 1.0f / (float)ctx->pwm_hz; while (ctx->alive) { esp_foc_wait_notifier(); - float du; - float dv; - float dw; + q16_t du; + q16_t dv; + q16_t dw; snapshot_duties(ctx, &du, &dv, &dw); - float vu; - float vv; - float vw; + q16_t vu; + q16_t vv; + q16_t vw; esp_foc_itl_plant_duties_to_phase_volts(du, dv, dw, &ctx->plant, &vu, &vv, &vw); - esp_foc_itl_plant_step(&ctx->plant, vu, vv, vw, dt); + esp_foc_itl_plant_step(&ctx->plant, vu, vv, vw); - const q16_t iu = esp_foc_biquad_q16_update(&ctx->bq_u, - q16_from_float(ctx->plant.iu_a)); - const q16_t iv = esp_foc_biquad_q16_update(&ctx->bq_v, - q16_from_float(ctx->plant.iv_a)); + const q16_t iu = esp_foc_biquad_q16_update(&ctx->bq_u, ctx->plant.iu_q16); + const q16_t iv = esp_foc_biquad_q16_update(&ctx->bq_v, ctx->plant.iv_q16); publish_currents(ctx, iu, iv); const int32_t ticks = esp_foc_itl_plant_encoder_ticks(&ctx->plant); @@ -388,6 +385,7 @@ esp_foc_err_t esp_foc_in_the_loop_create(const esp_foc_in_the_loop_config_t *cfg const esp_foc_itl_plant_params_t plant_params = params_from_config(cfg); esp_foc_itl_plant_init(&s_ctx.plant, &plant_params); + esp_foc_itl_plant_set_dt(&s_ctx.plant, s_ctx.pwm_hz); wire_vtables(&s_ctx); const float fc = (float)CONFIG_ESP_FOC_CURRENT_FILTER_CUTOFF_HZ; diff --git a/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.c b/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.c index dc81016f..951cc0a2 100644 --- a/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.c +++ b/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.c @@ -5,244 +5,285 @@ */ #include "esp_foc_itl_plant.h" -#include #include -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif +#include "espFoC/utils/foc_math_q16.h" -#define FOC_ITL_TWO_PI (2.0f * (float)M_PI) -#define FOC_ITL_INV_SQRT3 0.57735026919f +#define ESP_FOC_ITL_INV_SQRT3_Q16 ((q16_t)37837) typedef struct { - float id; - float iq; - float omega_m; - float theta_m; -} esp_foc_itl_state_t; + q16_t id; + q16_t iq; + q16_t omega_m; + q16_t theta_m; +} esp_foc_itl_state_q16_t; typedef struct { - float did; - float diq; - float domega_m; - float dtheta_m; -} esp_foc_itl_deriv_t; - -static float clampf(float x, float lo, float hi) + q16_t did; + q16_t diq; + q16_t domega_m; + q16_t dtheta_m; +} esp_foc_itl_deriv_q16_t; + +static void elec_sin_cos_q16(const esp_foc_itl_plant_t *plant, q16_t theta_m, + q16_t *sin_e, q16_t *cos_e) { - if (x < lo) { - return lo; - } - if (x > hi) { - return hi; - } - return x; + q16_t theta_e = q16_mul(theta_m, q16_from_int(plant->c.pole_pairs)); + theta_e = q16_normalize_angle(theta_e); + *sin_e = q16_sin(theta_e); + *cos_e = q16_cos(theta_e); } -static void abc_to_dq(float iu, float iv, float iw, float theta_e, - float *id, float *iq) +static void abc_to_dq_q16(q16_t iu, q16_t iv, q16_t iw, q16_t sin_e, q16_t cos_e, + q16_t *id, q16_t *iq) { - const float c = cosf(theta_e); - const float s = sinf(theta_e); - const float ialpha = iu; - const float ibeta = (iu + 2.0f * iv) * (1.0f / sqrtf(3.0f)); - *id = ialpha * c + ibeta * s; - *iq = ibeta * c - ialpha * s; - (void)iw; + q16_t alpha; + q16_t beta; + q16_clarke(iu, iv, iw, &alpha, &beta); + q16_park(sin_e, cos_e, alpha, beta, id, iq); } -static void dq_to_abc(float id, float iq, float theta_e, - float *iu, float *iv, float *iw) +static void dq_to_abc_q16(q16_t id, q16_t iq, q16_t sin_e, q16_t cos_e, + q16_t *iu, q16_t *iv, q16_t *iw) { - const float c = cosf(theta_e); - const float s = sinf(theta_e); - const float ialpha = id * c - iq * s; - const float ibeta = id * s + iq * c; - *iu = ialpha; - *iv = -0.5f * ialpha + 0.86602540378f * ibeta; - *iw = -(*iu + *iv); + q16_t alpha; + q16_t beta; + q16_inverse_park(sin_e, cos_e, id, iq, &alpha, &beta); + q16_inverse_clarke(alpha, beta, iu, iv, iw); } -static void plant_deriv(const esp_foc_itl_plant_t *plant, const esp_foc_itl_state_t *st, - float vu, float vv, float vw, esp_foc_itl_deriv_t *out) +static void plant_deriv_q16(const esp_foc_itl_plant_t *plant, const esp_foc_itl_state_q16_t *st, + q16_t vu, q16_t vv, q16_t vw, esp_foc_itl_deriv_q16_t *out) { - const esp_foc_itl_plant_params_t *p = &plant->p; - const float pp = (float)p->pole_pairs; - const float theta_e = pp * st->theta_m; - const float omega_e = pp * st->omega_m; - - float vd; - float vq; - abc_to_dq(vu, vv, vw, theta_e, &vd, &vq); - - const float psi_f = p->kt_nm_per_a / (1.5f * pp); - const float inv_l = 1.0f / p->l_henry; - - out->did = inv_l * (vd - p->r_ohm * st->id + omega_e * p->l_henry * st->iq); - out->diq = inv_l * (vq - p->r_ohm * st->iq - omega_e * p->l_henry * st->id - - omega_e * psi_f); + const esp_foc_itl_plant_coeff_t *c = &plant->c; + q16_t sin_e; + q16_t cos_e; + q16_t vd; + q16_t vq; + q16_t omega_e; + + elec_sin_cos_q16(plant, st->theta_m, &sin_e, &cos_e); + abc_to_dq_q16(vu, vv, vw, sin_e, cos_e, &vd, &vq); + + omega_e = q16_mul(st->omega_m, q16_from_int(c->pole_pairs)); + + { + q16_t back_emf = q16_mul(omega_e, c->l_q16); + back_emf = q16_mul(back_emf, st->iq); + q16_t num = q16_add(q16_sub(vd, q16_mul(c->r_q16, st->id)), back_emf); + out->did = q16_mul(c->inv_l_q16, num); + } + { + q16_t cross = q16_mul(omega_e, c->l_q16); + cross = q16_mul(cross, st->id); + q16_t emf = q16_mul(omega_e, c->psi_f_q16); + q16_t num = q16_sub(vq, q16_mul(c->r_q16, st->iq)); + num = q16_sub(num, cross); + num = q16_sub(num, emf); + out->diq = q16_mul(c->inv_l_q16, num); + } - if (p->locked_rotor) { - out->domega_m = 0.0f; - out->dtheta_m = 0.0f; + if (c->locked_rotor) { + out->domega_m = 0; + out->dtheta_m = 0; } else { - const float te = p->kt_nm_per_a * st->iq; - out->domega_m = (te - p->b_nms * st->omega_m) / p->j_kgm2; - out->dtheta_m = st->omega_m; + q16_t te = q16_mul(c->kt_q16, st->iq); + q16_t drag = q16_mul(c->b_q16, st->omega_m); + out->domega_m = q16_mul(c->inv_j_q16, q16_sub(te, drag)); + out->dtheta_m = q16_mul(st->omega_m, Q16_INV_TWO_PI); } } -static void plant_apply_saturation(esp_foc_itl_plant_t *plant) +static void rk_add_state_q16(const esp_foc_itl_state_q16_t *a, const esp_foc_itl_deriv_q16_t *b, + q16_t scale, esp_foc_itl_state_q16_t *out) { - dq_to_abc(plant->id_a, plant->iq_a, - (float)plant->p.pole_pairs * plant->theta_m_rad, - &plant->iu_a, &plant->iv_a, &plant->iw_a); - - const float lim = plant->p.i_max_a; - plant->iu_a = clampf(plant->iu_a, -lim, lim); - plant->iv_a = clampf(plant->iv_a, -lim, lim); - plant->iw_a = clampf(plant->iw_a, -lim, lim); - - abc_to_dq(plant->iu_a, plant->iv_a, plant->iw_a, - (float)plant->p.pole_pairs * plant->theta_m_rad, - &plant->id_a, &plant->iq_a); + out->id = q16_add(a->id, q16_mul(scale, b->did)); + out->iq = q16_add(a->iq, q16_mul(scale, b->diq)); + out->omega_m = q16_add(a->omega_m, q16_mul(scale, b->domega_m)); + out->theta_m = q16_add(a->theta_m, q16_mul(scale, b->dtheta_m)); } -static void rk_add_state(const esp_foc_itl_state_t *a, const esp_foc_itl_deriv_t *b, - float scale, esp_foc_itl_state_t *out) +static void plant_apply_saturation_q16(esp_foc_itl_plant_t *plant) { - out->id = a->id + scale * b->did; - out->iq = a->iq + scale * b->diq; - out->omega_m = a->omega_m + scale * b->domega_m; - out->theta_m = a->theta_m + scale * b->dtheta_m; + q16_t sin_e; + q16_t cos_e; + q16_t iw; + + elec_sin_cos_q16(plant, plant->theta_m_q16, &sin_e, &cos_e); + dq_to_abc_q16(plant->id_q16, plant->iq_q16, sin_e, cos_e, + &plant->iu_q16, &plant->iv_q16, &iw); + + plant->iu_q16 = q16_clamp(plant->iu_q16, q16_sub(0, plant->c.i_max_q16), plant->c.i_max_q16); + plant->iv_q16 = q16_clamp(plant->iv_q16, q16_sub(0, plant->c.i_max_q16), plant->c.i_max_q16); + + abc_to_dq_q16(plant->iu_q16, plant->iv_q16, q16_sub(0, q16_add(plant->iu_q16, plant->iv_q16)), + sin_e, cos_e, &plant->id_q16, &plant->iq_q16); } #if defined(CONFIG_FOC_ITL_USE_RK2) -static void plant_integrate(esp_foc_itl_plant_t *plant, float vu, float vv, float vw, - float dt_s) +static void plant_integrate_q16(esp_foc_itl_plant_t *plant, q16_t vu, q16_t vv, q16_t vw) { - esp_foc_itl_state_t st = { - .id = plant->id_a, - .iq = plant->iq_a, - .omega_m = plant->omega_m_rad_s, - .theta_m = plant->theta_m_rad, + const esp_foc_itl_plant_coeff_t *c = &plant->c; + esp_foc_itl_state_q16_t st = { + .id = plant->id_q16, + .iq = plant->iq_q16, + .omega_m = plant->omega_m_q16, + .theta_m = plant->theta_m_q16, }; - esp_foc_itl_deriv_t k1; - esp_foc_itl_deriv_t k2; - esp_foc_itl_state_t mid; - - plant_deriv(plant, &st, vu, vv, vw, &k1); - rk_add_state(&st, &k1, 0.5f * dt_s, &mid); - plant_deriv(plant, &mid, vu, vv, vw, &k2); - - plant->id_a = st.id + dt_s * k2.did; - plant->iq_a = st.iq + dt_s * k2.diq; - if (plant->p.locked_rotor) { - plant->omega_m_rad_s = 0.0f; - plant->theta_m_rad = st.theta_m; + esp_foc_itl_deriv_q16_t k1; + esp_foc_itl_deriv_q16_t k2; + esp_foc_itl_state_q16_t mid; + + plant_deriv_q16(plant, &st, vu, vv, vw, &k1); + rk_add_state_q16(&st, &k1, c->dt_half_q16, &mid); + plant_deriv_q16(plant, &mid, vu, vv, vw, &k2); + + plant->id_q16 = q16_add(st.id, q16_mul(c->dt_q16, k2.did)); + plant->iq_q16 = q16_add(st.iq, q16_mul(c->dt_q16, k2.diq)); + if (c->locked_rotor) { + plant->omega_m_q16 = 0; + plant->theta_m_q16 = st.theta_m; } else { - plant->omega_m_rad_s = st.omega_m + dt_s * k2.domega_m; - plant->theta_m_rad = st.theta_m + dt_s * k2.dtheta_m; + plant->omega_m_q16 = q16_add(st.omega_m, q16_mul(c->dt_q16, k2.domega_m)); + plant->theta_m_q16 = q16_add(st.theta_m, q16_mul(c->dt_q16, k2.dtheta_m)); } } #else -static void plant_integrate(esp_foc_itl_plant_t *plant, float vu, float vv, float vw, - float dt_s) +static void plant_integrate_q16(esp_foc_itl_plant_t *plant, q16_t vu, q16_t vv, q16_t vw) { - esp_foc_itl_state_t st = { - .id = plant->id_a, - .iq = plant->iq_a, - .omega_m = plant->omega_m_rad_s, - .theta_m = plant->theta_m_rad, + const esp_foc_itl_plant_coeff_t *c = &plant->c; + esp_foc_itl_state_q16_t st = { + .id = plant->id_q16, + .iq = plant->iq_q16, + .omega_m = plant->omega_m_q16, + .theta_m = plant->theta_m_q16, }; - esp_foc_itl_deriv_t k1; - esp_foc_itl_deriv_t k2; - esp_foc_itl_deriv_t k3; - esp_foc_itl_deriv_t k4; - esp_foc_itl_state_t s2; - esp_foc_itl_state_t s3; - esp_foc_itl_state_t s4; - - plant_deriv(plant, &st, vu, vv, vw, &k1); - rk_add_state(&st, &k1, 0.5f * dt_s, &s2); - plant_deriv(plant, &s2, vu, vv, vw, &k2); - rk_add_state(&st, &k2, 0.5f * dt_s, &s3); - plant_deriv(plant, &s3, vu, vv, vw, &k3); - rk_add_state(&st, &k3, dt_s, &s4); - plant_deriv(plant, &s4, vu, vv, vw, &k4); - - plant->id_a = st.id + (dt_s / 6.0f) * (k1.did + 2.0f * k2.did + 2.0f * k3.did + k4.did); - plant->iq_a = st.iq + (dt_s / 6.0f) * (k1.diq + 2.0f * k2.diq + 2.0f * k3.diq + k4.diq); - if (plant->p.locked_rotor) { - plant->omega_m_rad_s = 0.0f; - plant->theta_m_rad = st.theta_m; + esp_foc_itl_deriv_q16_t k1; + esp_foc_itl_deriv_q16_t k2; + esp_foc_itl_deriv_q16_t k3; + esp_foc_itl_deriv_q16_t k4; + esp_foc_itl_state_q16_t s2; + esp_foc_itl_state_q16_t s3; + esp_foc_itl_state_q16_t s4; + q16_t two; + + plant_deriv_q16(plant, &st, vu, vv, vw, &k1); + rk_add_state_q16(&st, &k1, c->dt_half_q16, &s2); + plant_deriv_q16(plant, &s2, vu, vv, vw, &k2); + rk_add_state_q16(&st, &k2, c->dt_half_q16, &s3); + plant_deriv_q16(plant, &s3, vu, vv, vw, &k3); + rk_add_state_q16(&st, &k3, c->dt_q16, &s4); + plant_deriv_q16(plant, &s4, vu, vv, vw, &k4); + + two = q16_from_int(2); + plant->id_q16 = q16_add(st.id, + q16_mul(c->dt_over_6_q16, + q16_add(k1.did, + q16_add(q16_mul(two, k2.did), + q16_add(q16_mul(two, k3.did), k4.did))))); + plant->iq_q16 = q16_add(st.iq, + q16_mul(c->dt_over_6_q16, + q16_add(k1.diq, + q16_add(q16_mul(two, k2.diq), + q16_add(q16_mul(two, k3.diq), k4.diq))))); + if (c->locked_rotor) { + plant->omega_m_q16 = 0; + plant->theta_m_q16 = st.theta_m; } else { - plant->omega_m_rad_s = st.omega_m - + (dt_s / 6.0f) * (k1.domega_m + 2.0f * k2.domega_m + 2.0f * k3.domega_m + k4.domega_m); - plant->theta_m_rad = st.theta_m - + (dt_s / 6.0f) * (k1.dtheta_m + 2.0f * k2.dtheta_m + 2.0f * k3.dtheta_m + k4.dtheta_m); + q16_t dom = q16_add(k1.domega_m, + q16_add(q16_mul(two, k2.domega_m), + q16_add(q16_mul(two, k3.domega_m), k4.domega_m))); + q16_t dth = q16_add(k1.dtheta_m, + q16_add(q16_mul(two, k2.dtheta_m), + q16_add(q16_mul(two, k3.dtheta_m), k4.dtheta_m))); + plant->omega_m_q16 = q16_add(st.omega_m, q16_mul(c->dt_over_6_q16, dom)); + plant->theta_m_q16 = q16_add(st.theta_m, q16_mul(c->dt_over_6_q16, dth)); } } #endif +static void plant_build_coeff(esp_foc_itl_plant_t *plant, const esp_foc_itl_plant_params_t *params) +{ + const float pp = (float)params->pole_pairs; + + plant->c.locked_rotor = params->locked_rotor; + plant->c.delta_connection = params->delta_connection; + plant->c.pole_pairs = params->pole_pairs; + plant->c.r_q16 = q16_from_float(params->r_ohm); + plant->c.l_q16 = q16_from_float(params->l_henry); + plant->c.inv_l_q16 = q16_reciprocal_positive(plant->c.l_q16); + plant->c.psi_f_q16 = q16_from_float(params->kt_nm_per_a / (1.5f * pp)); + plant->c.kt_q16 = q16_from_float(params->kt_nm_per_a); + plant->c.b_q16 = q16_from_float(params->b_nms); + plant->c.j_q16 = q16_from_float(params->j_kgm2); + plant->c.inv_j_q16 = q16_reciprocal_positive(plant->c.j_q16); + plant->c.i_max_q16 = q16_from_float(params->i_max_a); + plant->c.vdc_q16 = q16_from_float(params->vdc_volts); + plant->c.half_vdc_q16 = q16_mul(plant->c.vdc_q16, Q16_HALF); + plant->c.delta_scale_q16 = params->delta_connection ? ESP_FOC_ITL_INV_SQRT3_Q16 : Q16_ONE; +} + void esp_foc_itl_plant_init(esp_foc_itl_plant_t *plant, const esp_foc_itl_plant_params_t *params) { memset(plant, 0, sizeof(*plant)); if (params != NULL) { - plant->p = *params; + plant_build_coeff(plant, params); } esp_foc_itl_plant_reset_parked(plant); } +void esp_foc_itl_plant_set_dt(esp_foc_itl_plant_t *plant, uint32_t pwm_hz) +{ + if (plant == NULL || pwm_hz == 0u) { + return; + } + plant->c.dt_q16 = q16_from_float(1.0f / (float)pwm_hz); + plant->c.dt_half_q16 = q16_mul(plant->c.dt_q16, Q16_HALF); + plant->c.dt_over_6_q16 = q16_mul(plant->c.dt_q16, q16_from_float(1.0f / 6.0f)); +} + void esp_foc_itl_plant_reset_parked(esp_foc_itl_plant_t *plant) { - plant->id_a = 0.0f; - plant->iq_a = 0.0f; - plant->omega_m_rad_s = 0.0f; - plant->theta_m_rad = 0.0f; - plant->iu_a = 0.0f; - plant->iv_a = 0.0f; - plant->iw_a = 0.0f; + plant->id_q16 = 0; + plant->iq_q16 = 0; + plant->omega_m_q16 = 0; + plant->theta_m_q16 = 0; + plant->iu_q16 = 0; + plant->iv_q16 = 0; } -void esp_foc_itl_plant_duties_to_phase_volts(float duty_u, float duty_v, float duty_w, - const esp_foc_itl_plant_t *plant, - float *vu, float *vv, float *vw) +void esp_foc_itl_plant_duties_to_phase_volts(q16_t duty_u, q16_t duty_v, q16_t duty_w, + const esp_foc_itl_plant_t *plant, + q16_t *vu, q16_t *vv, q16_t *vw) { - const float vdc = plant->p.vdc_volts; - const float half = 0.5f * vdc; + const esp_foc_itl_plant_coeff_t *c = &plant->c; - *vu = (duty_u - 0.5f) * vdc; - *vv = (duty_v - 0.5f) * vdc; - *vw = (duty_w - 0.5f) * vdc; + *vu = q16_mul(q16_sub(duty_u, Q16_HALF), c->vdc_q16); + *vv = q16_mul(q16_sub(duty_v, Q16_HALF), c->vdc_q16); + *vw = q16_mul(q16_sub(duty_w, Q16_HALF), c->vdc_q16); - if (plant->p.delta_connection) { - *vu *= FOC_ITL_INV_SQRT3; - *vv *= FOC_ITL_INV_SQRT3; - *vw *= FOC_ITL_INV_SQRT3; + if (c->delta_connection) { + *vu = q16_mul(*vu, c->delta_scale_q16); + *vv = q16_mul(*vv, c->delta_scale_q16); + *vw = q16_mul(*vw, c->delta_scale_q16); } - *vu = clampf(*vu, -half, half); - *vv = clampf(*vv, -half, half); - *vw = clampf(*vw, -half, half); + *vu = q16_clamp(*vu, q16_sub(0, c->half_vdc_q16), c->half_vdc_q16); + *vv = q16_clamp(*vv, q16_sub(0, c->half_vdc_q16), c->half_vdc_q16); + *vw = q16_clamp(*vw, q16_sub(0, c->half_vdc_q16), c->half_vdc_q16); } -void esp_foc_itl_plant_step(esp_foc_itl_plant_t *plant, float vu, float vv, float vw, float dt_s) +void esp_foc_itl_plant_step(esp_foc_itl_plant_t *plant, q16_t vu, q16_t vv, q16_t vw) { - if (plant == NULL || dt_s <= 0.0f) { + if (plant == NULL || plant->c.dt_q16 <= 0) { return; } - plant_integrate(plant, vu, vv, vw, dt_s); - plant_apply_saturation(plant); + plant_integrate_q16(plant, vu, vv, vw); + plant_apply_saturation_q16(plant); - if (!plant->p.locked_rotor) { - if (plant->theta_m_rad >= FOC_ITL_TWO_PI) { - plant->theta_m_rad = fmodf(plant->theta_m_rad, FOC_ITL_TWO_PI); - } else if (plant->theta_m_rad < 0.0f) { - plant->theta_m_rad = fmodf(plant->theta_m_rad, FOC_ITL_TWO_PI) + FOC_ITL_TWO_PI; - } + if (!plant->c.locked_rotor) { + plant->theta_m_q16 = q16_normalize_angle(plant->theta_m_q16); } } @@ -251,13 +292,13 @@ int32_t esp_foc_itl_plant_encoder_ticks(const esp_foc_itl_plant_t *plant) if (plant == NULL) { return 0; } - const float turns = plant->theta_m_rad / FOC_ITL_TWO_PI; - int32_t ticks = (int32_t)lroundf(turns * (float)ESP_FOC_ITL_CPR); - while (ticks < 0) { - ticks += (int32_t)ESP_FOC_ITL_CPR; + int64_t ticks = ((int64_t)plant->theta_m_q16 * (int64_t)ESP_FOC_ITL_CPR) >> Q16_FRAC_BITS; + int32_t t = (int32_t)ticks; + while (t < 0) { + t += (int32_t)ESP_FOC_ITL_CPR; } - while (ticks >= (int32_t)ESP_FOC_ITL_CPR) { - ticks -= (int32_t)ESP_FOC_ITL_CPR; + while (t >= (int32_t)ESP_FOC_ITL_CPR) { + t -= (int32_t)ESP_FOC_ITL_CPR; } - return ticks; + return t; } diff --git a/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.h b/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.h index 6a468c59..92832c9f 100644 --- a/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.h +++ b/source/motor_control/foc_in_the_loop/esp_foc_itl_plant.h @@ -3,13 +3,15 @@ * * Copyright (c) 2021 Felipe Neves * - * Discretized PMSM plant (dq state, abc terminal voltages). Host-testable. + * Discretized PMSM plant (dq state, abc terminal voltages). Hot path is Q16-only. */ #pragma once #include #include +#include "espFoC/utils/esp_foc_q16.h" + #ifdef __cplusplus extern "C" { #endif @@ -30,22 +32,43 @@ typedef struct { } esp_foc_itl_plant_params_t; typedef struct { - esp_foc_itl_plant_params_t p; - float id_a; - float iq_a; - float omega_m_rad_s; - float theta_m_rad; - float iu_a; - float iv_a; - float iw_a; + bool locked_rotor; + bool delta_connection; + int pole_pairs; + q16_t dt_q16; + q16_t dt_half_q16; + q16_t dt_over_6_q16; + q16_t r_q16; + q16_t l_q16; + q16_t inv_l_q16; + q16_t psi_f_q16; + q16_t kt_q16; + q16_t b_q16; + q16_t j_q16; + q16_t inv_j_q16; + q16_t i_max_q16; + q16_t vdc_q16; + q16_t half_vdc_q16; + q16_t delta_scale_q16; +} esp_foc_itl_plant_coeff_t; + +typedef struct { + esp_foc_itl_plant_coeff_t c; + q16_t id_q16; + q16_t iq_q16; + q16_t omega_m_q16; + q16_t theta_m_q16; + q16_t iu_q16; + q16_t iv_q16; } esp_foc_itl_plant_t; void esp_foc_itl_plant_init(esp_foc_itl_plant_t *plant, const esp_foc_itl_plant_params_t *params); +void esp_foc_itl_plant_set_dt(esp_foc_itl_plant_t *plant, uint32_t pwm_hz); void esp_foc_itl_plant_reset_parked(esp_foc_itl_plant_t *plant); -void esp_foc_itl_plant_duties_to_phase_volts(float duty_u, float duty_v, float duty_w, +void esp_foc_itl_plant_duties_to_phase_volts(q16_t duty_u, q16_t duty_v, q16_t duty_w, const esp_foc_itl_plant_t *plant, - float *vu, float *vv, float *vw); -void esp_foc_itl_plant_step(esp_foc_itl_plant_t *plant, float vu, float vv, float vw, float dt_s); + q16_t *vu, q16_t *vv, q16_t *vw); +void esp_foc_itl_plant_step(esp_foc_itl_plant_t *plant, q16_t vu, q16_t vv, q16_t vw); int32_t esp_foc_itl_plant_encoder_ticks(const esp_foc_itl_plant_t *plant); #ifdef __cplusplus diff --git a/test/test_foc_itl_plant.c b/test/test_foc_itl_plant.c index ab856a4f..3466834e 100644 --- a/test/test_foc_itl_plant.c +++ b/test/test_foc_itl_plant.c @@ -8,10 +8,7 @@ #include #include "esp_foc_itl_plant.h" - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif +#include "espFoC/utils/esp_foc_q16.h" static esp_foc_itl_plant_t s_plant; @@ -30,36 +27,35 @@ static void init_locked_plant(float r, float l, float vdc) .delta_connection = false, }; esp_foc_itl_plant_init(&s_plant, &p); + esp_foc_itl_plant_set_dt(&s_plant, 20000); } TEST_CASE("plant: locked rotor reaches steady-state current", "[espFoC][foc_itl_plant]") { init_locked_plant(1.0f, 0.002f, 12.0f); - const float dt = 1.0f / 20000.0f; - const float duty = 0.55f; - const float vu = (duty - 0.5f) * 12.0f; - const float target = vu / 1.0f; + const q16_t vu = q16_mul(q16_sub(q16_from_float(0.55f), Q16_HALF), q16_from_float(12.0f)); + const q16_t target = q16_mul(vu, q16_reciprocal_positive(q16_from_float(1.0f))); for (int i = 0; i < 8000; i++) { - esp_foc_itl_plant_step(&s_plant, vu, 0.0f, -vu, dt); + esp_foc_itl_plant_step(&s_plant, vu, 0, q16_sub(0, vu)); } - TEST_ASSERT_FLOAT_WITHIN(0.5f, target, s_plant.iu_a); - TEST_ASSERT_FLOAT_WITHIN(0.05f, 0.0f, s_plant.omega_m_rad_s); + TEST_ASSERT_INT32_WITHIN(q16_from_float(0.5f), target, s_plant.iu_q16); + TEST_ASSERT_INT32_WITHIN(q16_from_float(0.05f), 0, s_plant.omega_m_q16); } TEST_CASE("plant: current saturation clamps phase current", "[espFoC][foc_itl_plant]") { init_locked_plant(0.01f, 0.0001f, 48.0f); - s_plant.p.i_max_a = 2.0f; - const float dt = 1.0f / 20000.0f; + s_plant.c.i_max_q16 = q16_from_float(2.0f); for (int i = 0; i < 4000; i++) { - esp_foc_itl_plant_step(&s_plant, 20.0f, -10.0f, -10.0f, dt); + esp_foc_itl_plant_step(&s_plant, q16_from_float(20.0f), q16_from_float(-10.0f), + q16_from_float(-10.0f)); } - TEST_ASSERT_LESS_OR_EQUAL(2.01f, fabsf(s_plant.iu_a)); - TEST_ASSERT_LESS_OR_EQUAL(2.01f, fabsf(s_plant.iv_a)); + TEST_ASSERT_LESS_OR_EQUAL(q16_from_float(2.01f), q16_abs(s_plant.iu_q16)); + TEST_ASSERT_LESS_OR_EQUAL(q16_from_float(2.01f), q16_abs(s_plant.iv_q16)); } TEST_CASE("plant: free rotor step response does not diverge", "[espFoC][foc_itl_plant]") @@ -77,21 +73,22 @@ TEST_CASE("plant: free rotor step response does not diverge", "[espFoC][foc_itl_ .delta_connection = false, }; esp_foc_itl_plant_init(&s_plant, &p); + esp_foc_itl_plant_set_dt(&s_plant, 20000); - const float dt = 1.0f / 20000.0f; for (int i = 0; i < 20000; i++) { - esp_foc_itl_plant_step(&s_plant, 1.0f, -0.5f, -0.5f, dt); + esp_foc_itl_plant_step(&s_plant, q16_from_float(1.0f), q16_from_float(-0.5f), + q16_from_float(-0.5f)); } - TEST_ASSERT_LESS_OR_EQUAL(10.01f, fabsf(s_plant.iu_a)); - TEST_ASSERT(isfinite(s_plant.omega_m_rad_s)); - TEST_ASSERT(isfinite(s_plant.theta_m_rad)); + TEST_ASSERT_LESS_OR_EQUAL(q16_from_float(10.01f), q16_abs(s_plant.iu_q16)); + TEST_ASSERT(s_plant.omega_m_q16 != (q16_t)INT32_MIN); + TEST_ASSERT(s_plant.theta_m_q16 >= 0); } TEST_CASE("plant: encoder ticks wrap at CPR", "[espFoC][foc_itl_plant]") { init_locked_plant(1.0f, 0.002f, 12.0f); - s_plant.theta_m_rad = (float)(2.0 * M_PI * 0.99); + s_plant.theta_m_q16 = q16_from_float(0.99f); const int32_t ticks = esp_foc_itl_plant_encoder_ticks(&s_plant); TEST_ASSERT_GREATER_OR_EQUAL(0, ticks); TEST_ASSERT_LESS_THAN((int)ESP_FOC_ITL_CPR, ticks);