From 11fd27ae2cdee7df1a9090c84be6b9643583a57b Mon Sep 17 00:00:00 2001 From: Felipe Neves Date: Mon, 1 Jun 2026 13:00:56 -0300 Subject: [PATCH 1/2] Replace esp_adc continuous with HAL/DMA unified isensor driver. Drop esp_adc dependency so current sense uses adc_hal, per-target DMA fragments, init-time cali LUT, and software-triggered 2-channel patterns. --- CMakeLists.txt | 17 +- examples/axis_tuning/main/CMakeLists.txt | 2 +- examples/axis_tuning/main/main.c | 10 +- .../main/test_current_sensor.c | 5 +- include/espFoC/esp_foc_adc_cali_lut.h | 29 + include/espFoC/esp_foc_err.h | 1 + source/drivers/cali/esp_foc_adc_cali.h | 32 + source/drivers/cali/esp_foc_adc_cali_curve.c | 181 ++++++ source/drivers/cali/esp_foc_adc_cali_line.c | 85 +++ .../drivers/cali/esp_foc_cali_curve_coeff.c | 100 ++++ source/drivers/current_sensor_adc.c | 560 +++++++++++------- source/drivers/espFoC/current_sensor_adc.h | 37 +- source/drivers/esp_foc_adc_cali_lut.c | 62 ++ source/drivers/isensor_adc_dma_esp32.c | 125 ++++ source/drivers/isensor_adc_dma_esp32s2.c | 131 ++++ source/drivers/isensor_adc_dma_gdma.c | 141 +++++ source/drivers/isensor_adc_internal.h | 39 ++ test/test_isensor_adc.c | 35 ++ 18 files changed, 1327 insertions(+), 265 deletions(-) create mode 100644 include/espFoC/esp_foc_adc_cali_lut.h create mode 100644 source/drivers/cali/esp_foc_adc_cali.h create mode 100644 source/drivers/cali/esp_foc_adc_cali_curve.c create mode 100644 source/drivers/cali/esp_foc_adc_cali_line.c create mode 100644 source/drivers/cali/esp_foc_cali_curve_coeff.c create mode 100644 source/drivers/esp_foc_adc_cali_lut.c create mode 100644 source/drivers/isensor_adc_dma_esp32.c create mode 100644 source/drivers/isensor_adc_dma_esp32s2.c create mode 100644 source/drivers/isensor_adc_dma_gdma.c create mode 100644 source/drivers/isensor_adc_internal.h create mode 100644 test/test_isensor_adc.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 948b35b2..7b5fc2b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,7 @@ if(ESP_PLATFORM) idf_build_get_property(target IDF_TARGET) -set(requires esp_driver_uart esp_driver_pcnt esp_driver_gpio esp_driver_mcpwm esp_adc driver freertos esp_system esp_timer nvs_flash) +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 @@ -32,9 +32,24 @@ list(APPEND srcs "source/drivers/inverter_3pwm_mcpwm.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_cali_line.c" + "source/drivers/cali/esp_foc_cali_curve_coeff.c" "source/drivers/current_sensor_adc_one_shot.c" "source/osal/os_interface_idf.c") +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() + if(CONFIG_ESP_FOC_TUNER_ENABLE) list(APPEND srcs "source/gui_link/esp_foc_tuner.c" "source/gui_link/esp_foc_link_session.c") diff --git a/examples/axis_tuning/main/CMakeLists.txt b/examples/axis_tuning/main/CMakeLists.txt index 7e7a6ffa..2538a831 100644 --- a/examples/axis_tuning/main/CMakeLists.txt +++ b/examples/axis_tuning/main/CMakeLists.txt @@ -1,2 +1,2 @@ idf_component_register(SRC_DIRS "." - PRIV_REQUIRES espFoC esp_system freertos esp_driver_i2c esp_adc) + PRIV_REQUIRES espFoC esp_system freertos esp_driver_i2c) diff --git a/examples/axis_tuning/main/main.c b/examples/axis_tuning/main/main.c index 640c30f1..ff4eeb8c 100644 --- a/examples/axis_tuning/main/main.c +++ b/examples/axis_tuning/main/main.c @@ -151,13 +151,11 @@ void app_main(void) s_shunts = isensor_adc_oneshot_new(&shunt_cfg, NULL); #else esp_foc_isensor_adc_config_t shunt_cfg = { - .axis_channels = {(adc_channel_t)CONFIG_AXIS_TUNING_ISENSE_CH_U, - (adc_channel_t)CONFIG_AXIS_TUNING_ISENSE_CH_V}, - .units = {(adc_unit_t)(CONFIG_AXIS_TUNING_ISENSE_ADC_UNIT - 1), - (adc_unit_t)(CONFIG_AXIS_TUNING_ISENSE_ADC_UNIT - 1)}, - .amp_gain = (float)CONFIG_AXIS_TUNING_ISENSE_AMP_GAIN_X100 / 100.0f, + .channels = {(adc_channel_t)CONFIG_AXIS_TUNING_ISENSE_CH_U, + (adc_channel_t)CONFIG_AXIS_TUNING_ISENSE_CH_V}, + .unit = ADC_UNIT_1, + .amp_gain = (float)CONFIG_AXIS_TUNING_ISENSE_AMP_GAIN_X100 / 100.0f, .shunt_resistance = (float)CONFIG_AXIS_TUNING_ISENSE_SHUNT_MOHM / 1000.0f, - .number_of_channels = 2, }; s_shunts = isensor_adc_new(&shunt_cfg); #endif diff --git a/examples/test_drivers/test_current_sense/main/test_current_sensor.c b/examples/test_drivers/test_current_sense/main/test_current_sensor.c index f198c3ec..8131397b 100644 --- a/examples/test_drivers/test_current_sense/main/test_current_sensor.c +++ b/examples/test_drivers/test_current_sense/main/test_current_sensor.c @@ -17,11 +17,10 @@ static esp_foc_isensor_t *shunts; static void initialize_foc_drivers(void) { esp_foc_isensor_adc_config_t shunt_cfg = { - .axis_channels = {ADC_CHANNEL_1, ADC_CHANNEL_5}, - .units = {ADC_UNIT_1, ADC_UNIT_1}, + .channels = {ADC_CHANNEL_1, ADC_CHANNEL_5}, + .unit = ADC_UNIT_1, .amp_gain = 50.0f, .shunt_resistance = 0.01f, - .number_of_channels = 2, }; shunts = isensor_adc_new(&shunt_cfg); diff --git a/include/espFoC/esp_foc_adc_cali_lut.h b/include/espFoC/esp_foc_adc_cali_lut.h new file mode 100644 index 00000000..4819eba9 --- /dev/null +++ b/include/espFoC/esp_foc_adc_cali_lut.h @@ -0,0 +1,29 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + */ +#pragma once + +#include +#include +#include "hal/adc_types.h" + +#define ESP_FOC_ISENSOR_ADC_LUT_SIZE 4096 + +bool esp_foc_adc_cali_lut_build(adc_unit_t unit, + adc_channel_t channel, + adc_atten_t atten, + int16_t *lut_out, + unsigned lut_len); + +static inline int32_t esp_foc_adc_cali_lut_apply(const int16_t *lut, int32_t raw12) +{ + if (raw12 < 0) { + raw12 = 0; + } + if (raw12 >= 4096) { + raw12 = 4095; + } + return (int32_t)lut[raw12]; +} diff --git a/include/espFoC/esp_foc_err.h b/include/espFoC/esp_foc_err.h index 1faa4964..d3173de5 100644 --- a/include/espFoC/esp_foc_err.h +++ b/include/espFoC/esp_foc_err.h @@ -14,6 +14,7 @@ typedef enum { ESP_FOC_ERR_TIMESTEP_TOO_SMALL = -5, ESP_FOC_ERR_ROTOR_STARTUP = -6, ESP_FOC_ERR_ROTOR_STARTUP_PI = -7, + ESP_FOC_ERR_NOT_SUPPORTED = -8, ESP_FOC_ERR_UNKNOWN = -128 } esp_foc_err_t; diff --git a/source/drivers/cali/esp_foc_adc_cali.h b/source/drivers/cali/esp_foc_adc_cali.h new file mode 100644 index 00000000..3dbf36c2 --- /dev/null +++ b/source/drivers/cali/esp_foc_adc_cali.h @@ -0,0 +1,32 @@ +/* + * SPDX-FileCopyrightText: 2019-2023 Espressif Systems (Shanghai) CO LTD + * SPDX-License-Identifier: Apache-2.0 + * + * Ported for espFoC isensor ADC LUT init (no esp_adc runtime dependency). + */ +#pragma once + +#include +#include "hal/adc_types.h" + +typedef struct { + uint8_t term_num; + const uint64_t (*coeff)[2]; + const int32_t (*sign); +} esp_foc_cali_chars_second_step_t; + +void esp_foc_curve_fitting_get_second_step_coeff(adc_unit_t unit, + adc_atten_t atten, + esp_foc_cali_chars_second_step_t *ctx); + +bool esp_foc_adc_cali_curve_raw_to_mv(adc_unit_t unit, + adc_channel_t channel, + adc_atten_t atten, + int raw, + int *mv_out); + +bool esp_foc_adc_cali_line_raw_to_mv(adc_unit_t unit, + adc_channel_t channel, + adc_atten_t atten, + int raw, + int *mv_out); diff --git a/source/drivers/cali/esp_foc_adc_cali_curve.c b/source/drivers/cali/esp_foc_adc_cali_curve.c new file mode 100644 index 00000000..39f2ff0b --- /dev/null +++ b/source/drivers/cali/esp_foc_adc_cali_curve.c @@ -0,0 +1,181 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + * + * Curve-fitting ADC calibration (ported from ESP-IDF, init-time LUT only). + */ + +#include +#include +#include +#include "esp_err.h" +#include "sdkconfig.h" +#include "esp_efuse_rtc_calib.h" +#include "esp_private/adc_share_hw_ctrl.h" +#include "esp_foc_adc_cali.h" + +#if SOC_ADC_CALIB_SCHEME_CURVE_FITTING_SUPPORTED + +static const int coeff_a_scaling = 65536; + +typedef struct { + uint32_t voltage; + uint32_t digi; +} esp_foc_adc_calib_data_ver1_t; + +typedef struct { + char version_num; + adc_unit_t unit_id; + adc_atten_t atten; + esp_foc_adc_calib_data_ver1_t ver1; +} esp_foc_adc_calib_info_t; + +typedef struct { + uint32_t coeff_a; + uint32_t coeff_b; +} esp_foc_cali_chars_first_step_t; + +typedef struct { + adc_unit_t unit_id; + adc_channel_t chan; + adc_atten_t atten; + esp_foc_cali_chars_first_step_t first; + esp_foc_cali_chars_second_step_t second; +} esp_foc_cali_curve_ctx_t; + +static esp_foc_cali_curve_ctx_t s_curve_ctx; + +static int32_t get_reading_error(uint64_t v_cali_1, const esp_foc_cali_chars_second_step_t *param) +{ + if (v_cali_1 == 0 || param->term_num == 0) { + return 0; + } + + uint8_t term_num = param->term_num; + int32_t error = 0; + uint64_t coeff = 0; + uint64_t variable[3]; + uint64_t term[3]; + + variable[0] = 1; + coeff = param->coeff[0][0]; + term[0] = variable[0] * coeff / param->coeff[0][1]; + error = (int32_t)term[0] * param->sign[0]; + + for (int i = 1; i < term_num; i++) { + variable[i] = variable[i - 1] * v_cali_1; + coeff = param->coeff[i][0]; + term[i] = variable[i] * coeff; + term[i] = term[i] / param->coeff[i][1]; + error += (int32_t)term[i] * param->sign[i]; + } + + return error; +} + +static void get_first_step_reference_point(int version_num, + adc_unit_t unit_id, + adc_atten_t atten, + esp_foc_adc_calib_info_t *calib_info) +{ + calib_info->version_num = (char)version_num; + calib_info->unit_id = unit_id; + calib_info->atten = atten; + + uint32_t voltage = 0; + uint32_t digi = 0; + esp_err_t ret = esp_efuse_rtc_calib_get_cal_voltage(version_num, unit_id, (int)atten, &digi, &voltage); + if (ret != ESP_OK) { + calib_info->ver1.voltage = 0; + calib_info->ver1.digi = 0; + return; + } + calib_info->ver1.voltage = voltage; + calib_info->ver1.digi = digi; +} + +static void calc_first_step_coefficients(const esp_foc_adc_calib_info_t *parsed, + esp_foc_cali_curve_ctx_t *ctx) +{ + if (parsed->ver1.digi == 0) { + ctx->first.coeff_a = coeff_a_scaling; + ctx->first.coeff_b = 0; + return; + } + ctx->first.coeff_a = coeff_a_scaling * parsed->ver1.voltage / parsed->ver1.digi; + ctx->first.coeff_b = 0; +} + +static bool curve_ctx_prepare(adc_unit_t unit, adc_channel_t channel, adc_atten_t atten) +{ + uint32_t ver = esp_efuse_rtc_calib_get_ver(); + if (ver < ESP_EFUSE_ADC_CALIB_VER_MIN || ver > ESP_EFUSE_ADC_CALIB_VER_MAX) { + return false; + } + + esp_foc_adc_calib_info_t info = {0}; + get_first_step_reference_point((int)ver, unit, atten, &info); + if (info.ver1.digi == 0) { + return false; + } + + calc_first_step_coefficients(&info, &s_curve_ctx); + esp_foc_curve_fitting_get_second_step_coeff(unit, atten, &s_curve_ctx.second); + s_curve_ctx.unit_id = unit; + s_curve_ctx.chan = channel; + s_curve_ctx.atten = atten; + return true; +} + +bool esp_foc_adc_cali_curve_raw_to_mv(adc_unit_t unit, + adc_channel_t channel, + adc_atten_t atten, + int raw, + int *mv_out) +{ + if (mv_out == NULL) { + return false; + } + if (s_curve_ctx.unit_id != unit || s_curve_ctx.chan != channel || s_curve_ctx.atten != atten) { + if (!curve_ctx_prepare(unit, channel, atten)) { + return false; + } + } + +#if SOC_ADC_CALIB_CHAN_COMPENS_SUPPORTED + int chan_comp = adc_get_hw_calibration_chan_compens(unit, channel, atten); + raw -= chan_comp; + int max_val = (1 << SOC_ADC_RTC_MAX_BITWIDTH) - 1; + if (raw < 0) { + raw = 0; + } + if (raw > max_val) { + raw = max_val; + } +#endif + + uint64_t v_cali_1 = (uint64_t)raw * s_curve_ctx.first.coeff_a / coeff_a_scaling + + s_curve_ctx.first.coeff_b; + int32_t error = get_reading_error(v_cali_1, &s_curve_ctx.second); + *mv_out = (int32_t)v_cali_1 - error; + return true; +} + +#else + +bool esp_foc_adc_cali_curve_raw_to_mv(adc_unit_t unit, + adc_channel_t channel, + adc_atten_t atten, + int raw, + int *mv_out) +{ + (void)unit; + (void)channel; + (void)atten; + (void)raw; + (void)mv_out; + return false; +} + +#endif diff --git a/source/drivers/cali/esp_foc_adc_cali_line.c b/source/drivers/cali/esp_foc_adc_cali_line.c new file mode 100644 index 00000000..c98c52be --- /dev/null +++ b/source/drivers/cali/esp_foc_adc_cali_line.c @@ -0,0 +1,85 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + * + * Line-fitting ADC calibration for ESP32 / ESP32-S2 (init-time LUT only). + */ + +#include +#include "sdkconfig.h" +#include "hal/adc_types.h" +#include "hal/efuse_ll.h" +#include "esp_foc_adc_cali.h" + +#if (CONFIG_IDF_TARGET_ESP32 || CONFIG_IDF_TARGET_ESP32S2) && SOC_ADC_CALIB_SCHEME_LINE_FITTING_SUPPORTED + +#define LIN_COEFF_A_SCALE 65536 +#define LIN_COEFF_A_ROUND (LIN_COEFF_A_SCALE / 2) + +static bool line_coeffs_ready; +static uint32_t s_coeff_a; +static int32_t s_coeff_b; + +static bool line_coeffs_init(adc_unit_t unit, adc_atten_t atten) +{ + if (line_coeffs_ready) { + return true; + } + + uint32_t coeff_a = 0; + int32_t coeff_b = 0; + if (efuse_ll_get_adc_calib_ver() == 0) { + return false; + } + + if (unit == ADC_UNIT_1) { + coeff_a = efuse_ll_get_adc_calib_cali_val(atten); + coeff_b = efuse_ll_get_adc_calib_offset(atten); + } else { + coeff_a = efuse_ll_get_adc2_calib_cali_val(atten); + coeff_b = efuse_ll_get_adc2_calib_offset(atten); + } + + if (coeff_a == 0) { + return false; + } + + s_coeff_a = coeff_a; + s_coeff_b = coeff_b; + line_coeffs_ready = true; + return true; +} + +bool esp_foc_adc_cali_line_raw_to_mv(adc_unit_t unit, + adc_channel_t channel, + adc_atten_t atten, + int raw, + int *mv_out) +{ + (void)channel; + if (mv_out == NULL || !line_coeffs_init(unit, atten)) { + return false; + } + + *mv_out = (int)(((int64_t)raw * s_coeff_a + LIN_COEFF_A_ROUND) / LIN_COEFF_A_SCALE) + s_coeff_b; + return true; +} + +#else + +bool esp_foc_adc_cali_line_raw_to_mv(adc_unit_t unit, + adc_channel_t channel, + adc_atten_t atten, + int raw, + int *mv_out) +{ + (void)unit; + (void)channel; + (void)atten; + (void)raw; + (void)mv_out; + return false; +} + +#endif diff --git a/source/drivers/cali/esp_foc_cali_curve_coeff.c b/source/drivers/cali/esp_foc_cali_curve_coeff.c new file mode 100644 index 00000000..a60c43c1 --- /dev/null +++ b/source/drivers/cali/esp_foc_cali_curve_coeff.c @@ -0,0 +1,100 @@ +/* + * SPDX-FileCopyrightText: 2023 Espressif Systems (Shanghai) CO LTD + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include "sdkconfig.h" +#include "esp_foc_adc_cali.h" + +#if SOC_ADC_CALIB_SCHEME_CURVE_FITTING_SUPPORTED +#include "esp_efuse_rtc_calib.h" + +#define COEFF_VERSION_NUM 2 // Currently C6 has two versions of curve calibration schemes +#define COEFF_GROUP_NUM 4 +#define TERM_MAX 3 + +/** + * @note Error Calculation + * Coefficients for calculating the reading voltage error. + * Four sets of coefficients for atten0 ~ atten3 respectively. + * + * For each item, first element is the Coefficient, second element is the Multiple. (Coefficient / Multiple) is the real coefficient. + * + * @note {0,0} stands for unused item + * @note In case of the overflow, these coefficients are recorded as Absolute Value + * @note For atten0 ~ 3, error = (K0 * X^0) + (K1 * X^1) + (K2 * X^2) + * @note Above formula is rewritten from the original documentation, please note that the coefficients are re-ordered. + */ +const static uint64_t adc1_error_coef_atten[COEFF_VERSION_NUM][COEFF_GROUP_NUM][TERM_MAX][2] = { + /* Coefficients of calibration version 1 */ + { + {{487166399931449, 1e15}, {6436483033201, 1e16}, {30410131806, 1e16}}, //atten0 + {{8665498165817785, 1e16}, {15239070452946, 1e16}, {13818878844, 1e16}}, //atten1 + {{12277821756674387, 1e16}, {22275554717885, 1e16}, {5924302667, 1e16}}, //atten2 + {{3801417550380255, 1e16}, {6020352420772, 1e16}, {12442478488, 1e16}}, //atten3 + }, + /* Coefficients of calibration version 2 */ + { + {{0, 0}, {0, 0}, {0, 0}}, //atten0 + {{0, 0}, {0, 0}, {0, 0}}, //atten1 + {{12217864764388775, 1e16}, {1954123107752, 1e16}, {6409679727, 1e16}}, //atten2 + {{3915910437042445, 1e16}, {31536470857564, 1e16}, {12493873014, 1e16}}, //atten3 + }, +}; + +/** + * Term sign + */ +const static int32_t adc1_error_sign[COEFF_VERSION_NUM][COEFF_GROUP_NUM][TERM_MAX] = { + /* Coefficient sign of calibration version 1 */ + { + {-1, 1, 1}, //atten0 + {-1, 1, 1}, //atten1 + {-1, 1, 1}, //atten2 + {-1, -1, 1}, //atten3 + }, + /* Coefficient sign of calibration version 2 */ + { + { 0, 0, 0}, //atten0 + { 0, 0, 0}, //atten1 + {-1, -1, 1}, //atten2 + {-1, -1, 1}, //atten3 + }, +}; + +void esp_foc_curve_fitting_get_second_step_coeff(adc_unit_t unit, + adc_atten_t atten, + esp_foc_cali_chars_second_step_t *ctx) +{ + (void)unit; + uint32_t adc_calib_ver = esp_efuse_rtc_calib_get_ver(); + assert((adc_calib_ver >= ESP_EFUSE_ADC_CALIB_VER_MIN) && + (adc_calib_ver <= ESP_EFUSE_ADC_CALIB_VER_MAX)); + if (adc_calib_ver == ESP_EFUSE_ADC_CALIB_VER2 && atten < 2) { + ctx->term_num = 0; + ctx->coeff = NULL; + ctx->sign = NULL; + } else { + ctx->term_num = 3; + ctx->coeff = adc1_error_coef_atten[VER2IDX(adc_calib_ver)][atten]; + ctx->sign = adc1_error_sign[VER2IDX(adc_calib_ver)][atten]; + } +} + +#else + +void esp_foc_curve_fitting_get_second_step_coeff(adc_unit_t unit, + adc_atten_t atten, + esp_foc_cali_chars_second_step_t *ctx) +{ + (void)unit; + (void)atten; + ctx->term_num = 0; + ctx->coeff = NULL; + ctx->sign = NULL; +} + +#endif /* SOC_ADC_CALIB_SCHEME_CURVE_FITTING_SUPPORTED */ diff --git a/source/drivers/current_sensor_adc.c b/source/drivers/current_sensor_adc.c index 5c1d5da7..11e71f48 100644 --- a/source/drivers/current_sensor_adc.c +++ b/source/drivers/current_sensor_adc.c @@ -3,36 +3,38 @@ * * Copyright (c) 2021 Felipe Neves * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. + * Unified ADC current-sense driver: adc_hal digi + target DMA fragment (no esp_adc). */ -#include #include -#include +#include #include +#include "sdkconfig.h" +#include "esp_log.h" +#include "esp_check.h" +#include "esp_heap_caps.h" +#include "esp_intr_alloc.h" +#include "esp_clk_tree.h" +#include "esp_private/regi2c_ctrl.h" +#include "esp_private/sar_periph_ctrl.h" +#include "esp_private/adc_share_hw_ctrl.h" +#include "esp_private/gpio.h" +#include "soc/adc_periph.h" +#include "soc/soc_caps.h" +#include "soc/clk_tree_defs.h" +#include "hal/adc_hal.h" +#include "hal/adc_ll.h" #include "espFoC/utils/esp_foc_q16.h" #include "espFoC/utils/biquad_q16.h" #include "espFoC/utils/foc_math_q16.h" #include "espFoC/driver_q16_local.h" #include "espFoC/current_sensor_adc.h" -#include "hal/adc_hal.h" -#include "esp_log.h" +#include "espFoC/esp_foc_adc_cali_lut.h" +#include "isensor_adc_internal.h" + +#if SOC_CACHE_INTERNAL_MEM_VIA_L1CACHE +#include "esp_cache.h" +#endif #if CONFIG_IDF_TARGET_ESP32 || CONFIG_IDF_TARGET_ESP32S2 #define ADC_GET_CHANNEL(p_data) ((p_data)->type1.channel) @@ -42,75 +44,86 @@ #define ADC_GET_DATA(p_data) ((p_data)->type2.data) #endif +#define ISENSOR_ADC_DMA_DESC_ALIGN 4 +#define ISENSOR_ADC_FRAME_BYTES (ESP_FOC_ISENSOR_ADC_NUM_CHANNELS * SOC_ADC_DIGI_RESULT_BYTES) +#define ISENSOR_ADC_CONVERT_LIMIT 2 + static const char *TAG = "ESP_FOC_ISENSOR"; typedef struct { - adc_channel_t channels[4]; - adc_unit_t units[4]; - adc_continuous_handle_t handle; + adc_hal_dma_ctx_t hal; + adc_hal_digi_ctrlr_cfg_t hal_cfg; + adc_digi_pattern_config_t patterns[ESP_FOC_ISENSOR_ADC_NUM_CHANNELS]; + uint8_t *rx_buf; + uint32_t rx_desc_size; + isensor_adc_dma_ctx_t dma_ctx; + adc_channel_t channels[ESP_FOC_ISENSOR_ADC_NUM_CHANNELS]; + adc_unit_t unit; + adc_atten_t atten; float adc_to_current_scale; q16_t adc_to_current_scale_q16; - /* latest_raw is the most recent unfiltered ADC sample per channel. - * Used by calibration (we want the raw DC at no-current). */ - int32_t latest_raw[4]; - /* filtered_count is bq[i] output mapped back into ADC count units - * (still int32 because the rest of the pipeline subtracts an int - * offset before scaling). */ - int32_t filtered_count[4]; - esp_foc_biquad_q16_t bq[4]; - float offsets[4]; - /* Rounded offset in raw counts; used in the ADC ISR (no float). */ - int32_t offset_counts[4]; - /* Optional Clarke publish targets. When both are non-NULL the ADC - * ISR computes (offset, gain, Clarke) on channels 0/1 right after - * the biquad and atomic-writes the result. The PWM ISR can then - * read i_alpha / i_beta directly without a fetch_isensors() call. */ + int16_t cali_lut[ESP_FOC_ISENSOR_ADC_NUM_CHANNELS][ESP_FOC_ISENSOR_ADC_LUT_SIZE]; + int32_t latest_raw[ESP_FOC_ISENSOR_ADC_NUM_CHANNELS]; + int32_t filtered_count[ESP_FOC_ISENSOR_ADC_NUM_CHANNELS]; + esp_foc_biquad_q16_t bq[ESP_FOC_ISENSOR_ADC_NUM_CHANNELS]; + float offsets[ESP_FOC_ISENSOR_ADC_NUM_CHANNELS]; + int32_t offset_counts[ESP_FOC_ISENSOR_ADC_NUM_CHANNELS]; volatile q16_t *publish_alpha; volatile q16_t *publish_beta; - /* Same-tick Iu, Iv (axis amperes, q16) for scope — optional. */ volatile q16_t *publish_iu; volatile q16_t *publish_iv; esp_foc_isensor_t interface; - int number_of_channels; + esp_foc_isensor_adc_trigger_t trigger; + esp_foc_isensor_adc_state_t state; isensor_callback_t callback; void *user_data; -}isensor_adc_t; + bool started; +} isensor_adc_t; + +DRAM_ATTR static isensor_adc_t s_isensor; +static bool s_adc_initialized; -static adc_continuous_handle_cfg_t adc_config = { - .max_store_buf_size = 2 * SOC_ADC_DIGI_RESULT_BYTES, - .conv_frame_size = 2 * SOC_ADC_DIGI_RESULT_BYTES, - .flags = { - .flush_pool = 1, - }, -}; +static const float adc_to_volts = 3.1f / 4096.0f; -static adc_continuous_config_t dig_cfg = { +static adc_bitwidth_t isensor_adc_digi_bitwidth(void) +{ #if CONFIG_IDF_TARGET_ESP32 - .sample_freq_hz = 80000, + return ADC_BITWIDTH_12; #else - .sample_freq_hz = 80000, + return SOC_ADC_DIGI_MAX_BITWIDTH; #endif - .conv_mode = ADC_CONV_SINGLE_UNIT_1, - .pattern_num = 2, -#if CONFIG_IDF_TARGET_ESP32 || CONFIG_IDF_TARGET_ESP32S2 - .format = ADC_DIGI_OUTPUT_FORMAT_TYPE1, +} + +static adc_atten_t isensor_adc_default_atten(void) +{ +#if CONFIG_IDF_TARGET_ESP32 + return ADC_ATTEN_DB_11; #else - .format = ADC_DIGI_OUTPUT_FORMAT_TYPE2, + return ADC_ATTEN_DB_12; #endif -}; +} -static adc_digi_pattern_config_t adc_pattern[SOC_ADC_PATT_LEN_MAX] = {0}; +static void isensor_adc_digi_apply_convert_limit(void) +{ + adc_ll_digi_set_convert_limit_num(ISENSOR_ADC_CONVERT_LIMIT); + adc_ll_digi_convert_limit_enable(true); +} -DRAM_ATTR static isensor_adc_t isensor_adc; -static bool adc_initialized = false; -static const float adc_to_volts = ((3.1f)/ 4096.0f); +static esp_err_t isensor_adc_gpio_init(adc_unit_t unit, uint32_t chan_mask) +{ + while (chan_mask) { + int ch = __builtin_ctz(chan_mask); + int8_t io = adc_channel_io_map[unit][ch]; + if (io < 0) { + return ESP_ERR_INVALID_ARG; + } + gpio_config_as_analog(io); + chan_mask &= ~(1U << ch); + } + return ESP_OK; +} -/* Apply offset + gain + Clarke on channels 0/1 and atomic-write - * (i_alpha, i_beta) into the publish targets. Inlined into the ISR - * for the new ISR-driven hot path to skip; called only when both - * targets are wired. Channels 2/3 (axis 1) are not handled here — - * Plan #2 is single-axis for now. */ -static inline void adc_publish_alpha_beta(isensor_adc_t *obj) +static inline void isensor_adc_publish_alpha_beta(isensor_adc_t *obj) { const int32_t adc_rng = 2048; int32_t d0 = obj->filtered_count[0] - obj->offset_counts[0]; @@ -121,16 +134,11 @@ static inline void adc_publish_alpha_beta(isensor_adc_t *obj) obj->adc_to_current_scale_q16); q16_t iv = q16_mul(esp_foc_q16_from_adc_diff_clamped(d1, adc_rng), obj->adc_to_current_scale_q16); - /* Three-phase KCL gives iw, then the standard Clarke (with the - * codebase's q16_clarke helper which expects u/v/w). */ q16_t iw = q16_sub(0, q16_add(iu, iv)); q16_t alpha, beta; q16_clarke(iu, iv, iw, &alpha, &beta); - /* Single 32-bit aligned writes; both Xtensa and RISC-V deliver - * these as one store, so the PWM ISR can read either field - * without a torn-read concern. */ *obj->publish_alpha = alpha; - *obj->publish_beta = beta; + *obj->publish_beta = beta; if (obj->publish_iu != NULL) { *obj->publish_iu = iu; } @@ -139,65 +147,159 @@ static inline void adc_publish_alpha_beta(isensor_adc_t *obj) } } -static bool isensor_adc_done_callback(adc_continuous_handle_t handle, const adc_continuous_evt_data_t *edata, void *user_data) +static void IRAM_ATTR isensor_adc_process_frame(isensor_adc_t *isensor, const uint8_t *frame, uint32_t size) { - adc_hal_digi_enable(false); - adc_hal_digi_connect(false); - - isensor_adc_t *isensor = (isensor_adc_t *)user_data; - adc_digi_output_data_t *p = (adc_digi_output_data_t *)edata->conv_frame_buffer; + if (frame == NULL || size < ISENSOR_ADC_FRAME_BYTES) { + return; + } - /* Push every fresh ADC sample straight through its per-channel - * biquad. Raw counts (12-bit) shift-left into Q16 with plenty of - * headroom (4095 << 16 = 268 M, well below INT32_MAX). The output - * shifts back to count units; the rest of the pipeline never sees - * the q16 representation. */ - for(int i = 0; i < isensor->number_of_channels; i++) { + adc_digi_output_data_t *p = (adc_digi_output_data_t *)frame; + for (int i = 0; i < ESP_FOC_ISENSOR_ADC_NUM_CHANNELS; i++) { int32_t raw = (int32_t)ADC_GET_DATA(p); + raw = esp_foc_adc_cali_lut_apply(isensor->cali_lut[i], raw); isensor->latest_raw[i] = raw; - q16_t y = esp_foc_biquad_q16_update(&isensor->bq[i], - (q16_t)(raw << 16)); + q16_t y = esp_foc_biquad_q16_update(&isensor->bq[i], (q16_t)(raw << 16)); isensor->filtered_count[i] = y >> 16; p++; } - if(isensor->publish_alpha != NULL && isensor->publish_beta != NULL) { - adc_publish_alpha_beta(isensor); + if (isensor->publish_alpha != NULL && isensor->publish_beta != NULL) { + isensor_adc_publish_alpha_beta(isensor); } - if(isensor->callback != NULL) { + if (isensor->callback != NULL) { isensor->callback(isensor->user_data); } +} + +static void IRAM_ATTR isensor_adc_dma_done(void *arg) +{ + isensor_adc_t *isensor = (isensor_adc_t *)arg; + adc_hal_dma_desc_status_t status; + uint8_t *finished_buffer = NULL; + uint32_t finished_size = 0; + + while (1) { + status = adc_hal_get_reading_result(&isensor->hal, isensor->dma_ctx.eof_desc_addr, + &finished_buffer, &finished_size); + if (status != ADC_HAL_DMA_DESC_VALID) { + break; + } +#if SOC_CACHE_INTERNAL_MEM_VIA_L1CACHE + esp_cache_msync(finished_buffer, finished_size, ESP_CACHE_MSYNC_FLAG_DIR_M2C); +#endif + isensor_adc_process_frame(isensor, finished_buffer, finished_size); + } + +#if SOC_CACHE_INTERNAL_MEM_VIA_L1CACHE + esp_cache_msync(isensor->hal.rx_desc, isensor->rx_desc_size, + ESP_CACHE_MSYNC_FLAG_DIR_C2M | ESP_CACHE_MSYNC_FLAG_INVALIDATE); +#endif - return false; + adc_hal_digi_enable(false); + adc_hal_digi_connect(false); + isensor->state = ESP_FOC_ISENSOR_ADC_STATE_IDLE; } -static const adc_continuous_evt_cbs_t cbs = { - .on_conv_done = isensor_adc_done_callback, -}; +static esp_err_t isensor_adc_hw_start(isensor_adc_t *isensor) +{ + ANALOG_CLOCK_ENABLE(); + + ADC_BUS_CLK_ATOMIC() { + adc_ll_reset_register(); + } + + sar_periph_ctrl_adc_continuous_power_acquire(); + adc_lock_acquire(isensor->unit); + +#if SOC_ADC_CALIBRATION_V1_SUPPORTED + adc_hal_calibration_init(isensor->unit); + adc_set_hw_calibration_code(isensor->unit, isensor->atten); +#endif + + adc_hal_set_controller(isensor->unit, ADC_HAL_CONTINUOUS_READ_MODE); + +#if !CONFIG_IDF_TARGET_ESP32 + ESP_ERROR_CHECK(esp_clk_tree_enable_src((soc_module_clk_t)isensor->hal_cfg.clk_src, true)); +#endif + + adc_hal_digi_init(&isensor->hal); + adc_hal_digi_controller_config(&isensor->hal, &isensor->hal_cfg); + isensor_adc_digi_apply_convert_limit(); + adc_hal_digi_enable(false); + adc_hal_digi_connect(false); + + isensor_adc_dma_stop(&isensor->dma_ctx); + adc_hal_digi_reset(); + adc_hal_digi_dma_link(&isensor->hal, isensor->rx_buf); + +#if SOC_CACHE_INTERNAL_MEM_VIA_L1CACHE + esp_cache_msync(isensor->hal.rx_desc, isensor->rx_desc_size, ESP_CACHE_MSYNC_FLAG_DIR_C2M); +#endif + + isensor_adc_dma_start(&isensor->dma_ctx, isensor->hal.rx_desc); + adc_hal_digi_connect(true); + adc_hal_digi_enable(true); + isensor->state = ESP_FOC_ISENSOR_ADC_STATE_BUSY; + isensor->started = true; + return ESP_OK; +} -static void continuous_adc_init(isensor_adc_t *isensor) +static esp_err_t isensor_adc_setup_hal(isensor_adc_t *isensor) { - adc_config.conv_frame_size = isensor->number_of_channels * SOC_ADC_DIGI_RESULT_BYTES; - adc_continuous_new_handle(&adc_config, &isensor->handle); - - for (int i = 0; i < isensor->number_of_channels; i++) { - adc_pattern[i].atten = ADC_ATTEN_DB_12; - adc_pattern[i].channel = isensor->channels[i] & 0x7; - adc_pattern[i].unit = isensor_adc.units[0]; - adc_pattern[i].bit_width = SOC_ADC_DIGI_MAX_BITWIDTH; - - ESP_LOGI(TAG, "adc_pattern[%d].atten is :%"PRIx8, i, adc_pattern[i].atten); - ESP_LOGI(TAG, "adc_pattern[%d].channel is :%"PRIx8, i, adc_pattern[i].channel); - ESP_LOGI(TAG, "adc_pattern[%d].unit is :%"PRIx8, i, adc_pattern[i].unit); + uint32_t clk_hz = 0; + esp_clk_tree_src_get_freq_hz(ADC_DIGI_CLK_SRC_DEFAULT, ESP_CLK_TREE_SRC_FREQ_PRECISION_CACHED, &clk_hz); + + isensor->hal_cfg.adc_pattern = isensor->patterns; + isensor->hal_cfg.adc_pattern_len = ESP_FOC_ISENSOR_ADC_NUM_CHANNELS; + isensor->hal_cfg.sample_freq_hz = ESP_FOC_ISENSOR_ADC_PATTERN_HZ; + isensor->hal_cfg.conv_mode = ADC_CONV_SINGLE_UNIT_1; + isensor->hal_cfg.clk_src = ADC_DIGI_CLK_SRC_DEFAULT; + isensor->hal_cfg.clk_src_freq_hz = clk_hz; + + uint32_t chan_mask = 0; + for (int i = 0; i < ESP_FOC_ISENSOR_ADC_NUM_CHANNELS; i++) { + isensor->patterns[i].atten = isensor->atten; + isensor->patterns[i].channel = isensor->channels[i] & 0x7; + isensor->patterns[i].unit = isensor->unit; + isensor->patterns[i].bit_width = isensor_adc_digi_bitwidth(); + chan_mask |= BIT(isensor->patterns[i].channel); + ESP_LOGI(TAG, "pattern[%d] unit=%d ch=%d atten=%d", i, + (int)isensor->patterns[i].unit, (int)isensor->patterns[i].channel, + (int)isensor->patterns[i].atten); } - dig_cfg.adc_pattern = adc_pattern; - dig_cfg.pattern_num = isensor->number_of_channels; - adc_continuous_config(isensor->handle, &dig_cfg); - adc_continuous_register_event_callbacks(isensor->handle, &cbs, isensor); - adc_continuous_start(isensor->handle); - esp_foc_sleep_ms(10); + adc_hal_dma_config_t dma_cfg = { + .eof_desc_num = 1, + .eof_step = 1, + .eof_num = ESP_FOC_ISENSOR_ADC_NUM_CHANNELS, + }; + adc_hal_dma_ctx_config(&isensor->hal, &dma_cfg); + + isensor->rx_buf = heap_caps_calloc(1, ISENSOR_ADC_FRAME_BYTES, + MALLOC_CAP_INTERNAL | MALLOC_CAP_DMA | MALLOC_CAP_8BIT); + if (isensor->rx_buf == NULL) { + return ESP_ERR_NO_MEM; + } + + isensor->hal.rx_desc = heap_caps_aligned_calloc(ISENSOR_ADC_DMA_DESC_ALIGN, 1, + sizeof(dma_descriptor_t), + MALLOC_CAP_INTERNAL | MALLOC_CAP_DMA | MALLOC_CAP_8BIT); + if (isensor->hal.rx_desc == NULL) { + return ESP_ERR_NO_MEM; + } + +#if SOC_CACHE_INTERNAL_MEM_VIA_L1CACHE + uint32_t line = 4; + isensor->rx_desc_size = (sizeof(dma_descriptor_t) + line - 1) & ~(line - 1); +#else + isensor->rx_desc_size = sizeof(dma_descriptor_t); +#endif + + ESP_RETURN_ON_ERROR(isensor_adc_gpio_init(isensor->unit, chan_mask), TAG, "gpio init failed"); + ESP_RETURN_ON_ERROR(isensor_adc_dma_init(&isensor->dma_ctx, isensor_adc_dma_done, isensor), + TAG, "dma init failed"); + return ESP_OK; } static void fetch_isensors(esp_foc_isensor_t *self, isensor_values_t *values) @@ -205,111 +307,96 @@ static void fetch_isensors(esp_foc_isensor_t *self, isensor_values_t *values) isensor_adc_t *obj = __containerof(self, isensor_adc_t, interface); const int32_t adc_rng = 2048; - /* Pull the latest filtered counts produced by the per-channel - * biquad in the ADC ISR. */ - int32_t f0 = obj->filtered_count[0]; - int32_t f1 = obj->filtered_count[1]; - int32_t f2 = obj->filtered_count[2]; - int32_t f3 = obj->filtered_count[3]; - - int32_t d0 = f0 - obj->offset_counts[0]; - int32_t d1 = f1 - obj->offset_counts[1]; - int32_t d2 = f2 - obj->offset_counts[2]; - int32_t d3 = f3 - obj->offset_counts[3]; - + int32_t d0 = obj->filtered_count[0] - obj->offset_counts[0]; + int32_t d1 = obj->filtered_count[1] - obj->offset_counts[1]; d0 = esp_foc_clamp_int32(d0, -adc_rng, adc_rng); d1 = esp_foc_clamp_int32(d1, -adc_rng, adc_rng); - d2 = esp_foc_clamp_int32(d2, -adc_rng, adc_rng); - d3 = esp_foc_clamp_int32(d3, -adc_rng, adc_rng); q16_t iu0 = q16_mul(esp_foc_q16_from_adc_diff_clamped(d0, adc_rng), obj->adc_to_current_scale_q16); q16_t iv0 = q16_mul(esp_foc_q16_from_adc_diff_clamped(d1, adc_rng), obj->adc_to_current_scale_q16); - q16_t iu1 = q16_mul(esp_foc_q16_from_adc_diff_clamped(d2, adc_rng), obj->adc_to_current_scale_q16); - q16_t iv1 = q16_mul(esp_foc_q16_from_adc_diff_clamped(d3, adc_rng), obj->adc_to_current_scale_q16); - q16_t zero = 0; q16_t iw0 = q16_sub(zero, q16_add(iu0, iv0)); - q16_t iw1 = q16_sub(zero, q16_add(iu1, iv1)); values->iu_axis_0 = iu0; values->iv_axis_0 = iv0; values->iw_axis_0 = iw0; - values->iu_axis_1 = iu1; - values->iv_axis_1 = iv1; - values->iw_axis_1 = iw1; + values->iu_axis_1 = 0; + values->iv_axis_1 = 0; + values->iw_axis_1 = 0; } static void sample_isensors(esp_foc_isensor_t *self) { + isensor_adc_t *obj = __containerof(self, isensor_adc_t, interface); + + if (obj->trigger != ESP_FOC_ISENSOR_ADC_TRIG_SOFTWARE) { + return; + } + if (obj->state == ESP_FOC_ISENSOR_ADC_STATE_BUSY) { + return; + } + + if (!obj->started) { + isensor_adc_hw_start(obj); + return; + } + + isensor_adc_dma_reset(&obj->dma_ctx); + adc_hal_digi_reset(); + adc_hal_digi_dma_link(&obj->hal, obj->rx_buf); +#if SOC_CACHE_INTERNAL_MEM_VIA_L1CACHE + esp_cache_msync(obj->hal.rx_desc, obj->rx_desc_size, ESP_CACHE_MSYNC_FLAG_DIR_C2M); +#endif + isensor_adc_dma_start(&obj->dma_ctx, obj->hal.rx_desc); adc_hal_digi_connect(true); adc_hal_digi_enable(true); + obj->state = ESP_FOC_ISENSOR_ADC_STATE_BUSY; } -static void calibrate_isensors (esp_foc_isensor_t *self, int calibration_rounds) +static void calibrate_isensors(esp_foc_isensor_t *self, int calibration_rounds) { - isensor_values_t val; - isensor_adc_t *obj = - __containerof(self, isensor_adc_t, interface); + isensor_adc_t *obj = __containerof(self, isensor_adc_t, interface); + + if (obj->trigger != ESP_FOC_ISENSOR_ADC_TRIG_SOFTWARE) { + return; + } + isensor_values_t val; esp_foc_sleep_ms(100); - obj->offsets[0] = 0.0f; - obj->offsets[1] = 0.0f; - obj->offsets[2] = 0.0f; - obj->offsets[3] = 0.0f; - for (int oi = 0; oi < 4; oi++) { + for (int oi = 0; oi < ESP_FOC_ISENSOR_ADC_NUM_CHANNELS; oi++) { + obj->offsets[oi] = 0.0f; obj->offset_counts[oi] = 0; } - for(int i = 0; i < calibration_rounds; i++) { + for (int i = 0; i < calibration_rounds; i++) { self->sample_isensors(self); esp_foc_sleep_ms(10); - - /* Use the unfiltered latest_raw value here on purpose: we want - * the genuine zero-current DC level, not whatever the biquad - * happens to be settling towards (the loop is still cold and - * the filter has not had time to converge across rounds). */ - obj->offsets[0] += ((float)obj->latest_raw[0]); - obj->offsets[1] += ((float)obj->latest_raw[1]); - obj->offsets[2] += ((float)obj->latest_raw[2]); - obj->offsets[3] += ((float)obj->latest_raw[3]); + for (int ch = 0; ch < ESP_FOC_ISENSOR_ADC_NUM_CHANNELS; ch++) { + obj->offsets[ch] += (float)obj->latest_raw[ch]; + } } - obj->offsets[0] /= calibration_rounds; - obj->offsets[1] /= calibration_rounds; - obj->offsets[2] /= calibration_rounds; - obj->offsets[3] /= calibration_rounds; - for (int oi = 0; oi < 4; oi++) { - obj->offset_counts[oi] = (int32_t)lroundf(obj->offsets[oi]); + for (int ch = 0; ch < ESP_FOC_ISENSOR_ADC_NUM_CHANNELS; ch++) { + obj->offsets[ch] /= calibration_rounds; + obj->offset_counts[ch] = (int32_t)lroundf(obj->offsets[ch]); + esp_foc_biquad_q16_reset(&obj->bq[ch]); + obj->filtered_count[ch] = obj->latest_raw[ch]; } - /* Clear filter state so the post-calibration steady state lines up - * with the freshly-measured offsets. */ - for (int i = 0; i < 4; ++i) { - esp_foc_biquad_q16_reset(&obj->bq[i]); - obj->filtered_count[i] = obj->latest_raw[i]; - } - - ESP_LOGI(TAG, "ADC calibrated, phase current offsets are: %f, %f, %f, %f", - obj->offsets[0], obj->offsets[1], obj->offsets[2], obj->offsets[3]); + ESP_LOGI(TAG, "ADC calibrated, offsets: %f, %f", obj->offsets[0], obj->offsets[1]); esp_foc_sleep_ms(100); - /* Dummy read to check reading when no current is flowing*/ self->sample_isensors(self); esp_foc_sleep_ms(10); self->fetch_isensors(self, &val); - - ESP_LOGI(TAG, "No current flow isensor test read: %f, %f, %f, %f, %f, %f", - q16_to_float(val.iu_axis_0), q16_to_float(val.iv_axis_0), q16_to_float(val.iw_axis_0), - q16_to_float(val.iu_axis_1), q16_to_float(val.iv_axis_1), q16_to_float(val.iw_axis_1)); - esp_foc_sleep_ms(100); + ESP_LOGI(TAG, "No-current test: iu=%f iv=%f iw=%f", + q16_to_float(val.iu_axis_0), q16_to_float(val.iv_axis_0), q16_to_float(val.iw_axis_0)); } static void set_callback(esp_foc_isensor_t *self, isensor_callback_t cb, void *arg) { - isensor_adc_t *obj = - __containerof(self, isensor_adc_t, interface); - + isensor_adc_t *obj = __containerof(self, isensor_adc_t, interface); esp_foc_critical_enter(); obj->callback = cb; obj->user_data = arg; @@ -318,13 +405,9 @@ static void set_callback(esp_foc_isensor_t *self, isensor_callback_t cb, void *a static void set_filter_cutoff(esp_foc_isensor_t *self, float fc_hz, float fs_hz) { - isensor_adc_t *obj = - __containerof(self, isensor_adc_t, interface); - - /* Designer is float-heavy; do it under critical section so the ADC - * ISR cannot land mid-redesign and use partially-updated coefs. */ + isensor_adc_t *obj = __containerof(self, isensor_adc_t, interface); esp_foc_critical_enter(); - for (int i = 0; i < 4; ++i) { + for (int i = 0; i < ESP_FOC_ISENSOR_ADC_NUM_CHANNELS; ++i) { esp_foc_biquad_butterworth_lpf_design_q16(&obj->bq[i], fc_hz, fs_hz); } esp_foc_critical_leave(); @@ -336,64 +419,81 @@ static void set_publish_targets(esp_foc_isensor_t *self, q16_t *i_u_target, q16_t *i_v_target) { - isensor_adc_t *obj = - __containerof(self, isensor_adc_t, interface); - + isensor_adc_t *obj = __containerof(self, isensor_adc_t, interface); esp_foc_critical_enter(); obj->publish_alpha = (volatile q16_t *)i_alpha_target; - obj->publish_beta = (volatile q16_t *)i_beta_target; + obj->publish_beta = (volatile q16_t *)i_beta_target; obj->publish_iu = (volatile q16_t *)i_u_target; obj->publish_iv = (volatile q16_t *)i_v_target; esp_foc_critical_leave(); } +esp_foc_err_t esp_foc_isensor_adc_set_trigger(esp_foc_isensor_t *isensor, + esp_foc_isensor_adc_trigger_t mode) +{ + if (isensor == NULL) { + return ESP_FOC_ERR_INVALID_ARG; + } + if (mode == ESP_FOC_ISENSOR_ADC_TRIG_ETM) { + return ESP_FOC_ERR_NOT_SUPPORTED; + } + isensor_adc_t *obj = __containerof(isensor, isensor_adc_t, interface); + obj->trigger = mode; + return ESP_FOC_OK; +} esp_foc_isensor_t *isensor_adc_new(esp_foc_isensor_adc_config_t *config) { + if (config == NULL) { + return NULL; + } + if (s_adc_initialized) { + return &s_isensor.interface; + } - if(adc_initialized == true) { - return &isensor_adc.interface; + if (config->unit != ADC_UNIT_1) { + ESP_LOGE(TAG, "only ADC1 supported in v1"); + return NULL; + } + if (config->amp_gain <= 0.0f || config->shunt_resistance <= 0.0f) { + ESP_LOGE(TAG, "invalid amp_gain or shunt_resistance"); + return NULL; } - isensor_adc.adc_to_current_scale = adc_to_volts * (1.0f / (config->amp_gain * config->shunt_resistance)); - isensor_adc.adc_to_current_scale_q16 = q16_from_float(2048.0f * isensor_adc.adc_to_current_scale); - - isensor_adc.interface.fetch_isensors = fetch_isensors; - isensor_adc.interface.sample_isensors = sample_isensors; - isensor_adc.interface.calibrate_isensors = calibrate_isensors; - isensor_adc.interface.set_isensor_callback = set_callback; - isensor_adc.interface.set_filter_cutoff = set_filter_cutoff; - isensor_adc.interface.set_publish_targets = set_publish_targets; - isensor_adc.publish_alpha = NULL; - isensor_adc.publish_beta = NULL; - isensor_adc.publish_iu = NULL; - isensor_adc.publish_iv = NULL; - /* Default to bypass so the driver works correctly even if the - * caller (axis init / tuner) never invokes set_filter_cutoff. */ - for (int i = 0; i < 4; ++i) { - esp_foc_biquad_q16_set_bypass(&isensor_adc.bq[i]); + memset(&s_isensor, 0, sizeof(s_isensor)); + s_isensor.unit = config->unit; + s_isensor.atten = isensor_adc_default_atten(); + s_isensor.channels[0] = config->channels[0]; + s_isensor.channels[1] = config->channels[1]; + s_isensor.trigger = ESP_FOC_ISENSOR_ADC_TRIG_SOFTWARE; + s_isensor.state = ESP_FOC_ISENSOR_ADC_STATE_IDLE; + + s_isensor.adc_to_current_scale = adc_to_volts * (1.0f / (config->amp_gain * config->shunt_resistance)); + s_isensor.adc_to_current_scale_q16 = q16_from_float(2048.0f * s_isensor.adc_to_current_scale); + + for (int ch = 0; ch < ESP_FOC_ISENSOR_ADC_NUM_CHANNELS; ch++) { + esp_foc_adc_cali_lut_build(s_isensor.unit, s_isensor.channels[ch], s_isensor.atten, + s_isensor.cali_lut[ch], ESP_FOC_ISENSOR_ADC_LUT_SIZE); + esp_foc_biquad_q16_set_bypass(&s_isensor.bq[ch]); } - isensor_adc.number_of_channels = config->number_of_channels; - isensor_adc.channels[0] = config->axis_channels[0]; - isensor_adc.channels[1] = config->axis_channels[1]; - isensor_adc.channels[2] = config->axis_channels[2]; - isensor_adc.channels[3] = config->axis_channels[3]; - isensor_adc.units[0] = config->units[0]; - isensor_adc.units[1] = config->units[1]; - isensor_adc.units[2] = config->units[2]; - isensor_adc.units[3] = config->units[3]; - isensor_adc.offsets[0] = 0.0f; - isensor_adc.offsets[1] = 0.0f; - isensor_adc.offsets[2] = 0.0f; - isensor_adc.offsets[3] = 0.0f; - isensor_adc.offset_counts[0] = 0; - isensor_adc.offset_counts[1] = 0; - isensor_adc.offset_counts[2] = 0; - isensor_adc.offset_counts[3] = 0; - isensor_adc.callback = NULL; - - continuous_adc_init(&isensor_adc); - adc_initialized = true; - - return &isensor_adc.interface; + + s_isensor.interface.fetch_isensors = fetch_isensors; + s_isensor.interface.sample_isensors = sample_isensors; + s_isensor.interface.calibrate_isensors = calibrate_isensors; + s_isensor.interface.set_isensor_callback = set_callback; + s_isensor.interface.set_filter_cutoff = set_filter_cutoff; + s_isensor.interface.set_publish_targets = set_publish_targets; + + adc_apb_periph_claim(); + + esp_err_t err = isensor_adc_setup_hal(&s_isensor); + if (err != ESP_OK) { + ESP_LOGE(TAG, "ADC HAL setup failed: %d", err); + return NULL; + } + + isensor_adc_hw_start(&s_isensor); + esp_foc_sleep_ms(10); + s_adc_initialized = true; + return &s_isensor.interface; } diff --git a/source/drivers/espFoC/current_sensor_adc.h b/source/drivers/espFoC/current_sensor_adc.h index 11f8e8a2..7a40172d 100644 --- a/source/drivers/espFoC/current_sensor_adc.h +++ b/source/drivers/espFoC/current_sensor_adc.h @@ -2,38 +2,27 @@ * MIT License * * Copyright (c) 2021 Felipe Neves - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. */ #pragma once #include "espFoC/esp_foc.h" -#include "esp_adc/adc_continuous.h" -#include "esp_err.h" +#include "hal/adc_types.h" +#include "espFoC/esp_foc_err.h" typedef struct { - adc_channel_t axis_channels[4]; - adc_unit_t units[4]; + adc_channel_t channels[2]; + adc_unit_t unit; float amp_gain; float shunt_resistance; - int number_of_channels; -}esp_foc_isensor_adc_config_t; +} esp_foc_isensor_adc_config_t; + +typedef enum { + ESP_FOC_ISENSOR_ADC_TRIG_SOFTWARE = 0, + ESP_FOC_ISENSOR_ADC_TRIG_ETM, +} esp_foc_isensor_adc_trigger_t; esp_foc_isensor_t *isensor_adc_new(esp_foc_isensor_adc_config_t *config); + +esp_foc_err_t esp_foc_isensor_adc_set_trigger(esp_foc_isensor_t *isensor, + esp_foc_isensor_adc_trigger_t mode); diff --git a/source/drivers/esp_foc_adc_cali_lut.c b/source/drivers/esp_foc_adc_cali_lut.c new file mode 100644 index 00000000..2946cc1f --- /dev/null +++ b/source/drivers/esp_foc_adc_cali_lut.c @@ -0,0 +1,62 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + */ + +#include +#include "esp_log.h" +#include "espFoC/esp_foc_adc_cali_lut.h" +#include "cali/esp_foc_adc_cali.h" + +static const char *TAG = "esp_foc_adc_cali_lut"; + +#define ESP_FOC_ADC_REF_MV 3300 + +static bool raw_to_mv(adc_unit_t unit, + adc_channel_t channel, + adc_atten_t atten, + int raw, + int *mv_out) +{ +#if (CONFIG_IDF_TARGET_ESP32 || CONFIG_IDF_TARGET_ESP32S2) + return esp_foc_adc_cali_line_raw_to_mv(unit, channel, atten, raw, mv_out); +#else + (void)unit; + return esp_foc_adc_cali_curve_raw_to_mv(unit, channel, atten, raw, mv_out); +#endif +} + +bool esp_foc_adc_cali_lut_build(adc_unit_t unit, + adc_channel_t channel, + adc_atten_t atten, + int16_t *lut_out, + unsigned lut_len) +{ + if (lut_out == NULL || lut_len < ESP_FOC_ISENSOR_ADC_LUT_SIZE) { + return false; + } + + bool any_cali = false; + for (unsigned raw = 0; raw < ESP_FOC_ISENSOR_ADC_LUT_SIZE; raw++) { + int mv = 0; + if (raw_to_mv(unit, channel, atten, (int)raw, &mv)) { + int linear = (mv * (int)ESP_FOC_ISENSOR_ADC_LUT_SIZE) / ESP_FOC_ADC_REF_MV; + if (linear < 0) { + linear = 0; + } + if (linear >= (int)ESP_FOC_ISENSOR_ADC_LUT_SIZE) { + linear = (int)ESP_FOC_ISENSOR_ADC_LUT_SIZE - 1; + } + lut_out[raw] = (int16_t)linear; + any_cali = true; + } else { + lut_out[raw] = (int16_t)raw; + } + } + + if (!any_cali) { + ESP_LOGW(TAG, "ADC cali LUT identity (unit=%d ch=%d atten=%d)", (int)unit, (int)channel, (int)atten); + } + return any_cali; +} diff --git a/source/drivers/isensor_adc_dma_esp32.c b/source/drivers/isensor_adc_dma_esp32.c new file mode 100644 index 00000000..f0e9f0ea --- /dev/null +++ b/source/drivers/isensor_adc_dma_esp32.c @@ -0,0 +1,125 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + * + * I2S DMA backend for ADC digi on ESP32 (i2s_ll only). + */ + +#include "sdkconfig.h" +#include "esp_err.h" +#include "esp_intr_alloc.h" +#include "esp_attr.h" +#include "hal/i2s_ll.h" +#include "soc/i2s_periph.h" +#include "esp_private/i2s_platform.h" +#include "isensor_adc_internal.h" + +#define ADC_DMA_I2S_HOST ADC_HAL_DMA_I2S_HOST +#define ADC_DMA_INTR_MASK BIT(9) + +static i2s_dev_t *s_i2s_dev; +static isensor_adc_dma_ctx_t *s_ctx; +static intr_handle_t s_intr; +static bool s_inited; + +static void IRAM_ATTR isensor_adc_i2s_isr(void *arg) +{ + (void)arg; + if ((i2s_ll_get_intr_status(s_i2s_dev) & ADC_DMA_INTR_MASK) == 0) { + return; + } + i2s_ll_clear_intr_status(s_i2s_dev, ADC_DMA_INTR_MASK); + + if (s_ctx != NULL) { + uint32_t desc_addr = 0; + i2s_ll_rx_get_eof_des_addr(s_i2s_dev, &desc_addr); + s_ctx->eof_desc_addr = (intptr_t)desc_addr; + if (s_ctx->on_done != NULL) { + s_ctx->on_done(s_ctx->on_done_arg); + } + } +} + +esp_err_t isensor_adc_dma_init(isensor_adc_dma_ctx_t *ctx, + isensor_adc_dma_done_fn_t on_done, + void *user) +{ + if (ctx == NULL || on_done == NULL) { + return ESP_ERR_INVALID_ARG; + } + if (s_inited) { + ctx->on_done = on_done; + ctx->on_done_arg = user; + s_ctx = ctx; + return ESP_OK; + } + + esp_err_t err = i2s_platform_acquire_occupation(I2S_CTLR_HP, ADC_DMA_I2S_HOST, "esp_foc_isensor"); + if (err != ESP_OK) { + return err; + } + s_i2s_dev = I2S_LL_GET_HW(ADC_DMA_I2S_HOST); + + err = esp_intr_alloc(i2s_periph_signal[ADC_DMA_I2S_HOST].irq, ESP_INTR_FLAG_IRAM, + isensor_adc_i2s_isr, NULL, &s_intr); + if (err != ESP_OK) { + i2s_platform_release_occupation(I2S_CTLR_HP, ADC_DMA_I2S_HOST); + return err; + } + + ctx->on_done = on_done; + ctx->on_done_arg = user; + s_ctx = ctx; + s_inited = true; + return ESP_OK; +} + +esp_err_t isensor_adc_dma_deinit(isensor_adc_dma_ctx_t *ctx) +{ + (void)ctx; + if (!s_inited) { + return ESP_OK; + } + isensor_adc_dma_stop(ctx); + if (s_intr != NULL) { + esp_intr_free(s_intr); + s_intr = NULL; + } + i2s_platform_release_occupation(I2S_CTLR_HP, ADC_DMA_I2S_HOST); + s_ctx = NULL; + s_inited = false; + return ESP_OK; +} + +esp_err_t isensor_adc_dma_start(isensor_adc_dma_ctx_t *ctx, dma_descriptor_t *desc) +{ + (void)ctx; + i2s_ll_clear_intr_status(s_i2s_dev, ADC_DMA_INTR_MASK); + i2s_ll_enable_intr(s_i2s_dev, ADC_DMA_INTR_MASK, true); + i2s_ll_enable_dma(s_i2s_dev, true); + i2s_ll_rx_start_link(s_i2s_dev, (uint32_t)desc); + return ESP_OK; +} + +esp_err_t isensor_adc_dma_stop(isensor_adc_dma_ctx_t *ctx) +{ + (void)ctx; + if (!s_inited) { + return ESP_OK; + } + i2s_ll_enable_intr(s_i2s_dev, ADC_DMA_INTR_MASK, false); + i2s_ll_clear_intr_status(s_i2s_dev, ADC_DMA_INTR_MASK); + i2s_ll_rx_stop_link(s_i2s_dev); + return ESP_OK; +} + +esp_err_t isensor_adc_dma_reset(isensor_adc_dma_ctx_t *ctx) +{ + (void)ctx; + if (!s_inited) { + return ESP_OK; + } + i2s_ll_rx_reset_dma(s_i2s_dev); + return ESP_OK; +} diff --git a/source/drivers/isensor_adc_dma_esp32s2.c b/source/drivers/isensor_adc_dma_esp32s2.c new file mode 100644 index 00000000..79c033cd --- /dev/null +++ b/source/drivers/isensor_adc_dma_esp32s2.c @@ -0,0 +1,131 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + * + * SPI3 DMA backend for ADC digi on ESP32-S2 (spi_ll + spicommon). + */ + +#include "sdkconfig.h" +#include "esp_err.h" +#include "esp_intr_alloc.h" +#include "esp_attr.h" +#include "hal/spi_ll.h" +#include "esp_private/spi_common_internal.h" +#include "isensor_adc_internal.h" + +#define ADC_DMA_SPI_HOST SPI3_HOST +#define ADC_DMA_INTR_MASK SPI_LL_INTR_IN_SUC_EOF + +static spi_dev_t *s_spi_dev; +static spi_dma_ctx_t *s_spi_dma; +static isensor_adc_dma_ctx_t *s_ctx; +static intr_handle_t s_intr; +static bool s_inited; + +static void IRAM_ATTR isensor_adc_spi_isr(void *arg) +{ + (void)arg; + if (!spi_ll_get_intr(s_spi_dev, ADC_DMA_INTR_MASK)) { + return; + } + spi_ll_clear_intr(s_spi_dev, ADC_DMA_INTR_MASK); + + if (s_ctx != NULL) { + s_ctx->eof_desc_addr = spi_dma_ll_get_in_suc_eof_desc_addr(s_spi_dev, + s_spi_dma->rx_dma_chan.chan_id); + if (s_ctx->on_done != NULL) { + s_ctx->on_done(s_ctx->on_done_arg); + } + } +} + +esp_err_t isensor_adc_dma_init(isensor_adc_dma_ctx_t *ctx, + isensor_adc_dma_done_fn_t on_done, + void *user) +{ + if (ctx == NULL || on_done == NULL) { + return ESP_ERR_INVALID_ARG; + } + if (s_inited) { + ctx->on_done = on_done; + ctx->on_done_arg = user; + s_ctx = ctx; + return ESP_OK; + } + + if (!spicommon_periph_claim(ADC_DMA_SPI_HOST, "esp_foc_isensor")) { + return ESP_FAIL; + } + + esp_err_t err = spicommon_dma_chan_alloc(ADC_DMA_SPI_HOST, SPI_DMA_CH_AUTO, &s_spi_dma); + if (err != ESP_OK) { + spicommon_periph_free(ADC_DMA_SPI_HOST); + return err; + } + + s_spi_dev = SPI_LL_GET_HW(ADC_DMA_SPI_HOST); + err = esp_intr_alloc(spicommon_irqdma_source_for_host(ADC_DMA_SPI_HOST), ESP_INTR_FLAG_IRAM, + isensor_adc_spi_isr, NULL, &s_intr); + if (err != ESP_OK) { + spicommon_dma_chan_free(s_spi_dma); + spicommon_periph_free(ADC_DMA_SPI_HOST); + return err; + } + + ctx->on_done = on_done; + ctx->on_done_arg = user; + s_ctx = ctx; + s_inited = true; + return ESP_OK; +} + +esp_err_t isensor_adc_dma_deinit(isensor_adc_dma_ctx_t *ctx) +{ + (void)ctx; + if (!s_inited) { + return ESP_OK; + } + isensor_adc_dma_stop(ctx); + if (s_intr != NULL) { + esp_intr_free(s_intr); + s_intr = NULL; + } + spicommon_dma_chan_free(s_spi_dma); + spicommon_periph_free(ADC_DMA_SPI_HOST); + s_spi_dma = NULL; + s_ctx = NULL; + s_inited = false; + return ESP_OK; +} + +esp_err_t isensor_adc_dma_start(isensor_adc_dma_ctx_t *ctx, dma_descriptor_t *desc) +{ + (void)ctx; + spi_ll_clear_intr(s_spi_dev, ADC_DMA_INTR_MASK); + spi_ll_enable_intr(s_spi_dev, ADC_DMA_INTR_MASK); + spi_dma_ll_rx_start(s_spi_dev, s_spi_dma->rx_dma_chan.chan_id, (lldesc_t *)desc); + return ESP_OK; +} + +esp_err_t isensor_adc_dma_stop(isensor_adc_dma_ctx_t *ctx) +{ + (void)ctx; + if (!s_inited) { + return ESP_OK; + } + spi_ll_disable_intr(s_spi_dev, ADC_DMA_INTR_MASK); + spi_ll_clear_intr(s_spi_dev, ADC_DMA_INTR_MASK); + spi_dma_ll_rx_stop(s_spi_dev, s_spi_dma->rx_dma_chan.chan_id); + return ESP_OK; +} + +esp_err_t isensor_adc_dma_reset(isensor_adc_dma_ctx_t *ctx) +{ + (void)ctx; + if (!s_inited) { + return ESP_OK; + } + spi_dma_ll_rx_reset(s_spi_dev, s_spi_dma->rx_dma_chan.chan_id); + return ESP_OK; +} diff --git a/source/drivers/isensor_adc_dma_gdma.c b/source/drivers/isensor_adc_dma_gdma.c new file mode 100644 index 00000000..b667f447 --- /dev/null +++ b/source/drivers/isensor_adc_dma_gdma.c @@ -0,0 +1,141 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + * + * GDMA RX backend for ADC digi (ESP32-S3, C3, C6, …) using gdma_hal + gdma_ll only. + */ + +#include +#include "sdkconfig.h" +#include "esp_err.h" +#include "esp_intr_alloc.h" +#include "esp_attr.h" +#include "hal/gdma_hal.h" +#include "hal/gdma_hal_ahb.h" +#include "hal/gdma_ll.h" +#include "hal/gdma_types.h" +#include "soc/gdma_periph.h" +#include "soc/lldesc.h" +#include "isensor_adc_internal.h" + +#define ISENSOR_ADC_GDMA_GROUP 0 +#define ISENSOR_ADC_GDMA_PAIR 2 +#define ISENSOR_ADC_GDMA_RX_EOF GDMA_LL_EVENT_RX_SUC_EOF + +static gdma_hal_context_t s_gdma_hal; +static isensor_adc_dma_ctx_t *s_ctx; +static intr_handle_t s_intr; +static bool s_inited; + +static void IRAM_ATTR isensor_adc_gdma_isr(void *arg) +{ + (void)arg; + gdma_hal_context_t *hal = &s_gdma_hal; + const int ch = ISENSOR_ADC_GDMA_PAIR; + + uint32_t st = hal->read_intr_status(hal, ch, GDMA_CHANNEL_DIRECTION_RX, true); + if ((st & ISENSOR_ADC_GDMA_RX_EOF) == 0) { + return; + } + hal->clear_intr(hal, ch, GDMA_CHANNEL_DIRECTION_RX, ISENSOR_ADC_GDMA_RX_EOF); + + if (s_ctx != NULL) { + s_ctx->eof_desc_addr = (intptr_t)hal->get_eof_desc_addr(hal, ch, GDMA_CHANNEL_DIRECTION_RX, true); + if (s_ctx->on_done != NULL) { + s_ctx->on_done(s_ctx->on_done_arg); + } + } +} + +esp_err_t isensor_adc_dma_init(isensor_adc_dma_ctx_t *ctx, + isensor_adc_dma_done_fn_t on_done, + void *user) +{ + if (ctx == NULL || on_done == NULL) { + return ESP_ERR_INVALID_ARG; + } + if (s_inited) { + ctx->on_done = on_done; + ctx->on_done_arg = user; + s_ctx = ctx; + return ESP_OK; + } + + gdma_ll_enable_bus_clock(ISENSOR_ADC_GDMA_GROUP, true); + gdma_ll_reset_register(ISENSOR_ADC_GDMA_GROUP); + + gdma_hal_config_t hal_cfg = { + .group_id = ISENSOR_ADC_GDMA_GROUP, + }; + gdma_ahb_hal_init(&s_gdma_hal, &hal_cfg); + + const int ch = ISENSOR_ADC_GDMA_PAIR; + s_gdma_hal.reset(&s_gdma_hal, ch, GDMA_CHANNEL_DIRECTION_RX); + s_gdma_hal.connect_peri(&s_gdma_hal, ch, GDMA_CHANNEL_DIRECTION_RX, + GDMA_TRIG_PERIPH_ADC, 0); + s_gdma_hal.set_strategy(&s_gdma_hal, ch, GDMA_CHANNEL_DIRECTION_RX, + true, false, false); + s_gdma_hal.enable_burst(&s_gdma_hal, ch, GDMA_CHANNEL_DIRECTION_RX, false, false); + s_gdma_hal.enable_intr(&s_gdma_hal, ch, GDMA_CHANNEL_DIRECTION_RX, + ISENSOR_ADC_GDMA_RX_EOF, true); + + int irq = gdma_periph_signals.groups[ISENSOR_ADC_GDMA_GROUP].pairs[ch].rx_irq_id; + esp_err_t err = esp_intr_alloc(irq, ESP_INTR_FLAG_IRAM, isensor_adc_gdma_isr, NULL, &s_intr); + if (err != ESP_OK) { + return err; + } + + ctx->on_done = on_done; + ctx->on_done_arg = user; + s_ctx = ctx; + s_inited = true; + return ESP_OK; +} + +esp_err_t isensor_adc_dma_deinit(isensor_adc_dma_ctx_t *ctx) +{ + (void)ctx; + if (!s_inited) { + return ESP_OK; + } + isensor_adc_dma_stop(ctx); + if (s_intr != NULL) { + esp_intr_free(s_intr); + s_intr = NULL; + } + s_ctx = NULL; + s_inited = false; + return ESP_OK; +} + +esp_err_t isensor_adc_dma_start(isensor_adc_dma_ctx_t *ctx, dma_descriptor_t *desc) +{ + (void)ctx; + if (desc == NULL) { + return ESP_ERR_INVALID_ARG; + } + s_gdma_hal.start_with_desc(&s_gdma_hal, ISENSOR_ADC_GDMA_PAIR, + GDMA_CHANNEL_DIRECTION_RX, (intptr_t)desc); + return ESP_OK; +} + +esp_err_t isensor_adc_dma_stop(isensor_adc_dma_ctx_t *ctx) +{ + (void)ctx; + if (!s_inited) { + return ESP_OK; + } + s_gdma_hal.stop(&s_gdma_hal, ISENSOR_ADC_GDMA_PAIR, GDMA_CHANNEL_DIRECTION_RX); + return ESP_OK; +} + +esp_err_t isensor_adc_dma_reset(isensor_adc_dma_ctx_t *ctx) +{ + (void)ctx; + if (!s_inited) { + return ESP_OK; + } + s_gdma_hal.reset(&s_gdma_hal, ISENSOR_ADC_GDMA_PAIR, GDMA_CHANNEL_DIRECTION_RX); + return ESP_OK; +} diff --git a/source/drivers/isensor_adc_internal.h b/source/drivers/isensor_adc_internal.h new file mode 100644 index 00000000..273950cc --- /dev/null +++ b/source/drivers/isensor_adc_internal.h @@ -0,0 +1,39 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + */ +#pragma once + +#include +#include +#include "sdkconfig.h" +#include "esp_err.h" +#include "hal/dma_types.h" +#include "soc/soc_caps.h" + +#define ESP_FOC_ISENSOR_ADC_PATTERN_HZ 80000 +#define ESP_FOC_ISENSOR_ADC_NUM_CHANNELS 2 + +typedef struct isensor_adc_dma_ctx isensor_adc_dma_ctx_t; + +typedef void (*isensor_adc_dma_done_fn_t)(void *user); + +struct isensor_adc_dma_ctx { + isensor_adc_dma_done_fn_t on_done; + void *on_done_arg; + volatile intptr_t eof_desc_addr; +}; + +esp_err_t isensor_adc_dma_init(isensor_adc_dma_ctx_t *ctx, + isensor_adc_dma_done_fn_t on_done, + void *user); +esp_err_t isensor_adc_dma_deinit(isensor_adc_dma_ctx_t *ctx); +esp_err_t isensor_adc_dma_start(isensor_adc_dma_ctx_t *ctx, dma_descriptor_t *desc); +esp_err_t isensor_adc_dma_stop(isensor_adc_dma_ctx_t *ctx); +esp_err_t isensor_adc_dma_reset(isensor_adc_dma_ctx_t *ctx); + +typedef enum { + ESP_FOC_ISENSOR_ADC_STATE_IDLE = 0, + ESP_FOC_ISENSOR_ADC_STATE_BUSY, +} esp_foc_isensor_adc_state_t; diff --git a/test/test_isensor_adc.c b/test/test_isensor_adc.c new file mode 100644 index 00000000..4dfa0129 --- /dev/null +++ b/test/test_isensor_adc.c @@ -0,0 +1,35 @@ +/* + * Unit tests for isensor ADC cali LUT (no hardware). + */ +#include +#include "espFoC/esp_foc_adc_cali_lut.h" +#include "espFoC/current_sensor_adc.h" +#include "espFoC/esp_foc_err.h" +#include "hal/adc_types.h" + +TEST_CASE("adc cali LUT builds 4096 entries", "[espFoC][isensor_adc]") +{ + int16_t lut[ESP_FOC_ISENSOR_ADC_LUT_SIZE]; + bool ok = esp_foc_adc_cali_lut_build(ADC_UNIT_1, ADC_CHANNEL_0, ADC_ATTEN_DB_12, + lut, ESP_FOC_ISENSOR_ADC_LUT_SIZE); + (void)ok; + TEST_ASSERT_EQUAL(0, lut[0]); + TEST_ASSERT_EQUAL(4095, lut[4095]); +} + +TEST_CASE("adc cali LUT apply clamps out of range", "[espFoC][isensor_adc]") +{ + int16_t lut[ESP_FOC_ISENSOR_ADC_LUT_SIZE]; + for (int i = 0; i < ESP_FOC_ISENSOR_ADC_LUT_SIZE; i++) { + lut[i] = (int16_t)i; + } + TEST_ASSERT_EQUAL(0, esp_foc_adc_cali_lut_apply(lut, -10)); + TEST_ASSERT_EQUAL(4095, esp_foc_adc_cali_lut_apply(lut, 5000)); + TEST_ASSERT_EQUAL(100, esp_foc_adc_cali_lut_apply(lut, 100)); +} + +TEST_CASE("isensor trigger ETM returns not supported", "[espFoC][isensor_adc]") +{ + esp_foc_err_t err = esp_foc_isensor_adc_set_trigger(NULL, ESP_FOC_ISENSOR_ADC_TRIG_ETM); + TEST_ASSERT_EQUAL(ESP_FOC_ERR_INVALID_ARG, err); +} From 4f937d5debfb9992e27b7181681b0437d4958502 Mon Sep 17 00:00:00 2001 From: Felipe Neves Date: Mon, 1 Jun 2026 17:02:21 -0300 Subject: [PATCH 2/2] Add bench axis mode and locked-rotor isensor characterization example. Expose link-layer attach for dummy axes without FOC loops so Studio can drive open-loop Vq, read shunt currents, and scope the response. --- CMakeLists.txt | 1 + .../test_isensor_characterize/CMakeLists.txt | 7 + .../main/CMakeLists.txt | 2 + .../main/Kconfig.projbuild | 1 + .../test_isensor_characterize/main/main.c | 164 +++++++++++++++++ .../sdkconfig.defaults | 16 ++ include/espFoC/esp_foc.h | 16 ++ include/espFoC/esp_foc_axis.h | 2 + include/espFoC/esp_foc_err.h | 6 + .../espFoC/gui_link/esp_foc_link_session.h | 7 + include/espFoC/gui_link/esp_foc_tuner.h | 11 ++ source/gui_link/esp_foc_tuner.c | 44 +++++ source/motor_control/esp_foc_axis_bench.c | 174 ++++++++++++++++++ source/motor_control/esp_foc_core.c | 2 + test/test_tuner.c | 43 +++++ 15 files changed, 496 insertions(+) create mode 100644 examples/test_drivers/test_isensor_characterize/CMakeLists.txt create mode 100644 examples/test_drivers/test_isensor_characterize/main/CMakeLists.txt create mode 100644 examples/test_drivers/test_isensor_characterize/main/Kconfig.projbuild create mode 100644 examples/test_drivers/test_isensor_characterize/main/main.c create mode 100644 examples/test_drivers/test_isensor_characterize/sdkconfig.defaults create mode 100644 source/motor_control/esp_foc_axis_bench.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 7b5fc2b0..b1bf84c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,5 @@ set(srcs "source/motor_control/esp_foc_core.c" + "source/motor_control/esp_foc_axis_bench.c" "source/motor_control/esp_foc_estimator_q16.c" "source/motor_control/esp_foc_biquad_q16.c" "source/calibration/esp_foc_calibration_format.c" diff --git a/examples/test_drivers/test_isensor_characterize/CMakeLists.txt b/examples/test_drivers/test_isensor_characterize/CMakeLists.txt new file mode 100644 index 00000000..d6c3f5df --- /dev/null +++ b/examples/test_drivers/test_isensor_characterize/CMakeLists.txt @@ -0,0 +1,7 @@ + +cmake_minimum_required(VERSION 3.5) + +set(EXTRA_COMPONENT_DIRS "./../../../.") + +include($ENV{IDF_PATH}/tools/cmake/project.cmake) +project(espfoc_isensor_char) diff --git a/examples/test_drivers/test_isensor_characterize/main/CMakeLists.txt b/examples/test_drivers/test_isensor_characterize/main/CMakeLists.txt new file mode 100644 index 00000000..e9ad7caf --- /dev/null +++ b/examples/test_drivers/test_isensor_characterize/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + PRIV_REQUIRES espFoC esp_system freertos) diff --git a/examples/test_drivers/test_isensor_characterize/main/Kconfig.projbuild b/examples/test_drivers/test_isensor_characterize/main/Kconfig.projbuild new file mode 100644 index 00000000..0035d981 --- /dev/null +++ b/examples/test_drivers/test_isensor_characterize/main/Kconfig.projbuild @@ -0,0 +1 @@ +source "../../../kconfig/hw_axis_tuning.kconfig" diff --git a/examples/test_drivers/test_isensor_characterize/main/main.c b/examples/test_drivers/test_isensor_characterize/main/main.c new file mode 100644 index 00000000..fdc947de --- /dev/null +++ b/examples/test_drivers/test_isensor_characterize/main/main.c @@ -0,0 +1,164 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + * + * Locked-rotor current-sense characterization: open-loop Vq at fixed theta, + * isensor + inverter on a bench axis exposed to the link layer (tuner/scope). + * + * Workflow: + * 1. Mechanically lock the rotor. + * 2. Flash this firmware and connect espFoC Studio / tunerctl on UART. + * 3. CONNECT → RUN (arms bench, enables inverter) → ramp WRITE UQ. + * 4. SCOPE_START to plot Uq, Iq, Iu, Iv vs time. + * 5. CALISENSOR to re-run DC offset calibration at zero torque. + * + * No encoder and no FOC alignment — the electrical angle is fixed in firmware + * (default 0 rad, tunable via WRITE BENCH_THETA). + */ + +#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/gui_link/esp_foc_link_session.h" +#include "espFoC/inverter_6pwm_mcpwm.h" +#include "espFoC/current_sensor_adc.h" +#include "espFoC/utils/esp_foc_q16.h" + +static const char *TAG = "isensor_char"; + +static esp_foc_axis_t s_axis; +static esp_foc_inverter_t *s_inverter; +static esp_foc_isensor_t *s_shunts; + +uint32_t esp_foc_tuner_firmware_type(void) +{ + return ESP_FOC_TUNER_FIRMWARE_TYPE_ISCHAR; +} + +static int pwm_enable_gpio(void) +{ + if (CONFIG_AXIS_TUNING_PWM_EN_PIN < 0) { + return -1; + } +#ifdef CONFIG_AXIS_TUNING_PWM_EN_ACT_LOW + if (CONFIG_AXIS_TUNING_PWM_EN_ACT_LOW) { + return -CONFIG_AXIS_TUNING_PWM_EN_PIN; + } +#endif + return CONFIG_AXIS_TUNING_PWM_EN_PIN; +} + +static void wire_scope_channels(void) +{ +#if defined(CONFIG_ESP_FOC_SCOPE) + esp_foc_scope_add_channel(&s_axis.u_q.raw, 0); + esp_foc_scope_add_channel(&s_axis.u_d.raw, 1); + esp_foc_scope_add_channel(&s_axis.i_q.raw, 2); + esp_foc_scope_add_channel(&s_axis.i_d.raw, 3); + esp_foc_scope_add_channel(&s_axis.i_u, 4); + esp_foc_scope_add_channel(&s_axis.i_v, 5); + esp_foc_scope_add_channel(&s_axis.latest_i_alpha, 6); + esp_foc_scope_add_channel(&s_axis.latest_i_beta, 7); + esp_foc_scope_initalize(); +#endif +} + +#if defined(CONFIG_ESP_FOC_SCOPE) +static void pump_scope_idle(void) +{ + for (int n = 0; n < CONFIG_ESP_FOC_SCOPE_BUFFER_SIZE; n++) { + esp_foc_scope_data_push(); + } +} +#endif + +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); + } +} + +void app_main(void) +{ + ESP_LOGI(TAG, "isensor characterization — lock rotor, connect host, arm bench, ramp Uq"); + + s_inverter = inverter_6pwm_mpcwm_new( + CONFIG_AXIS_TUNING_PWM_U_HI, + CONFIG_AXIS_TUNING_PWM_U_LO, + CONFIG_AXIS_TUNING_PWM_V_HI, + CONFIG_AXIS_TUNING_PWM_V_LO, + CONFIG_AXIS_TUNING_PWM_W_HI, + CONFIG_AXIS_TUNING_PWM_W_LO, + pwm_enable_gpio(), + (float)CONFIG_AXIS_TUNING_DC_LINK_V, + 0); + if (s_inverter == NULL) { + ESP_LOGE(TAG, "inverter init failed"); + return; + } + + esp_foc_isensor_adc_config_t shunt_cfg = { + .channels = {(adc_channel_t)CONFIG_AXIS_TUNING_ISENSE_CH_U, + (adc_channel_t)CONFIG_AXIS_TUNING_ISENSE_CH_V}, + .unit = ADC_UNIT_1, + .amp_gain = (float)CONFIG_AXIS_TUNING_ISENSE_AMP_GAIN_X100 / 100.0f, + .shunt_resistance = (float)CONFIG_AXIS_TUNING_ISENSE_SHUNT_MOHM / 1000.0f, + }; + s_shunts = isensor_adc_new(&shunt_cfg); + if (s_shunts == NULL) { + ESP_LOGE(TAG, "current sensor init failed"); + return; + } + + esp_foc_axis_bench_config_t bench_cfg = { + .motor = { + .motor_pole_pairs = CONFIG_AXIS_TUNING_POLE_PAIRS, + .natural_direction = ESP_FOC_MOTOR_NATURAL_DIRECTION_CW, + .motor_unit = 0, + }, + .calibrate_isensor_at_init = true, + .bench_theta_e = 0, + }; + + if (esp_foc_initialize_axis_bench(&s_axis, s_inverter, s_shunts, &bench_cfg) != + ESP_FOC_OK) { + ESP_LOGE(TAG, "bench axis init failed"); + return; + } + + wire_scope_channels(); +#if defined(CONFIG_ESP_FOC_SCOPE) + pump_scope_idle(); +#endif + + ESP_ERROR_CHECK(esp_foc_link_attach_axis(0, &s_axis)); + + if (esp_foc_task_spawn(bench_task, NULL, 4096, 5, NULL) != 0) { + ESP_LOGE(TAG, "bench task spawn failed"); + return; + } + + ESP_LOGW(TAG, + ">>> LOCK the rotor mechanically. Connect Studio/UART. " + "RUN=arm bench, write UQ to stress the windings, SCOPE for I vs U."); + + while (1) { +#if defined(CONFIG_ESP_FOC_SCOPE) + if (s_axis.state != ESP_FOC_AXIS_STATE_BENCH) { + pump_scope_idle(); + } +#endif + vTaskDelay(pdMS_TO_TICKS(200)); + } +} diff --git a/examples/test_drivers/test_isensor_characterize/sdkconfig.defaults b/examples/test_drivers/test_isensor_characterize/sdkconfig.defaults new file mode 100644 index 00000000..763c2fb2 --- /dev/null +++ b/examples/test_drivers/test_isensor_characterize/sdkconfig.defaults @@ -0,0 +1,16 @@ +CONFIG_ESP_TASK_WDT_EN=n + +CONFIG_ESP_FOC_TUNER_ENABLE=y +CONFIG_ESP_FOC_BRIDGE_UART=y +CONFIG_ESP_FOC_BRIDGE_UART_BAUD=921600 +CONFIG_ESP_FOC_BRIDGE_UART_TX_PIN=17 +CONFIG_ESP_FOC_BRIDGE_UART_RX_PIN=18 + +CONFIG_ESP_FOC_SCOPE=y +CONFIG_ESP_FOC_SCOPE_NUM_CHANNELS=8 +CONFIG_ESP_FOC_SCOPE_BUFFER_SIZE=512 + +CONFIG_ESP_FOC_PWM_RATE_HZ=20000 +CONFIG_ESP_FOC_CALIBRATION_NVS=n + +CONFIG_COMPILER_OPTIMIZATION_PERF=y diff --git a/include/espFoC/esp_foc.h b/include/espFoC/esp_foc.h index e570314c..70e7c111 100644 --- a/include/espFoC/esp_foc.h +++ b/include/espFoC/esp_foc.h @@ -22,6 +22,22 @@ esp_foc_err_t esp_foc_initialize_axis(esp_foc_axis_t *axis, esp_foc_rotor_sensor_t *rotor, esp_foc_isensor_t *isensor, esp_foc_motor_control_settings_t settings); + +typedef struct { + esp_foc_motor_control_settings_t motor; + bool calibrate_isensor_at_init; + q16_t bench_theta_e; +} esp_foc_axis_bench_config_t; + +/** Bench axis: inverter + isensor only (no rotor, no FOC loops). For link/scope characterization. */ +esp_foc_err_t esp_foc_initialize_axis_bench(esp_foc_axis_t *axis, + esp_foc_inverter_t *inverter, + esp_foc_isensor_t *isensor, + const esp_foc_axis_bench_config_t *config); + +esp_foc_err_t esp_foc_bench_arm(esp_foc_axis_t *axis); +esp_foc_err_t esp_foc_bench_disarm(esp_foc_axis_t *axis); +esp_foc_err_t esp_foc_bench_step(esp_foc_axis_t *axis); /** Recompute encoder_inv_cpr_q16 after pole pairs or CPR change. */ void esp_foc_axis_refresh_encoder_q16_scales(esp_foc_axis_t *axis); esp_foc_err_t esp_foc_align_axis(esp_foc_axis_t *axis); diff --git a/include/espFoC/esp_foc_axis.h b/include/espFoC/esp_foc_axis.h index 68b928b8..f21b86a8 100644 --- a/include/espFoC/esp_foc_axis.h +++ b/include/espFoC/esp_foc_axis.h @@ -90,6 +90,8 @@ struct esp_foc_axis_s { q16_t natural_direction; esp_foc_axis_state_t state; + esp_foc_axis_mode_t mode; + q16_t bench_theta_e; volatile bool runner_shutdown; void *runner_low_speed_hdl; void *runner_outer_hdl; diff --git a/include/espFoC/esp_foc_err.h b/include/espFoC/esp_foc_err.h index d3173de5..efe3f502 100644 --- a/include/espFoC/esp_foc_err.h +++ b/include/espFoC/esp_foc_err.h @@ -28,4 +28,10 @@ typedef enum { ESP_FOC_AXIS_STATE_ALIGNING = 1, ESP_FOC_AXIS_STATE_ALIGNED = 2, ESP_FOC_AXIS_STATE_RUNNING = 3, + ESP_FOC_AXIS_STATE_BENCH = 4, } esp_foc_axis_state_t; + +typedef enum { + ESP_FOC_AXIS_MODE_FOC = 0, + ESP_FOC_AXIS_MODE_BENCH, +} esp_foc_axis_mode_t; diff --git a/include/espFoC/gui_link/esp_foc_link_session.h b/include/espFoC/gui_link/esp_foc_link_session.h index 1f2ed1a0..11d0fdbe 100644 --- a/include/espFoC/gui_link/esp_foc_link_session.h +++ b/include/espFoC/gui_link/esp_foc_link_session.h @@ -63,6 +63,13 @@ void esp_foc_link_session_on_disconnect(void); void esp_foc_link_session_on_scope_start(void); void esp_foc_link_session_on_scope_stop(void); +/** Register an axis for tuner/scope/heartbeat (alias of esp_foc_tuner_attach_axis). */ +static inline esp_foc_err_t esp_foc_link_attach_axis(uint8_t axis_id, + esp_foc_axis_t *axis) +{ + return esp_foc_tuner_attach_axis(axis_id, axis); +} + #ifdef __cplusplus } #endif diff --git a/include/espFoC/gui_link/esp_foc_tuner.h b/include/espFoC/gui_link/esp_foc_tuner.h index d294f944..b761e1aa 100644 --- a/include/espFoC/gui_link/esp_foc_tuner.h +++ b/include/espFoC/gui_link/esp_foc_tuner.h @@ -58,6 +58,12 @@ typedef enum { ESP_FOC_TUNER_PARAM_AXIS_LAST_ERR = 0x0041, ESP_FOC_TUNER_PARAM_NVS_PRESENT = 0x0042, ESP_FOC_TUNER_PARAM_FIRMWARE_TYPE = 0x0050, + ESP_FOC_TUNER_PARAM_UQ_Q16 = 0x0051, + ESP_FOC_TUNER_PARAM_UD_Q16 = 0x0052, + ESP_FOC_TUNER_PARAM_IU_Q16 = 0x0053, + ESP_FOC_TUNER_PARAM_IV_Q16 = 0x0054, + ESP_FOC_TUNER_PARAM_IQ_MEAS_Q16 = 0x0055, + ESP_FOC_TUNER_PARAM_BENCH_THETA_Q16 = 0x0056, /* Write: gain swap (atomic) */ ESP_FOC_TUNER_WRITE_KP_Q16 = 0x0020, @@ -70,6 +76,9 @@ typedef enum { ESP_FOC_TUNER_WRITE_TARGET_ID_Q16 = 0x0060, ESP_FOC_TUNER_WRITE_TARGET_IQ_Q16 = 0x0061, + ESP_FOC_TUNER_WRITE_UQ_Q16 = 0x0062, + ESP_FOC_TUNER_WRITE_UD_Q16 = 0x0063, + ESP_FOC_TUNER_WRITE_BENCH_THETA_Q16 = 0x0064, ESP_FOC_TUNER_CMD_CONNECT = 0x00A0, ESP_FOC_TUNER_CMD_DISCONNECT = 0x00A1, @@ -77,6 +86,7 @@ typedef enum { ESP_FOC_TUNER_CMD_ALIGN_AXIS = 0x00A2, ESP_FOC_TUNER_CMD_STOP_AXIS = 0x00A3, ESP_FOC_TUNER_CMD_RUN_AXIS = 0x00A4, + ESP_FOC_TUNER_CMD_CALISENSOR = 0x00A5, ESP_FOC_TUNER_CMD_STORE_NVS = 0x00B0, ESP_FOC_TUNER_CMD_ERASE_NVS = 0x00B2, @@ -181,6 +191,7 @@ uint32_t esp_foc_tuner_firmware_type(void); #define ESP_FOC_TUNER_FIRMWARE_TYPE_GENERIC 0u #define ESP_FOC_TUNER_FIRMWARE_TYPE_TSGX 0x58475354u /* 'TSGX' LE */ +#define ESP_FOC_TUNER_FIRMWARE_TYPE_ISCHAR 0x52484349u /* 'ICHR' LE — isensor characterization */ #ifdef __cplusplus } diff --git a/source/gui_link/esp_foc_tuner.c b/source/gui_link/esp_foc_tuner.c index cebf730a..b8ce242a 100644 --- a/source/gui_link/esp_foc_tuner.c +++ b/source/gui_link/esp_foc_tuner.c @@ -55,6 +55,10 @@ static uint8_t compute_axis_state(const esp_foc_axis_t *axis) if (axis->state == ESP_FOC_AXIS_STATE_RUNNING) { s |= ESP_FOC_AXIS_STATE_RUNNING; } + if (axis->mode == ESP_FOC_AXIS_MODE_BENCH && + axis->state == ESP_FOC_AXIS_STATE_BENCH) { + s |= ESP_FOC_AXIS_STATE_ALIGNED; + } return s; } @@ -149,6 +153,12 @@ static esp_foc_err_t handle_read(esp_foc_axis_t *axis, case ESP_FOC_TUNER_PARAM_LOOP_FS_HZ_Q16: value = axis->current_filter_fs_hz_q16; break; case ESP_FOC_TUNER_PARAM_KD_Q16: value = kd; break; case ESP_FOC_TUNER_PARAM_KFF_Q16: value = kff; break; + case ESP_FOC_TUNER_PARAM_UQ_Q16: value = axis->u_q.raw; break; + case ESP_FOC_TUNER_PARAM_UD_Q16: value = axis->u_d.raw; break; + case ESP_FOC_TUNER_PARAM_IU_Q16: value = axis->i_u; break; + case ESP_FOC_TUNER_PARAM_IV_Q16: value = axis->i_v; break; + case ESP_FOC_TUNER_PARAM_IQ_MEAS_Q16: value = axis->i_q.raw; break; + case ESP_FOC_TUNER_PARAM_BENCH_THETA_Q16: value = axis->bench_theta_e; break; default: return ESP_FOC_ERR_INVALID_ARG; } @@ -213,6 +223,22 @@ static esp_foc_err_t handle_write(esp_foc_axis_t *axis, return ESP_FOC_OK; } + if (axis->mode == ESP_FOC_AXIS_MODE_BENCH) { + esp_foc_critical_enter(); + if (id == ESP_FOC_TUNER_WRITE_UQ_Q16) { + axis->u_q.raw = v; + } else if (id == ESP_FOC_TUNER_WRITE_UD_Q16) { + axis->u_d.raw = v; + } else if (id == ESP_FOC_TUNER_WRITE_BENCH_THETA_Q16) { + axis->bench_theta_e = q16_normalize_angle(v); + } else { + esp_foc_critical_leave(); + return ESP_FOC_ERR_INVALID_ARG; + } + esp_foc_critical_leave(); + return ESP_FOC_OK; + } + if (id == ESP_FOC_TUNER_WRITE_TARGET_ID_Q16 || id == ESP_FOC_TUNER_WRITE_TARGET_IQ_Q16) { if (axis->state != ESP_FOC_AXIS_STATE_RUNNING) { @@ -320,20 +346,38 @@ static esp_foc_err_t handle_exec(esp_foc_axis_t *axis, } if (id == ESP_FOC_TUNER_CMD_STOP_AXIS) { + if (axis->mode == ESP_FOC_AXIS_MODE_BENCH) { + return esp_foc_bench_disarm(axis); + } return esp_foc_stop(axis); } if (id == ESP_FOC_TUNER_CMD_ALIGN_AXIS) { + if (axis->mode == ESP_FOC_AXIS_MODE_BENCH) { + return ESP_FOC_ERR_NOT_SUPPORTED; + } return esp_foc_align_axis(axis); } if (id == ESP_FOC_TUNER_CMD_RUN_AXIS) { + if (axis->mode == ESP_FOC_AXIS_MODE_BENCH) { + return esp_foc_bench_arm(axis); + } if (axis->state != ESP_FOC_AXIS_STATE_ALIGNED) { return ESP_FOC_ERR_NOT_ALIGNED; } return esp_foc_run(axis); } + if (id == ESP_FOC_TUNER_CMD_CALISENSOR) { + if (axis->isensor_driver == NULL) { + return ESP_FOC_ERR_AXIS_INVALID_STATE; + } + axis->isensor_driver->calibrate_isensors( + axis->isensor_driver, CONFIG_ESP_FOC_ISENSOR_CALIBRATION_ROUNDS); + return ESP_FOC_OK; + } + if (id == ESP_FOC_TUNER_CMD_STORE_NVS) { return esp_foc_calibration_axis_tuner_store(axis); } diff --git a/source/motor_control/esp_foc_axis_bench.c b/source/motor_control/esp_foc_axis_bench.c new file mode 100644 index 00000000..8eafc0d0 --- /dev/null +++ b/source/motor_control/esp_foc_axis_bench.c @@ -0,0 +1,174 @@ +/* + * MIT License + * + * Copyright (c) 2021 Felipe Neves + * + * Bench / characterization axis: open-loop Vdq at fixed theta, no FOC loops. + */ + +#include "esp_log.h" +#include "espFoC/esp_foc.h" +#include "espFoC/utils/foc_math_q16.h" +#include "espFoC/utils/modulator.h" + +static const char *TAG = "ESP_FOC_BENCH"; + +static void bench_park_inverter_safe(esp_foc_axis_t *axis) +{ + if (axis->inverter_driver == NULL) { + return; + } + axis->inverter_driver->set_duties(axis->inverter_driver, 0, 0, 0); + axis->inverter_driver->disable(axis->inverter_driver); +} + +static void bench_apply_filter(esp_foc_axis_t *axis, float fc_hz, float fs_hz) +{ + if (axis->isensor_driver == NULL || + axis->isensor_driver->set_filter_cutoff == NULL) { + return; + } + axis->isensor_driver->set_filter_cutoff(axis->isensor_driver, fc_hz, fs_hz); + axis->current_filter_fc_hz_q16 = q16_from_float(fc_hz); + axis->current_filter_fs_hz_q16 = q16_from_float(fs_hz); +} + +esp_foc_err_t esp_foc_initialize_axis_bench(esp_foc_axis_t *axis, + esp_foc_inverter_t *inverter, + esp_foc_isensor_t *isensor, + const esp_foc_axis_bench_config_t *config) +{ + if (axis == NULL || inverter == NULL || isensor == NULL || config == NULL) { + return ESP_FOC_ERR_INVALID_ARG; + } + + float pwm_rate_hz_f = (float)inverter->get_inverter_pwm_rate(inverter); + float dt_f = (pwm_rate_hz_f > 1e-9f) ? (1.0f / pwm_rate_hz_f) : 0.0f; + float loop_fs_hz = (dt_f > 1e-9f) ? (1.0f / dt_f) : 0.0f; + +#if defined(CONFIG_ESP_FOC_TUNER_ENABLE) + axis->magic = ESP_FOC_AXIS_MAGIC; +#endif + + axis->mode = ESP_FOC_AXIS_MODE_BENCH; + axis->bench_theta_e = config->bench_theta_e; + axis->state = ESP_FOC_AXIS_STATE_IDLE; + axis->runner_shutdown = false; + axis->runner_low_speed_hdl = NULL; + axis->runner_outer_hdl = NULL; + axis->regulator_ev = NULL; + axis->low_speed_ev = NULL; + axis->inverter_driver = inverter; + axis->rotor_sensor_driver = NULL; + axis->isensor_driver = isensor; + axis->motor_pole_pairs = config->motor.motor_pole_pairs; + axis->natural_direction = (config->motor.natural_direction == + ESP_FOC_MOTOR_NATURAL_DIRECTION_CW) + ? Q16_ONE + : Q16_MINUS_ONE; + + esp_foc_calibration_axis_init_store(axis, &config->motor); + + axis->vdc_q16 = inverter->get_dc_link_voltage(inverter); + axis->mod_index_limit_q16 = ESP_FOC_MOD_INDEX_LIMIT_Q16; + axis->dt = q16_from_float(dt_f); + axis->inv_dt = q16_from_float(loop_fs_hz); + + axis->u_d.raw = 0; + axis->u_q.raw = 0; + axis->i_d.raw = 0; + axis->i_q.raw = 0; + axis->target_i_d.raw = 0; + axis->target_i_q.raw = 0; + axis->latest_i_alpha = 0; + axis->latest_i_beta = 0; + + inverter->set_duties(inverter, 0, 0, 0); + + if (config->calibrate_isensor_at_init) { + isensor->calibrate_isensors(isensor, CONFIG_ESP_FOC_ISENSOR_CALIBRATION_ROUNDS); + } + + float fc_hz = (float)CONFIG_ESP_FOC_CURRENT_FILTER_CUTOFF_HZ; + esp_foc_calibration_axis_boot_apply(axis, &fc_hz, loop_fs_hz); + bench_apply_filter(axis, fc_hz, loop_fs_hz); + + if (isensor->set_publish_targets != NULL) { + isensor->set_publish_targets(isensor, + (q16_t *)&axis->latest_i_alpha, + (q16_t *)&axis->latest_i_beta, + &axis->i_u, + &axis->i_v); + } + + ESP_LOGI(TAG, "bench axis ready (theta_e=%f rad)", + (double)q16_to_float(axis->bench_theta_e)); + return ESP_FOC_OK; +} + +esp_foc_err_t esp_foc_bench_arm(esp_foc_axis_t *axis) +{ + if (axis == NULL || axis->mode != ESP_FOC_AXIS_MODE_BENCH) { + return ESP_FOC_ERR_INVALID_ARG; + } + if (axis->state == ESP_FOC_AXIS_STATE_BENCH) { + return ESP_FOC_OK; + } + if (axis->state != ESP_FOC_AXIS_STATE_IDLE) { + return ESP_FOC_ERR_AXIS_INVALID_STATE; + } + axis->inverter_driver->enable(axis->inverter_driver); + axis->state = ESP_FOC_AXIS_STATE_BENCH; + return ESP_FOC_OK; +} + +esp_foc_err_t esp_foc_bench_disarm(esp_foc_axis_t *axis) +{ + if (axis == NULL || axis->mode != ESP_FOC_AXIS_MODE_BENCH) { + return ESP_FOC_ERR_INVALID_ARG; + } + if (axis->state != ESP_FOC_AXIS_STATE_BENCH) { + return ESP_FOC_OK; + } + bench_park_inverter_safe(axis); + axis->u_d.raw = 0; + axis->u_q.raw = 0; + axis->state = ESP_FOC_AXIS_STATE_IDLE; + return ESP_FOC_OK; +} + +esp_foc_err_t esp_foc_bench_step(esp_foc_axis_t *axis) +{ + if (axis == NULL || axis->mode != ESP_FOC_AXIS_MODE_BENCH) { + return ESP_FOC_ERR_INVALID_ARG; + } + if (axis->state != ESP_FOC_AXIS_STATE_BENCH) { + return ESP_FOC_ERR_AXIS_INVALID_STATE; + } + + if (axis->isensor_driver != NULL) { + axis->isensor_driver->sample_isensors(axis->isensor_driver); + isensor_values_t val; + axis->isensor_driver->fetch_isensors(axis->isensor_driver, &val); + axis->i_u = val.iu_axis_0; + axis->i_v = val.iv_axis_0; + axis->i_w = val.iw_axis_0; + + q16_t sin_t = q16_sin(axis->bench_theta_e); + q16_t cos_t = q16_cos(axis->bench_theta_e); + q16_park(sin_t, cos_t, axis->latest_i_alpha, axis->latest_i_beta, + &axis->i_d.raw, &axis->i_q.raw); + } + + q16_t sin_t = q16_sin(axis->bench_theta_e); + q16_t cos_t = q16_cos(axis->bench_theta_e); + q16_t alpha, beta, da, db, dc; + esp_foc_modulate_dq_to_duties(sin_t, cos_t, + axis->u_d.raw, axis->u_q.raw, + &alpha, &beta, &da, &db, &dc, + axis->mod_index_limit_q16); + axis->u_alpha.raw = alpha; + axis->u_beta.raw = beta; + axis->inverter_driver->set_duties(axis->inverter_driver, da, db, dc); + return ESP_FOC_OK; +} diff --git a/source/motor_control/esp_foc_core.c b/source/motor_control/esp_foc_core.c index 8f39c2fe..3cac2409 100644 --- a/source/motor_control/esp_foc_core.c +++ b/source/motor_control/esp_foc_core.c @@ -295,6 +295,8 @@ esp_foc_err_t esp_foc_initialize_axis(esp_foc_axis_t *axis, #endif axis->state = ESP_FOC_AXIS_STATE_IDLE; + axis->mode = ESP_FOC_AXIS_MODE_FOC; + axis->bench_theta_e = 0; axis->runner_shutdown = false; axis->runner_low_speed_hdl = NULL; axis->runner_outer_hdl = NULL; diff --git a/test/test_tuner.c b/test/test_tuner.c index fd2a8d2c..daee4479 100644 --- a/test/test_tuner.c +++ b/test/test_tuner.c @@ -282,6 +282,49 @@ TEST_CASE("tuner: motion writes update axis targets while running", TEST_ASSERT_EQUAL_INT32(targets[1], s_axis.target_i_q.raw); } +TEST_CASE("tuner: bench mode UQ write and arm without align", "[espFoC][tuner]") +{ + esp_foc_axis_t axis; + esp_foc_motor_control_settings_t settings; + mock_inverter_init(&s_inv, 1.0f, 20000.0f); + mock_isensor_init(&s_isensor); + settings.natural_direction = ESP_FOC_MOTOR_NATURAL_DIRECTION_CW; + settings.motor_pole_pairs = 7; + settings.motor_unit = 0; + + esp_foc_axis_bench_config_t bench_cfg = { + .motor = settings, + .calibrate_isensor_at_init = false, + .bench_theta_e = 0, + }; + TEST_ASSERT_EQUAL(ESP_FOC_OK, esp_foc_initialize_axis_bench( + &axis, mock_inverter_interface(&s_inv), + mock_isensor_interface(&s_isensor), &bench_cfg)); + TEST_ASSERT_EQUAL(ESP_FOC_OK, esp_foc_tuner_attach_axis(0, &axis)); + esp_foc_link_session_force_connected(true); + + q16_t uq = q16_from_float(0.1f); + uint8_t pl[4]; + serialize_q16_le(pl, uq); + size_t resp_len = 0; + TEST_ASSERT_EQUAL(ESP_FOC_OK, esp_foc_tuner_handle_request( + 0, ESP_FOC_TUNER_OP_WRITE, ESP_FOC_TUNER_WRITE_UQ_Q16, + pl, sizeof(pl), NULL, &resp_len)); + TEST_ASSERT_EQUAL_INT32(uq, axis.u_q.raw); + + TEST_ASSERT_EQUAL(ESP_FOC_OK, esp_foc_tuner_handle_request( + 0, ESP_FOC_TUNER_OP_EXEC, ESP_FOC_TUNER_CMD_RUN_AXIS, + NULL, 0, NULL, &resp_len)); + TEST_ASSERT_EQUAL(ESP_FOC_AXIS_STATE_BENCH, axis.state); + + resp_len = 4; + uint8_t state_buf = 0; + TEST_ASSERT_EQUAL(ESP_FOC_OK, esp_foc_tuner_handle_request( + 0, ESP_FOC_TUNER_OP_READ, ESP_FOC_TUNER_PARAM_AXIS_STATE, + NULL, 0, &state_buf, &resp_len)); + TEST_ASSERT_TRUE(state_buf & ESP_FOC_AXIS_STATE_ALIGNED); +} + #else /* Tuner disabled: keep this TU non-empty so the test build stays consistent. */