From b4f03871e3bbe9830697b82ec9f29b1e052ccc44 Mon Sep 17 00:00:00 2001 From: yanke Date: Mon, 25 Nov 2024 20:41:58 +0800 Subject: [PATCH] feat: esp_simplefoc support 6pwm driver close https://github.com/espressif/esp-iot-solution/issues/432 --- components/motor/esp_simplefoc/CHANGELOG.md | 5 + components/motor/esp_simplefoc/CMakeLists.txt | 6 +- .../motor/esp_simplefoc/idf_component.yml | 2 +- .../port/esp/esp_hal_bldc_6pwm.cpp | 223 ++++++++++++++++++ .../port/esp/include/esp_hal_bldc_6pwm.h | 101 ++++++++ .../port/esp/include/esp_simplefoc.h | 1 + .../test_apps/main/test_esp_simplefoc.cpp | 15 ++ 7 files changed, 351 insertions(+), 2 deletions(-) create mode 100644 components/motor/esp_simplefoc/port/esp/esp_hal_bldc_6pwm.cpp create mode 100644 components/motor/esp_simplefoc/port/esp/include/esp_hal_bldc_6pwm.h diff --git a/components/motor/esp_simplefoc/CHANGELOG.md b/components/motor/esp_simplefoc/CHANGELOG.md index 3a7c8e03d..6787e1d9c 100644 --- a/components/motor/esp_simplefoc/CHANGELOG.md +++ b/components/motor/esp_simplefoc/CHANGELOG.md @@ -1,5 +1,10 @@ # ChangeLog +## v1.1.0 - 2024-11-26 +### Enhancements: +* FOC: + * Support 6PWM driver. Note that only chips that support ``MCPWM`` can use 6PWM driver. + ## v1.0.0 - 2024-9-20 ### Enhancements: * FOC: diff --git a/components/motor/esp_simplefoc/CMakeLists.txt b/components/motor/esp_simplefoc/CMakeLists.txt index 25e316dea..c862c7923 100644 --- a/components/motor/esp_simplefoc/CMakeLists.txt +++ b/components/motor/esp_simplefoc/CMakeLists.txt @@ -9,8 +9,12 @@ set(SRC_FILES "port/angle_sensor/mt6701.cpp" "port/angle_sensor/as5048a.cpp") +if(CONFIG_SOC_MCPWM_SUPPORTED) + list(APPEND SRC_FILES "port/esp/esp_hal_bldc_6pwm.cpp") +endif() + set(INC_FILES "port/esp/include/" "port/angle_sensor") -set(IGNORE_FILES "port/esp/esp_hal_adc.cpp" "port/esp/esp_hal_mcpwm.cpp" +set(IGNORE_FILES "port/esp/esp_hal_adc.cpp" "port/esp/esp_hal_bldc_6pwm.cpp" "port/esp/esp_hal_bldc_3pwm.cpp" "port/angle_sensor/mt6701.cpp" "port/angle_sensor/as5048a.cpp") idf_component_register( diff --git a/components/motor/esp_simplefoc/idf_component.yml b/components/motor/esp_simplefoc/idf_component.yml index 04cb1ca31..34bd77d39 100644 --- a/components/motor/esp_simplefoc/idf_component.yml +++ b/components/motor/esp_simplefoc/idf_component.yml @@ -1,4 +1,4 @@ -version: 1.0.0 +version: 1.1.0 targets: - esp32 - esp32s2 diff --git a/components/motor/esp_simplefoc/port/esp/esp_hal_bldc_6pwm.cpp b/components/motor/esp_simplefoc/port/esp/esp_hal_bldc_6pwm.cpp new file mode 100644 index 000000000..b4a23e1e3 --- /dev/null +++ b/components/motor/esp_simplefoc/port/esp/esp_hal_bldc_6pwm.cpp @@ -0,0 +1,223 @@ +/* + * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "esp_hal_bldc_6pwm.h" +#include "esp_log.h" +#include "esp_check.h" + +#define _PWM_FREQUENCY 20000 /*!< default frequency of MCPWM */ +#define _PWM_FREQUENCY_MAX 50000 /*!< Max frequency of MCPWM */ +#define _PWM_TIMEBASE_RESOLUTION_HZ (10 * 1000 * 1000) /*!< Resolution of MCPWM */ + +static const char* TAG = "esp_hal_bldc_6pwm"; + +BLDCDriver6PWM::BLDCDriver6PWM(int phA_h, int phA_l, int phB_h, int phB_l, int phC_h, int phC_l, int en, int mcpwm_group_id) +{ + pwmA_h = phA_h; + pwmB_h = phB_h; + pwmC_h = phC_h; + pwmA_l = phA_l; + pwmB_l = phB_l; + pwmC_l = phC_l; + + // enable_pin pin + enable_pin = en; + + // default power-supply value + voltage_power_supply = DEF_POWER_SUPPLY; + voltage_limit = NOT_SET; + pwm_frequency = NOT_SET; + + // dead zone initial - 2% + dead_zone = 0.02f; + + this->mcpwm_group_id = mcpwm_group_id; +} + +void BLDCDriver6PWM::enable() +{ + // enable_pin the driver - if enable_pin pin available + if (_isset(enable_pin)) { + digitalWrite(enable_pin, enable_active_high); + } + // set phase state enabled + setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_ON); + // set zero to PWM + setPwm(0, 0, 0); +} + +void BLDCDriver6PWM::disable() +{ + // set phase state to disabled + setPhaseState(PhaseState::PHASE_OFF, PhaseState::PHASE_OFF, PhaseState::PHASE_OFF); + // set zero to PWM + setPwm(0, 0, 0); + // disable the driver - if enable_pin pin available + if (_isset(enable_pin)) { + digitalWrite(enable_pin, !enable_active_high); + } +} + +int BLDCDriver6PWM::init() +{ + // PWM pins + pinMode(pwmA_h, OUTPUT); + pinMode(pwmB_h, OUTPUT); + pinMode(pwmC_h, OUTPUT); + pinMode(pwmA_l, OUTPUT); + pinMode(pwmB_l, OUTPUT); + pinMode(pwmC_l, OUTPUT); + if (_isset(enable_pin)) { + pinMode(enable_pin, OUTPUT); + } + + // sanity check for the voltage limit configuration + if (!_isset(voltage_limit) || voltage_limit > voltage_power_supply) { + voltage_limit = voltage_power_supply; + } + + // set phase state to disabled + phase_state[0] = PhaseState::PHASE_OFF; + phase_state[1] = PhaseState::PHASE_OFF; + phase_state[2] = PhaseState::PHASE_OFF; + + // set zero to PWM + dc_a = dc_b = dc_c = 0; + + // configure 6pwm + // hardware specific function - depending on driver and mcu + if (mcpwm_group_id < 0 || mcpwm_group_id >= SOC_MCPWM_GROUPS) { + ESP_LOGE(TAG, "MCPWM group id: %d is not available\n", mcpwm_group_id); + return 0; + } + + // Init mcpwm driver + mcpwm_timer_config_t timer_config = { + .group_id = mcpwm_group_id, + .clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT, + .resolution_hz = _PWM_TIMEBASE_RESOLUTION_HZ, + .count_mode = MCPWM_TIMER_COUNT_MODE_UP_DOWN, + .period_ticks = (uint32_t)(1 * _PWM_TIMEBASE_RESOLUTION_HZ / _PWM_FREQUENCY), + }; + ESP_ERROR_CHECK(mcpwm_new_timer(&timer_config, &timer)); + + mcpwm_operator_config_t operator_config = { + .group_id = mcpwm_group_id, + }; + + for (int i = 0; i < 3; i++) { + ESP_ERROR_CHECK(mcpwm_new_operator(&operator_config, &oper[i])); + ESP_ERROR_CHECK(mcpwm_operator_connect_timer(oper[i], timer)); + } + + mcpwm_comparator_config_t comparator_config = {0}; + comparator_config.flags.update_cmp_on_tez = true; + comparator_config.flags.update_cmp_on_tep = false; + comparator_config.flags.update_cmp_on_sync = false; + + for (int i = 0; i < 3; i++) { + ESP_ERROR_CHECK(mcpwm_new_comparator(oper[i], &comparator_config, &comparator[i])); + ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator[i], (0))); + } + + mcpwm_generator_config_t generator_config = {}; + int gen_gpios[3][2] = { + {pwmA_h, pwmA_l}, + {pwmB_h, pwmB_l}, + {pwmC_h, pwmC_l}, + }; + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 2; j++) { + generator_config.gen_gpio_num = gen_gpios[i][j]; + ESP_ERROR_CHECK(mcpwm_new_generator(oper[i], &generator_config, &generator[i][j])); + } + } + + for (int i = 0; i < 3; i++) { + ESP_ERROR_CHECK(mcpwm_generator_set_actions_on_compare_event(generator[i][0], + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, comparator[i], MCPWM_GEN_ACTION_LOW), + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_DOWN, comparator[i], MCPWM_GEN_ACTION_HIGH), + MCPWM_GEN_COMPARE_EVENT_ACTION_END())); + } + + mcpwm_dead_time_config_t dt_config = { + .posedge_delay_ticks = (uint32_t)(dead_zone * timer_config.period_ticks), + }; + + mcpwm_dead_time_config_t inv_dt_config = { + .negedge_delay_ticks = (uint32_t)(dead_zone * timer_config.period_ticks), + }; + inv_dt_config.flags.invert_output = true; + + for (int i = 0; i < 3; i++) { + ESP_ERROR_CHECK(mcpwm_generator_set_dead_time(generator[i][0], generator[i][0], &dt_config)); + ESP_ERROR_CHECK(mcpwm_generator_set_dead_time(generator[i][0], generator[i][1], &inv_dt_config)); + } + + // enable mcpwm + ESP_ERROR_CHECK(mcpwm_timer_enable(timer)); + ESP_ERROR_CHECK(mcpwm_timer_start_stop(timer, MCPWM_TIMER_START_NO_STOP)); + + // save timer period + mcpwm_period = timer_config.period_ticks / 2; + initialized = 1; + + return 1; +} + +int BLDCDriver6PWM::deinit() +{ + if (initialized) { + ESP_ERROR_CHECK(mcpwm_timer_disable(timer)); + + for (int i = 0; i < 3; i++) { + ESP_ERROR_CHECK(mcpwm_del_generator(generator[i][0])); + ESP_ERROR_CHECK(mcpwm_del_generator(generator[i][1])); + ESP_ERROR_CHECK(mcpwm_del_comparator(comparator[i])); + ESP_ERROR_CHECK(mcpwm_del_operator(oper[i])); + } + ESP_ERROR_CHECK(mcpwm_del_timer(timer)); + initialized = 0; + return 1; + } + + return 0; +} + +void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) +{ + // limit the voltage in driver + Ua = _constrain(Ua, 0, voltage_limit); + Ub = _constrain(Ub, 0, voltage_limit); + Uc = _constrain(Uc, 0, voltage_limit); + // calculate duty cycle + // limited in [0,1] + dc_a = _constrain(Ua / voltage_power_supply, 0.0f, 1.0f); + dc_b = _constrain(Ub / voltage_power_supply, 0.0f, 1.0f); + dc_c = _constrain(Uc / voltage_power_supply, 0.0f, 1.0f); + // hardware specific writing + // hardware specific function - depending on driver and mcu + halPwmWrite(); +} + +void BLDCDriver6PWM::setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) +{ + phase_state[0] = sa; + phase_state[1] = sb; + phase_state[2] = sc; +} + +void BLDCDriver6PWM::halPwmWrite() +{ + if (!initialized) { + return; + } + + ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator[0], (uint32_t)((mcpwm_period * dc_a)))); + ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator[1], (uint32_t)((mcpwm_period * dc_b)))); + ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(comparator[2], (uint32_t)((mcpwm_period * dc_c)))); +} diff --git a/components/motor/esp_simplefoc/port/esp/include/esp_hal_bldc_6pwm.h b/components/motor/esp_simplefoc/port/esp/include/esp_hal_bldc_6pwm.h new file mode 100644 index 000000000..43ed38bd6 --- /dev/null +++ b/components/motor/esp_simplefoc/port/esp/include/esp_hal_bldc_6pwm.h @@ -0,0 +1,101 @@ +/* + * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ +#pragma once + +#include "BLDCDriver.h" +#include "foc_utils.h" +#include "time_utils.h" +#include "defaults.h" +#include "drivers/hardware_api.h" +#include "driver/mcpwm_prelude.h" + +class BLDCDriver6PWM : public BLDCDriver { +public: + /** + * @brief Construct a new BLDCDriver6PWM object + * + * @param phA_h High-side PWM pin for phase A + * @param phA_l Low-side PWM pin for phase A + * @param phB_h High-side PWM pin for phase B + * @param phB_l Low-side PWM pin for phase B + * @param phC_h High-side PWM pin for phase C + * @param phC_l Low-side PWM pin for phase C + * @param en Optional enable pin (default: NOT_SET) + * @param mcpwm_group_id The MCPWM group ID to be used (default: 0) + */ + BLDCDriver6PWM(int phA_h, int phA_l, int phB_h, int phB_l, int phC_h, int phC_l, int en = NOT_SET, int mcpwm_group_id = 0); + + /** + * @brief Motor hardware init function + * + * @return + * - 0 Failed + * - 1 Success + */ + int init() override; + + /** + * @brief Motor hardware deinit function + * + * @return + * - 0 Failed + * - 1 Success + */ + int deinit(); + + /** + * @brief Motor disable function + * + */ + void disable() override; + + /** + * @brief Motor enable function + * + */ + void enable() override; + + // hardware variables + int pwmA_h, pwmA_l; /*!< phase A pwm pin number */ + int pwmB_h, pwmB_l; /*!< phase B pwm pin number */ + int pwmC_h, pwmC_l; /*!< phase C pwm pin number */ + int enable_pin; /*!< enable pin number */ + int mcpwm_group_id; /*!< mcpwm group id */ + bool enable_active_high = true; + float dead_zone; /*!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1] */ + PhaseState phase_state[3]; /*!< phase state (active / disabled) */ + + /** + * @brief Set voltage to the pwm pin + * + * @param Ua phase A voltage + * @param Ub phase B voltage + * @param Uc phase C voltage + */ + void setPwm(float Ua, float Ub, float Uc) override; + + /** + * @brief Set the phase state + * + * @param sa phase A state + * @param sb phase B state + * @param sc phase C state + */ + virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) override; + + /** + * @brief PWM generating function + * + */ + void halPwmWrite(); + +private: + uint32_t mcpwm_period; + mcpwm_timer_handle_t timer = NULL; + mcpwm_oper_handle_t oper[3]; + mcpwm_cmpr_handle_t comparator[3]; + mcpwm_gen_handle_t generator[3][2]; +}; diff --git a/components/motor/esp_simplefoc/port/esp/include/esp_simplefoc.h b/components/motor/esp_simplefoc/port/esp/include/esp_simplefoc.h index d748c8a3f..5529fe1d0 100644 --- a/components/motor/esp_simplefoc/port/esp/include/esp_simplefoc.h +++ b/components/motor/esp_simplefoc/port/esp/include/esp_simplefoc.h @@ -8,6 +8,7 @@ #include "esp_platform.h" #include "BLDCMotor.h" #include "esp_hal_bldc_3pwm.h" +#include "esp_hal_bldc_6pwm.h" #include "communication/SimpleFOCDebug.h" #include "communication/Commander.h" #include "sensors/GenericSensor.h" diff --git a/components/motor/esp_simplefoc/test_apps/main/test_esp_simplefoc.cpp b/components/motor/esp_simplefoc/test_apps/main/test_esp_simplefoc.cpp index 3fabca99e..a53f268de 100644 --- a/components/motor/esp_simplefoc/test_apps/main/test_esp_simplefoc.cpp +++ b/components/motor/esp_simplefoc/test_apps/main/test_esp_simplefoc.cpp @@ -78,6 +78,21 @@ TEST_CASE("test as5048a", "[sensor][as5048a][spi]") as5048a.deinit(); } +#if CONFIG_SOC_MCPWM_SUPPORTED +TEST_CASE("test 6pwm driver", "[driver][6pwm]") +{ + BLDCDriver6PWM driver = BLDCDriver6PWM(1, 2, 3, 4, 5, 6); + TEST_ASSERT_EQUAL(driver.init(), 1); + driver.dc_a = 0.2; + driver.dc_b = 0.4; + driver.dc_c = 0.8; + + driver.halPwmWrite(); + vTaskDelay(2000 / portTICK_PERIOD_MS); + driver.deinit(); +} +#endif + TEST_CASE("test esp_simplefoc openloop control", "[single motor][openloop][14pp][ledc][drv8313][c3]") { BLDCMotor motor = BLDCMotor(14);