From 6064126c792384023f5a7a84612d662e577b8143 Mon Sep 17 00:00:00 2001 From: Demian Zenkov Date: Fri, 5 Jul 2024 14:40:47 +0300 Subject: [PATCH] feat(motor): as5048a encoder support - 14-bit rotary position encoder AS5048A support added --- components/motor/esp_simplefoc/CHANGELOG.md | 6 + components/motor/esp_simplefoc/CMakeLists.txt | 5 +- .../motor/esp_simplefoc/idf_component.yml | 2 +- .../port/angle_sensor/as5048a.cpp | 123 ++++++++++++++++++ .../esp_simplefoc/port/angle_sensor/as5048a.h | 61 +++++++++ .../port/esp/include/esp_simplefoc.h | 1 + .../test_apps/main/test_esp_simplefoc.cpp | 15 +++ 7 files changed, 210 insertions(+), 3 deletions(-) create mode 100644 components/motor/esp_simplefoc/port/angle_sensor/as5048a.cpp create mode 100644 components/motor/esp_simplefoc/port/angle_sensor/as5048a.h diff --git a/components/motor/esp_simplefoc/CHANGELOG.md b/components/motor/esp_simplefoc/CHANGELOG.md index cf798dd97..d94463c99 100644 --- a/components/motor/esp_simplefoc/CHANGELOG.md +++ b/components/motor/esp_simplefoc/CHANGELOG.md @@ -1,5 +1,11 @@ # ChangeLog + +## v0.2.0 - 2024-7-5 +### Enhancements: +* FOC: + * Support for as5048a encoder added + ## v0.1.2 - 2023-12-11 ### Bug Fix: * FOC: diff --git a/components/motor/esp_simplefoc/CMakeLists.txt b/components/motor/esp_simplefoc/CMakeLists.txt index d283faf98..25e316dea 100644 --- a/components/motor/esp_simplefoc/CMakeLists.txt +++ b/components/motor/esp_simplefoc/CMakeLists.txt @@ -6,11 +6,12 @@ set(SRC_FILES "port/esp/esp_app_fmath.cpp" "port/esp/esp_app_print.cpp" "port/angle_sensor/as5600.cpp" - "port/angle_sensor/mt6701.cpp") + "port/angle_sensor/mt6701.cpp" + "port/angle_sensor/as5048a.cpp") set(INC_FILES "port/esp/include/" "port/angle_sensor") set(IGNORE_FILES "port/esp/esp_hal_adc.cpp" "port/esp/esp_hal_mcpwm.cpp" - "port/esp/esp_hal_bldc_3pwm.cpp" "port/angle_sensor/mt6701.cpp") + "port/esp/esp_hal_bldc_3pwm.cpp" "port/angle_sensor/mt6701.cpp" "port/angle_sensor/as5048a.cpp") idf_component_register( SRCS diff --git a/components/motor/esp_simplefoc/idf_component.yml b/components/motor/esp_simplefoc/idf_component.yml index 44dde4277..eb317105c 100644 --- a/components/motor/esp_simplefoc/idf_component.yml +++ b/components/motor/esp_simplefoc/idf_component.yml @@ -1,4 +1,4 @@ -version: 0.1.1 +version: 0.2.0 targets: - esp32 - esp32s2 diff --git a/components/motor/esp_simplefoc/port/angle_sensor/as5048a.cpp b/components/motor/esp_simplefoc/port/angle_sensor/as5048a.cpp new file mode 100644 index 000000000..b3a5f0b60 --- /dev/null +++ b/components/motor/esp_simplefoc/port/angle_sensor/as5048a.cpp @@ -0,0 +1,123 @@ +/* + * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ +#include "as5048a.h" +#include "esp_log.h" +#include "esp_check.h" + +#define AS5048A_REG_ANGLE 0x3FFF +#define PI 3.14159265358979f + +static const char *TAG = "AS5048a"; + +AS5048a::AS5048a(spi_host_device_t spi_host, gpio_num_t sclk_io, gpio_num_t miso_io, gpio_num_t mosi_io, gpio_num_t cs_io) +{ + _spi_host = spi_host; + _sclk_io = sclk_io; + _miso_io = miso_io; + _mosi_io = mosi_io; + _cs_io = cs_io; + is_installed = false; +} + +AS5048a::~AS5048a() +{ + if (is_installed) { + deinit(); + } +} + +void AS5048a::deinit() +{ + esp_err_t ret; + ret = spi_bus_remove_device(spi_device); + ESP_RETURN_ON_FALSE(ret == ESP_OK,, TAG, "SPI remove device fail"); + ret = spi_bus_free(_spi_host); + ESP_RETURN_ON_FALSE(ret == ESP_OK,, TAG, "SPI free fail"); + is_installed = false; +} + +void AS5048a::init() +{ + esp_err_t ret; + + /*!< Configuration for the spi bus */ + spi_bus_config_t buscfg = { + .mosi_io_num = _mosi_io, + .miso_io_num = _miso_io, + .sclk_io_num = _sclk_io, + .quadwp_io_num = -1, + .quadhd_io_num = -1, + .max_transfer_sz = 1000, + }; + ret = spi_bus_initialize(_spi_host, &buscfg, SPI_DMA_CH_AUTO); + ESP_RETURN_ON_FALSE(ret == ESP_OK,, TAG, "SPI bus init fail"); + + spi_device_interface_config_t devcfg = { + .command_bits = 0, + .address_bits = 0, + .dummy_bits = 0, + .mode = 1, + .cs_ena_pretrans = 1, + .clock_speed_hz = 4 * 1000 * 1000, + .spics_io_num = _cs_io, + .queue_size = 1, + }; + + ret = spi_bus_add_device(_spi_host, &devcfg, &spi_device); + ESP_RETURN_ON_FALSE(ret == ESP_OK,, TAG, "SPI bus add device fail"); + + is_installed = true; +} + +uint8_t AS5048a::calculateParity(uint16_t value) +{ + uint8_t count = 0; + for (int i = 0; i < 16; i++) { + if (value & 0x1) { + count++; + } + value >>= 1; + } + return count & 0x1; +} + +uint16_t AS5048a::readRegister(uint16_t reg_address) +{ + uint16_t command = 1 << 14; // PAR=0 R/W=R (Read command) + command |= reg_address; // Command to read angle + command |= ((uint16_t)calculateParity(command) << 15); // Adding parity bit + + uint8_t cmd_high = (command >> 8) & 0xFF; + uint8_t cmd_low = command & 0xFF; + uint8_t tx_buffer[2] = {cmd_high, cmd_low}; + spi_transaction_t t = {}; + t.length = 16; // 16 bits + t.tx_buffer = tx_buffer; + t.rx_buffer = NULL; + spi_device_transmit(spi_device, &t); + + uint8_t rx_buffer[2] = {}; + t.length = 16; // 16 bits + t.tx_buffer = NULL; + t.rxlength = 16; // 16 bits + t.rx_buffer = rx_buffer; + spi_device_transmit(spi_device, &t); + + uint16_t reg_value = (rx_buffer[0] << 8) | rx_buffer[1]; + return reg_value & 0x3FFF; // Masking to get 14 bits angle value +} + +uint16_t AS5048a::readRawPosition() +{ + return readRegister(AS5048A_REG_ANGLE); +} + +float AS5048a::getSensorAngle() +{ + uint16_t raw_angle = readRawPosition(); + float angle = (((int)raw_angle & 0b0011111111111111) * 360.0f / 16383.0f) * (PI / 180.0f); + return angle; +} diff --git a/components/motor/esp_simplefoc/port/angle_sensor/as5048a.h b/components/motor/esp_simplefoc/port/angle_sensor/as5048a.h new file mode 100644 index 000000000..85fd11d82 --- /dev/null +++ b/components/motor/esp_simplefoc/port/angle_sensor/as5048a.h @@ -0,0 +1,61 @@ +/* + * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#pragma once + +#include "common/base_classes/Sensor.h" +#include "driver/spi_master.h" +#include "driver/gpio.h" + +class AS5048a : public Sensor { +public: + /** + * @brief Construct a new AS5048a object + * + * @param spi_host + * @param sclk_io + * @param miso_io + * @param mosi_io + * @param cs_io + */ + AS5048a(spi_host_device_t spi_host, gpio_num_t sclk_io, gpio_num_t miso_io, gpio_num_t mosi_io, gpio_num_t cs_io); + /** + * @brief Destroy the AS5048a object + * + */ + ~AS5048a(); + /** + * @brief Init spi for AS5048a + * + */ + void init(); + /** + * @brief Deinit spi for AS5048a + * + */ + void deinit(); + /** + * @brief Get the output of AS5048a + * + * @return float + */ + float getSensorAngle(); + +private: + uint16_t readRegister(uint16_t reg_address); + uint16_t readRawPosition(); + static uint8_t calculateParity(uint16_t value); + +private: + spi_host_device_t _spi_host; + gpio_num_t _sclk_io; + gpio_num_t _miso_io; + gpio_num_t _mosi_io; + gpio_num_t _cs_io; + spi_device_handle_t spi_device; + bool is_installed; + +}; 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 0deb5d840..d748c8a3f 100644 --- a/components/motor/esp_simplefoc/port/esp/include/esp_simplefoc.h +++ b/components/motor/esp_simplefoc/port/esp/include/esp_simplefoc.h @@ -15,3 +15,4 @@ #include "current_sense/LowsideCurrentSense.h" #include "../../angle_sensor/as5600.h" #include "../../angle_sensor/mt6701.h" +#include "../../angle_sensor/as5048a.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 72d7e29ac..7d5a7198f 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 @@ -42,6 +42,21 @@ TEST_CASE("test mt6701", "[sensor][mt6701]") mt6701.deinit(); } +TEST_CASE("test as5048a", "[sensor][as5048a]") +{ +#if CONFIG_IDF_TARGET_ESP32C3 + AS5048a as5048a = AS5048a(SPI2_HOST, GPIO_NUM_2, GPIO_NUM_1, (gpio_num_t) -1, GPIO_NUM_3); +#else + AS5048a as5048a = AS5048a(SPI2_HOST, GPIO_NUM_2, GPIO_NUM_1, (gpio_num_t) -1, GPIO_NUM_42); +#endif + as5048a.init(); + for (int i = 0; i < 10; ++i) { + ESP_LOGI(TAG, "angle:%.2f", as5048a.getSensorAngle()); + vTaskDelay(1000 / portTICK_PERIOD_MS); + } + as5048a.deinit(); +} + TEST_CASE("test esp_simplefoc openloop control", "[single motor][openloop][14pp][ledc][drv8313][c3]") { BLDCMotor motor = BLDCMotor(14);