diff --git a/README.md b/README.md index 268e95c..ae90190 100644 --- a/README.md +++ b/README.md @@ -3,22 +3,26 @@ ![Build](https://github.com/uLipe/espFoC/workflows/Build/badge.svg) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) -espFoC is a simple implementation of voltage mode, vector controller intended to be used with permanent-magnet synchronous motors (PMSM), and general brushless motors. This component was developed to be used with Zephyr RTOS for example and is aimed to transform -your esp32 chip into a dual axis PMSM motor controller chip +espFoC is a simple implementation of voltage mode, vector controller intended to be used with permanent-magnet synchronous motors (PMSM), and general brushless motors for ESP32S3 Espressif SoC, other SoC from espressif might be supported but the primary requirement is to be a dual +core variant. espFoC started as a library for motor control, but it suffered a goal change, and now it is intended to create a programmable +BLDC (FoC based) motor control-IC, the dual core capabillity splits the controller into application side and motor control core side +the former is intended for user to write it own application or exchange packets with external controller via CAN-Bus, the later is +100% focused on perform motor control algorithm achieving better control bandwidth. Up two axis are supported in this new variant, refer the +simplified architecture below: +![espFoC Simplified Architecture](/doc/images/arch.png) ## Features: * Voltage mode control, control a PMSM like a DC motor!; * Position and Speed closed-loop control; * Single-precision Floating point implementation; -* Sample inverter driver based on esp32 LEDC PWM (easy to wire!); -* Sample rotor position driver based on as5600 encoder (very popular!); +* Easy to wire motor using common drivers and I2C encoders out there! * Uses openAMP to offload motor control tasks to one of the core, and leave the other for communication; * support UART and CAN communication; ## Limitations: -* Support for esp32 and esp32s3 only; +* Support for esp32 and esp32s3(primary) only; * Requires and rotor position sensor, for example, incremental encoder. ## Getting started: diff --git a/app/CMakeLists.txt b/app/CMakeLists.txt new file mode 100644 index 0000000..6243673 --- /dev/null +++ b/app/CMakeLists.txt @@ -0,0 +1,40 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) + +set(REMOTE_ZEPHYR_DIR ${CMAKE_CURRENT_BINARY_DIR}/ipm_esp32_appcpu-prefix/src/ipm_esp32_appcpu-build/zephyr) + +if("${BOARD}" STREQUAL "esp32s3_devkitm/esp32s3/procpu") +set(BOARD_REMOTE "esp32s3_devkitm/esp32s3/appcpu") +else() + message(FATAL_ERROR "${BOARD} was not supported for this project yet!") +endif() + +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(espfoc) + +set(MOTOR_CONTROL_SRC ${APPLICATION_SOURCE_DIR}/../common) +set(MOTOR_CONTROL_INC ${APPLICATION_SOURCE_DIR}/../common/include/espFoC) + +FILE(GLOB ipc ${MOTOR_CONTROL_SRC}/ipc/*.c) + +set_source_files_properties(${REMOTE_ZEPHYR_DIR}/esp32_appcpu_firmware.c PROPERTIES GENERATED TRUE) +target_sources(app PRIVATE esp_foc_app_core/esp_foc_app.c + esp_foc_app_core/esp_foc_app_shell.c + ${ipc} + ${REMOTE_ZEPHYR_DIR}/esp32_appcpu_firmware.c) + +target_include_directories(app PRIVATE ${MOTOR_CONTROL_INC}) + +include(ExternalProject) + +ExternalProject_Add( + espfoc_remote + SOURCE_DIR ${APPLICATION_SOURCE_DIR}/espfoc_motor_core + INSTALL_COMMAND "" + CMAKE_CACHE_ARGS -DBOARD:STRING=${BOARD_REMOTE} + BUILD_BYPRODUCTS "${REMOTE_ZEPHYR_DIR}/${KERNEL_BIN_NAME}" + BUILD_ALWAYS True +) + +add_dependencies(app espfoc_remote) diff --git a/app/boards/esp32s3_devkitm.conf b/app/boards/esp32s3_devkitm.conf new file mode 100644 index 0000000..a8ee714 --- /dev/null +++ b/app/boards/esp32s3_devkitm.conf @@ -0,0 +1 @@ +CONFIG_SOC_ESP32S3_PROCPU=y diff --git a/app/boards/esp32s3_devkitm.overlay b/app/boards/esp32s3_devkitm.overlay new file mode 100644 index 0000000..ad437c1 --- /dev/null +++ b/app/boards/esp32s3_devkitm.overlay @@ -0,0 +1,19 @@ +/* + * Copyright (c) 2023 Felipe Neves + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/ { + chosen { + /* + * shared memory reserved for the inter-processor communication + */ + zephyr,ipc_shm = &shm0; + zephyr,ipc = &ipm0; + }; +}; + +&ipm0 { + status = "okay"; +}; diff --git a/CMakeLists.txt b/app/espfoc_app_core/esp_foc_app.c similarity index 100% rename from CMakeLists.txt rename to app/espfoc_app_core/esp_foc_app.c diff --git a/app/espfoc_app_core/esp_foc_app_shell.c b/app/espfoc_app_core/esp_foc_app_shell.c new file mode 100644 index 0000000..e69de29 diff --git a/app/espfoc_motor_core/CMakeLists.txt b/app/espfoc_motor_core/CMakeLists.txt new file mode 100644 index 0000000..dceaa4b --- /dev/null +++ b/app/espfoc_motor_core/CMakeLists.txt @@ -0,0 +1,19 @@ +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) + +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(espfoc_remote) + +set(MOTOR_CONTROL_SRC ${APPLICATION_SOURCE_DIR}/../../common) +set(MOTOR_CONTROL_INC ${APPLICATION_SOURCE_DIR}/../../common/include/espFoC) + +FILE(GLOB ipc ${MOTOR_CONTROL_SRC}/ipc/*.c) +FILE(GLOB motor_control ${MOTOR_CONTROL_SRC}/motor_control/*.c) + +target_sources(app PRIVATE esp_foc_motor.c + esp_foc_hardware_if.c + ${ipc} + ${motor_control}) + +target_include_directories(app PRIVATE ${MOTOR_CONTROL_INC}) diff --git a/app/espfoc_motor_core/boards/esp32s3_devkitm_appcpu.overlay b/app/espfoc_motor_core/boards/esp32s3_devkitm_appcpu.overlay new file mode 100644 index 0000000..e82605e --- /dev/null +++ b/app/espfoc_motor_core/boards/esp32s3_devkitm_appcpu.overlay @@ -0,0 +1,172 @@ +/* + * Copyright (c) 2023 Felipe Neves + * + * SPDX-License-Identifier: Apache-2.0 + */ + #include + #include + #include + +/ { + chosen { + /* + * shared memory reserved for the inter-processor communication + */ + zephyr,ipc_shm = &shm0; + zephyr,ipc = &ipm0; + }; + + aliases { + motor-pwm-0 = &ledc0; + motor-pwm-1 = &ledc1; + angle-0 = &i2c0; + angle-0 = &i2c1; + }; + + gpio_enable { + compatible = "gpio-keys"; + inverter_enable_0: button { + label = "inverter_enable"; + gpios = <&gpio0 9 GPIO_ACTIVE_HIGH>; + }; + + inverter_enable_1: button { + label = "inverter_enable"; + gpios = <&gpio0 10 GPIO_ACTIVE_HIGH>; + }; + + }; +}; + +&pinctrl { + ledc0_default: ledc0_default { + group1 { + pinmux = ; + output-enable; + }; + group2 { + pinmux = ; + output-enable; + }; + group3 { + pinmux = ; + output-enable; + }; + + }; + + i2c0_default: i2c0_default { + group1 { + pinmux = , + ; + bias-pull-up; + drive-open-drain; + output-high; + }; + }; + + ledc1_default: ledc1_default { + group1 { + pinmux = ; + output-enable; + }; + group2 { + pinmux = ; + output-enable; + }; + group3 { + pinmux = ; + output-enable; + }; + + }; + + i2c1_default: i2c1_default { + group1 { + pinmux = , + ; + bias-pull-up; + drive-open-drain; + output-high; + }; + }; + +}; + +&ipm0 { + status = "okay"; +}; + +&ledc0 { + pinctrl-0 = <&ledc0_default>; + pinctrl-names = "default"; + status = "okay"; + #address-cells = <1>; + #size-cells = <0>; + channel0@0 { + reg = <0x0>; + timer = <0>; + }; + + channel1@0 { + reg = <0x0>; + timer = <0>; + }; + + channel2@0 { + reg = <0x0>; + timer = <0>; + }; +}; + +&i2c0 { + pinctrl-0 = <&i2c0_default>; + pinctrl-names = "default"; + status = "okay"; + clock-frequency = ; + + angle_sensor_0: as5600@36 { + compatible = "ams,as5600"; + status = "okay"; + reg = <0x36> + }; +}; + +&ledc1 { + pinctrl-0 = <&ledc1_default>; + pinctrl-names = "default"; + status = "okay"; + #address-cells = <1>; + #size-cells = <0>; + channel0@0 { + reg = <0x0>; + timer = <0>; + }; + + channel1@0 { + reg = <0x0>; + timer = <0>; + }; + + channel2@0 { + reg = <0x0>; + timer = <0>; + }; +}; + +&i2c1 { + pinctrl-0 = <&i2c1_default>; + pinctrl-names = "default"; + status = "okay"; + clock-frequency = ; + + angle_sensor_0: as5600@36 { + compatible = "ams,as5600"; + status = "okay"; + reg = <0x36> + }; +}; + +&timer3 { + status = "okay"; +}; \ No newline at end of file diff --git a/app/espfoc_motor_core/esp_foc_hardware_if.c b/app/espfoc_motor_core/esp_foc_hardware_if.c new file mode 100644 index 0000000..ad476ff --- /dev/null +++ b/app/espfoc_motor_core/esp_foc_hardware_if.c @@ -0,0 +1,268 @@ +/* + * 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. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "esp_foc_hardware_if.h" + +#define PWM_PERIOD_NSEC 31250 +#define DC_BUS_VOLTAGE 24.0f + +struct z_esp_foc_motor_if { + struct device *pwm_device; + struct device *angle_sensor; + struct gpio_dt_spec enable_pins; + float accumulated_angle_deg; + float current_rotor_deg; + float prev_rotor_deg; + float current_rotor_speed_dps; + struct esp_foc_motor_interface interface; +}; + +static int reset(struct esp_foc_motor_interface *self) +{ + int ret; + + ret = esp_foc_motor_enable(self); + if (ret < 0) { + return ret; + } + + return esp_foc_motor_align_rotor(self); +} + +static int enable(struct esp_foc_motor_interface *self) +{ + struct z_esp_foc_motor_if *if = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface); + + ret = esp_foc_motor_set_duty_cycles(self, 0.0f, 0.0f, 0.0f); + if (ret < 0) { + return ret; + } + + k_msleep(100); + + gpio_pin_configure_dt(&enable_pins, GPIO_OUTPUT); + gpio_pin_set_dt(&enable_pins, 1); + + k_msleep(100); + + return 0; +} + +static int disable(struct esp_foc_motor_interface *self) +{ + struct z_esp_foc_motor_if *if = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface); + + gpio_pin_configure_dt(&enable_pins, GPIO_OUTPUT); + gpio_pin_set_dt(&enable_pins, 1); + esp_foc_motor_set_duty_cycles(self, 0.0f, 0.0f, 0.0f); + k_msleep(100); + + return 0; +} + +static int align_rotor(struct esp_foc_motor_interface *self) +{ + int ret; + + ret = esp_foc_motor_set_duty_cycles(self, 0.0f, 0.0f, 0.0f); + if (ret < 0) { + return ret; + } + + k_msleep(500); + + ret = esp_foc_motor_set_duty_cycles(self, 0.2f, 0.0f, 0.0f); + if (ret < 0) { + return ret; + } + + k_msleep(500); + + return esp_foc_motor_fetch_sensors(self); +} + +static int set_duty_cycles(struct esp_foc_motor_interface *self, float dc_a, float dc_b, float dc_c) +{ + struct z_esp_foc_motor_if *if = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface); + + if(dc_a > 1.0f) { + dc_a = 1.0f; + } else if (dc_a < 0.0f) { + dc_a = 0.0f; + } + + if(dc_b > 1.0f) { + dc_b = 1.0f; + } else if (dc_b < 0.0f) { + dc_b = 0.0f; + } + + if(dc_c > 1.0f) { + dc_c = 1.0f; + } else if (dc_c < 0.0f) { + dc_c = 0.0f; + } + + dc_a *= PWM_PERIOD_NSEC - 1; + dc_b *= PWM_PERIOD_NSEC - 1; + dc_c *= PWM_PERIOD_NSEC - 1; + + pwm_set(pwm_device, 1 , PWM_PERIOD_NSEC, dc_a, 0); + pwm_set(pwm_device, 2 , PWM_PERIOD_NSEC, dc_b, 0); + pwm_set(pwm_device, 3 , PWM_PERIOD_NSEC, dc_c, 0); + + return 0; +} + +static int get_rotor_angle(struct esp_foc_motor_interface *self, float *angle) +{ + struct z_esp_foc_motor_if *if = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface); + + if(!angle) + return -EINVAL; + + *angle = if->current_rotor_deg; + + return 0; +} + +static int get_rotor_speed_dps(struct esp_foc_motor_interface *self, float *speed_dps) +{ + struct z_esp_foc_motor_if *if = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface); + + if(!speed_dps) + return -EINVAL; + + *speed_dps = if->current_rotor_speed_dps; + + return 0; +} + +static int get_acumulated_angle(struct esp_foc_motor_interface *self, float *angle) +{ + struct z_esp_foc_motor_if *if = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface); + + if(!angle) + return -EINVAL; + + *angle = if->accumulated_angle_deg; + + return 0; +} + +static int get_dc_bus_voltage(struct esp_foc_motor_interface *self, float *vdc) +{ + if(!vdc) + return -EINVAL; + + *vdc = DC_BUS_VOLTAGE; + + return 0; +} + +static int fetch_sensors(struct esp_foc_motor_interface *self) +{ + struct z_esp_foc_motor_if *if = CONTAINER_OF(self, struct z_esp_foc_motor_if, interface); + struct sensor_value raw; + int ret; + + if->prev_rotor_deg = if->current_rotor_deg; + + ret = sensor_sample_fetch(if->angle_sensor); + if(ret) { + return ret; + } + + ret = sensor_channel_get(if->angle_sensor, SENSOR_CHAN_ROTATION, &raw); + if(ret) { + return ret; + } + + if->current_rotor_deg = sensor_value_to_float(&raw); + if->current_rotor_speed_dps = (if->current_rotor_deg - if->prev_rotor_deg); + if->accumulated_angle_deg += if->current_rotor_speed_dps; + + return ret; +} + +static struct z_esp_foc_motor_if z_esp_hw_if[Z_ESP_FOC_MAX] = { + { + .pwm_device = DEVICE_DT_GET(DT_NODELABEL(ledc0)), + .angle_sensor = DEVICE_DT_GET(DT_NODELABEL(angle_sensor_0)), + .enable_pins = GPIO_DT_SPEC_GET_BY_IDX(DT_NODELABEL(inverter_enable_0),gpios,0), + .reset = reset, + .enable = enable, + .disable = disable, + .set_duty_cycles = set_duty_cycles, + .get_rotor_angle = get_rotor_angle, + .get_rotor_speed_dps = get_rotor_speed_dps, + .align_rotor = align_rotor, + .get_acumulated_angle = get_acumulated_angle, + .get_encoder_ppr = NULL, + .get_dc_bus_voltage = get_dc_bus_voltage, + .get_currents = NULL, + .number_of_shunts = NULL, + .fetch_sensors = fetch_sensors, + .current_rotor_deg = 0.0f, + .current_rotor_speed_dps = 0.0f, + .accumulated_angle_deg = 0.0f, + }, + { + .pwm_device = DEVICE_DT_GET(DT_NODELABEL(ledc1)), + .angle_sensor = DEVICE_DT_GET(DT_NODELABEL(angle_sensor_1)), + .enable_pins = GPIO_DT_SPEC_GET_BY_IDX(DT_NODELABEL(inverter_enable_1),gpios,0), + .reset = reset, + .enable = enable, + .disable = disable, + .set_duty_cycles = set_duty_cycles, + .get_rotor_angle = get_rotor_angle, + .get_rotor_speed_dps = get_rotor_speed_dps, + .align_rotor = align_rotor, + .get_acumulated_angle = get_acumulated_angle, + .get_encoder_ppr = NULL, + .get_dc_bus_voltage = get_dc_bus_voltage, + .get_currents = NULL, + .number_of_shunts = NULL, + .fetch_sensors = fetch_sensors, + .current_rotor_deg = 0.0f, + .current_rotor_speed_dps = 0.0f, + .accumulated_angle_deg = 0.0f, + } +} + +struct esp_foc_motor_interface* z_esp_foc_get_interface(enum zephyr_esp_foc_instances instance) +{ + if(instance >= Z_ESP_FOC_MAX) + return NULL; + + return &z_esp_hw_if.self[instance]; +} diff --git a/examples/stubs/motor_hardware_stub.h b/app/espfoc_motor_core/esp_foc_hardware_if.h similarity index 82% rename from examples/stubs/motor_hardware_stub.h rename to app/espfoc_motor_core/esp_foc_hardware_if.h index cc544d8..abe7b4d 100644 --- a/examples/stubs/motor_hardware_stub.h +++ b/app/espfoc_motor_core/esp_foc_hardware_if.h @@ -21,13 +21,14 @@ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * SOFTWARE. */ -#pragma once +#pragma once -#include +#include -struct motor_hardware_stub { - struct esp_foc_motor_interface self; +enum zephyr_esp_foc_instances { + Z_ESP_FOC_1 = 0, + Z_ESP_FOC_2, + Z_ESP_FOC_MAX; }; -int motor_hardware_stub_init(struct motor_hardware_stub *stub, - struct esp_foc_motor_interface **itf); +struct esp_foc_motor_interface* z_esp_foc_get_interface(enum zephyr_esp_foc_instances instance); \ No newline at end of file diff --git a/app/espfoc_motor_core/esp_foc_motor.c b/app/espfoc_motor_core/esp_foc_motor.c new file mode 100644 index 0000000..25624ea --- /dev/null +++ b/app/espfoc_motor_core/esp_foc_motor.c @@ -0,0 +1,82 @@ +/* + * 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. + */ + +#include +#include +#include +#include + +#define ESP_FOC_INNER_CONTROL_US_PERIOD (250) + +static const struct device *timer_dev = DEVICE_DT_GET(DT_INST(3,espressif_esp32_timer)); +static float now_seconds = 0.0f; +static struct counter_alarm_cfg alarm_cfg; +static struct esp_foc_motor_control axis; + +static struct esp_foc_pid pid_vel_1; +static struct esp_foc_pid pid_pos_1; + +static void esp_foc_control_work_handler(struct k_work *work) +{ + esp_foc_run(&axis); +} + +K_WORK_DEFINE(esp_foc_work, motor_control_work_handler); + +static void esp_foc_timer_isr(const struct device *counter_dev, + uint8_t chan_id, uint32_t ticks, + void *user_data) +{ + k_work_submit(&esp_foc_work); + counter_set_channel_alarm(timer_dev, 0, &alarm_cfg); +} + +/* We don't actually need a main function*/ +static int esp_foc_early_init(void) +{ + float sample_time_seconds = (float)ESP_FOC_INNER_CONTROL_US_PERIOD * 0.000001f; + + esp_foc_pid_init(&pid_vel_1, + sample_time_seconds * ESP_FOC_SPEED_CONTROL_RATIO, + 10000.0f); + + esp_foc_pid_init(&pid_pos_1, + sample_time_seconds * ESP_FOC_POSITION_CONTROL_RATIO, + 10000.0f); + + esp_foc_init_controller(&axis, z_esp_foc_get_interface(Z_ESP_FOC_1)); + esp_foc_add_position_control(&axis, &pid_pos_1); + esp_foc_add_speed_control(&axis, &pid_vel_1); + + counter_start(timer_dev); + alarm_cfg.flags = 0; + alarm_cfg.ticks = counter_us_to_ticks(timer_dev, CONTROL_PIPELINE_US_PERIOD); + alarm_cfg.callback = esp_foc_timer_isr; + alarm_cfg.user_data = &alarm_cfg; + counter_set_channel_alarm(timer_dev, 0, &alarm_cfg); + + return 0; +} + +SYS_INIT(esp_foc_early_init, POST_KERNEL, CONFIG_APPLICATION_INIT_PRIORITY); diff --git a/app/espfoc_motor_core/prj.conf b/app/espfoc_motor_core/prj.conf new file mode 100644 index 0000000..7b505d2 --- /dev/null +++ b/app/espfoc_motor_core/prj.conf @@ -0,0 +1,13 @@ +CONFIG_STDOUT_CONSOLE=n +CONFIG_PRINTK=n +CONFIG_HEAP_MEM_POOL_SIZE=256 + +CONFIG_IPM=y +CONFIG_PWM=y +CONFIG_I2C=y +CONFIG_SENSOR=y +CONFIG_COUNTER=y + +CONFIG_SENSOR=y +CONFIG_AMS_AS5600=y +CONFIG_FLOAT=y \ No newline at end of file diff --git a/app/prj.conf b/app/prj.conf new file mode 100644 index 0000000..3393bef --- /dev/null +++ b/app/prj.conf @@ -0,0 +1,6 @@ +CONFIG_PRINTK=y +CONFIG_IPM=y +CONFIG_TIMESLICE_SIZE=1 +CONFIG_MAIN_STACK_SIZE=8192 +CONFIG_HEAP_MEM_POOL_SIZE=2048 +CONFIG_FLOAT=y \ No newline at end of file diff --git a/common/include/espFoC/ipc/esp_foc_ipc.h b/common/include/espFoC/ipc/esp_foc_ipc.h new file mode 100644 index 0000000..0eed4ef --- /dev/null +++ b/common/include/espFoC/ipc/esp_foc_ipc.h @@ -0,0 +1,79 @@ +/* + * MIT License + * + * Copyright (c) 2024 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 +#include +#include + +#define MOTOR_CMD_TORQUE_MASK 0x00 +#define MOTOR_CMD_SPEED_MASK 0x02 +#define MOTOR_CMD_POSITION_MASK 0x04 +#define MOTOR_CMD_SHUTDOWN_MASK 0x08 +#define MOTOR_CMD_RESET_MASK 0x10 + +struct motor_state { + uint32_t timestamp_us; + uint32_t position_mdeg; + uint32_t speed_mdps; + uint32_t qvoltage_mvolts; + uint32_t dvoltage_mvolts; + uint32_t encoder; + uint8_t power_state; + uint8_t last_cmd_result; + uint8_t motor_number; + uint8_t direction; +}; + +struct motor_command { + uint32_t command_mask; + uint32_t speed_mdps; + uint32_t qvoltage_mvolts; + uint32_t dvoltage_mvolts; + uint8_t power_state; + uint8_t motor_number; + uint8_t direction; + uint8_t res3; +}; + +typedef void (*esp_foc_ipc_motor_state_callback_t) (const struct motor_state *state); +typedef void (*esp_foc_ipc_motor_cmd_callback_t) (const struct motor_command *cmd); + +struct motor_report_callback { + esp_foc_ipc_motor_state_callback_t cmd_cb; + esp_foc_ipc_motor_cmd_callback_t state_cb + sys_dnode_t node; +}; + +#define ESP_FOC_DEFINE_IPC_CALLBACK(name,cmd_callback, state_callback) \ +struct motor_report_callback cn_##name = { \ + .state_cb = state_callback, \ + .state_cb = cmd_callback, \ +} + +int esp_foc_ipc_init(void); +int esp_foc_ipc_send_command(const struct motor_command *cmd); +int esp_foc_ipc_send_state(const struct motor_state *state); +int esp_foc_ipc_register_callback(const struct motor_report_callback *cb); diff --git a/include/espFoC/esp_foc.h b/common/include/espFoC/motor_control/esp_foc.h similarity index 86% rename from include/espFoC/esp_foc.h rename to common/include/espFoC/motor_control/esp_foc.h index 0105b2a..90b22a4 100644 --- a/include/espFoC/esp_foc.h +++ b/common/include/espFoC/motor_control/esp_foc.h @@ -24,11 +24,12 @@ #pragma once +#include #include #include -#include -#include -#include +#include "esp_foc_motor_interface.h" +#include "esp_foc_svm.h" +#include "esp_foc_pid.h" struct esp_foc_motor_control { struct esp_foc_motor_interface *motor_hw; @@ -42,8 +43,13 @@ struct esp_foc_motor_control { float target_position_degrees; float current_speed_dps; float current_position_degrees; + + bool is_enabled; }; +extern const int ESP_FOC_SPEED_CONTROL_RATIO; +extern const int ESP_FOC_POSITION_CONTROL_RATIO; + int esp_foc_init_controller(struct esp_foc_motor_control *ctl, const struct esp_foc_motor_interface *hw); @@ -59,4 +65,8 @@ int esp_foc_add_position_control(struct esp_foc_motor_control *ctl, int esp_foc_add_speed_control(struct esp_foc_motor_control *ctl, const struct esp_foc_pid *speed_pid); +int esp_foc_enable_axis(struct esp_foc_motor_control *ctl); + +int esp_foc_disable_axis(struct esp_foc_motor_control *ctl); + int esp_foc_controller_run(struct esp_foc_motor_control *ctl); diff --git a/include/espFoC/esp_foc_motor_interface.h b/common/include/espFoC/motor_control/esp_foc_motor_interface.h similarity index 72% rename from include/espFoC/esp_foc_motor_interface.h rename to common/include/espFoC/motor_control/esp_foc_motor_interface.h index 3322acd..a103a23 100644 --- a/include/espFoC/esp_foc_motor_interface.h +++ b/common/include/espFoC/motor_control/esp_foc_motor_interface.h @@ -27,6 +27,7 @@ struct esp_foc_motor_interface { int (*reset)(struct esp_foc_motor_interface *self); int (*enable)(struct esp_foc_motor_interface *self); + int (*disable)(struct esp_foc_motor_interface *self); int (*set_duty_cycles)(struct esp_foc_motor_interface *self, float a, float b, float c); int (*get_rotor_angle)(struct esp_foc_motor_interface *self, float *angle); int (*get_rotor_speed_dps)(struct esp_foc_motor_interface *self, float *speed_dps); @@ -39,7 +40,7 @@ struct esp_foc_motor_interface { int (*fetch_sensors)(struct esp_foc_motor_interface *self); }; -int esp_foc_motor_reset(struct esp_foc_motor_interface *self) +static inline int esp_foc_motor_reset(struct esp_foc_motor_interface *self) { if(!self) return -EINVAL; @@ -51,7 +52,7 @@ int esp_foc_motor_reset(struct esp_foc_motor_interface *self) } -int esp_foc_motor_enable(struct esp_foc_motor_interface *self) +static inline int esp_foc_motor_enable(struct esp_foc_motor_interface *self) { if(!self) return -EINVAL; @@ -62,7 +63,18 @@ int esp_foc_motor_enable(struct esp_foc_motor_interface *self) return self->enable(self); } -int esp_foc_motor_set_duty_cycles(struct esp_foc_motor_interface *self, float a, float b, float c) +static inline int esp_foc_motor_disable(struct esp_foc_motor_interface *self) +{ + if(!self) + return -EINVAL; + + if(!self->disable) + return -ENOTSUP; + + return self->disable(self); +} + +static inline int esp_foc_motor_set_duty_cycles(struct esp_foc_motor_interface *self, float a, float b, float c) { if(!self) return -EINVAL; @@ -73,7 +85,7 @@ int esp_foc_motor_set_duty_cycles(struct esp_foc_motor_interface *self, float a, return self->set_duty_cycles(Self, a, b, c); } -int esp_foc_motor_get_rotor_angle(struct esp_foc_motor_interface *self, float *angle) +static inline int esp_foc_motor_get_rotor_angle(struct esp_foc_motor_interface *self, float *angle) { if(!self) return -EINVAL; @@ -84,7 +96,7 @@ int esp_foc_motor_get_rotor_angle(struct esp_foc_motor_interface *self, float *a return self->get_rotor_angle(self, angle); } -int esp_foc_motor_get_rotor_speed_dps(struct esp_foc_motor_interface *self, float *speed_dps) +static inline int esp_foc_motor_get_rotor_speed_dps(struct esp_foc_motor_interface *self, float *speed_dps) { if(!self) return -EINVAL; @@ -95,7 +107,7 @@ int esp_foc_motor_get_rotor_speed_dps(struct esp_foc_motor_interface *self, floa return self->get_rotor_speed_dps(self, speed_dps); } -int esp_foc_motor_align_rotor(struct esp_foc_motor_interface *self) +static inline int esp_foc_motor_align_rotor(struct esp_foc_motor_interface *self) { if(!self) return -EINVAL; @@ -106,7 +118,7 @@ int esp_foc_motor_align_rotor(struct esp_foc_motor_interface *self) return self->align_rotor(self); } -int esp_foc_motor_get_acumulated_angle(struct esp_foc_motor_interface *self, float *angle) +static inline int esp_foc_motor_get_acumulated_angle(struct esp_foc_motor_interface *self, float *angle) { if(!self) return -EINVAL; @@ -117,7 +129,7 @@ int esp_foc_motor_get_acumulated_angle(struct esp_foc_motor_interface *self, flo return self->get_acumulated_angle(self, angle); } -int esp_foc_motor_get_encoder_ppr(struct esp_foc_motor_interface *self, float *ppr) +static inline int esp_foc_motor_get_encoder_ppr(struct esp_foc_motor_interface *self, float *ppr) { if(!self) return -EINVAL; @@ -128,7 +140,7 @@ int esp_foc_motor_get_encoder_ppr(struct esp_foc_motor_interface *self, float *p return self->get_encoder_ppr(self, ppr); } -int esp_foc_motor_get_dc_bus_voltage(struct esp_foc_motor_interface *self, float *vdc) +static inline int esp_foc_motor_get_dc_bus_voltage(struct esp_foc_motor_interface *self, float *vdc) { if(!self) return -EINVAL; @@ -139,7 +151,7 @@ int esp_foc_motor_get_dc_bus_voltage(struct esp_foc_motor_interface *self, float return self->get_dc_bus_voltage(self, vdc); } -int esp_foc_motor_get_currents(struct esp_foc_motor_interface *self, float *ia, float *ib, float *ic) +static inline int esp_foc_motor_get_currents(struct esp_foc_motor_interface *self, float *ia, float *ib, float *ic) { if(!self) return -EINVAL; @@ -150,7 +162,7 @@ int esp_foc_motor_get_currents(struct esp_foc_motor_interface *self, float *ia, return self->get_currents(self, ia, ib, ic); } -int esp_foc_motor_number_of_shunts(struct esp_foc_motor_interface *self, int *shunts) +static inline int esp_foc_motor_number_of_shunts(struct esp_foc_motor_interface *self, int *shunts) { if(!self) return -EINVAL; @@ -161,7 +173,7 @@ int esp_foc_motor_number_of_shunts(struct esp_foc_motor_interface *self, int *sh return self->number_of_shunts(self, shunts); } -int esp_foc_motor_fetch_sensors(struct esp_foc_motor_interface *self) +static inline int esp_foc_motor_fetch_sensors(struct esp_foc_motor_interface *self) { if(!self) return -EINVAL; @@ -170,4 +182,4 @@ int esp_foc_motor_fetch_sensors(struct esp_foc_motor_interface *self) return -ENOTSUP; return self->fetch_sensors(self); -} \ No newline at end of file +} diff --git a/include/espFoC/esp_foc_pid.h b/common/include/espFoC/motor_control/esp_foc_pid.h similarity index 100% rename from include/espFoC/esp_foc_pid.h rename to common/include/espFoC/motor_control/esp_foc_pid.h diff --git a/include/espFoC/esp_foc_svm.h b/common/include/espFoC/motor_control/esp_foc_svm.h similarity index 100% rename from include/espFoC/esp_foc_svm.h rename to common/include/espFoC/motor_control/esp_foc_svm.h diff --git a/common/ipc/esp_foc_ipc.c b/common/ipc/esp_foc_ipc.c new file mode 100644 index 0000000..c0298b2 --- /dev/null +++ b/common/ipc/esp_foc_ipc.c @@ -0,0 +1,83 @@ +/* + * MIT License + * + * Copyright (c) 2024 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. + */ + +#include +#include +#include + +#define DIRECTION_TX 0x01 +#define DIRECTION_RX 0x81 + +static const struct device *ipc_dev = DEVICE_DT_GET(espfoc_ipc_dev); +static uint8_t *ipc_shm = DT_REG_ADDR(DT_NODELABEL(espfoc_shm)); +const static uint32_t ipc_shm_size = DT_REG_SIZE(DT_NODELABEL(espfoc_shm)); + +static struct motor_command *tx_motor_cmd; +static struct motor_command *rx_motor_cmd; +static struct motor_state *tx_motor_state; +static struct motor_state *rx_motor_state; + +BUILD_ASSERT(ipc_shm_size >= (2 * sizeof(motor_command)) + + (2 * sizeof(motor_state)), "Not enough shared memory!"); + +#define TX_CMD_OFFSET 0 +#define RX_CMD_OFFSET (TX_CMD_OFFSET + sizeof(struct motor_command)) +#define TX_STATE_OFFSET (RX_CMD_OFFSET + sizeof(struct motor_command)) +#define RX_STATE_OFFSET (TX_STATE_OFFSET + sizeof(struct motor_state)) + +static sys_dlist_t ipc_callbacks; + +static void ipc_dev_callback(const struct device *dev, void *context, + uint32_t id, volatile void *data) +{ + +} + +int esp_foc_ipc_init(void) +{ + tx_motor_cmd = (struct motor_command *)&ipc_shm[TX_CMD_OFFSET]; + rx_motor_cmd = (struct motor_command *)&ipc_shm[RX_CMD_OFFSET]; + tx_motor_state = (struct motor_state *)&ipc_shm[TX_STATE_OFFSET]; + rx_motor_state = (struct motor_state *)&ipc_shm[RX_STATE_OFFSET]; + + sys_dlist_init(&ipc_callbacks); + ipm_register_callback(ipc_dev, ipc_dev_callback, NULL); + ipm_set_enabled(ipc_dev, 1); + + return 0; +} +int esp_foc_ipc_send_command(const struct motor_command *cmd) +{ + return 0; +} + +int esp_foc_ipc_send_state(const struct motor_state *state) +{ + return 0; +} + +int esp_foc_ipc_register_callback(const struct motor_report_callback *cb) +{ + return 0; +} diff --git a/src/esp_foc.c b/common/motor_control/esp_foc.c similarity index 85% rename from src/esp_foc.c rename to common/motor_control/esp_foc.c index dc33bfb..2dc9c3c 100644 --- a/src/esp_foc.c +++ b/common/motor_control/esp_foc.c @@ -23,7 +23,7 @@ */ #include -#include +#include extern const float ESP_FOC_M_PI; extern const float ESP_FOC_2_M_PI; @@ -78,11 +78,13 @@ int esp_foc_init_controller(struct esp_foc_motor_control *ctl, ctl->target_position = 0.0f; ctl->motor_hw = hw; - if(esp_foc_motor_enable(ctl->motor_hw)) { + if(esp_foc_motor_disable(ctl->motor_hw)) { return -ENODEV; } - return esp_foc_motor_reset(ctl->motor_hw); + ctl->is_enabled = false; + + return 0; } int esp_foc_controller_set_speed(struct esp_foc_motor_control *ctl, @@ -130,6 +132,46 @@ int esp_foc_add_speed_control(struct esp_foc_motor_control *ctl, return 0; } +int esp_foc_enable_axis(struct esp_foc_motor_control *ctl) +{ + if(!ctl) + return -EINVAL; + + if(ctl->is_enabled) + return EALREADY; + + int r = esp_foc_motor_enable(ctl->hw) + if(r) + return r; + + r = esp_foc_motor_reset(ctl->motor_hw); + if(r) + return r; + + ctl->is_enabled = true; + + return 0; +} + +int esp_foc_disable_axis(struct esp_foc_motor_control *ctl) +{ + if(!ctl) + return -EINVAL; + + if(!ctl->is_enabled) + return EALREADY; + + int r = esp_foc_motor_disable(ctl->hw) + if(r) + return r; + + ctl->target_speed = 0.0f; + ctl->target_position = 0.0f; + ctl->is_enabled = false; + + return 0; +} + int esp_foc_controller_run(struct esp_foc_motor_control *ctl) { float vd = 0.0f; @@ -141,6 +183,9 @@ int esp_foc_controller_run(struct esp_foc_motor_control *ctl) if(!ctl) return -EINVAL; + if(!ctl->is_enabled) + return -EIO; + esp_foc_motor_fetch_sensors(ctl->hw); if(ctl->pid_position) { diff --git a/src/esp_foc_consts.c b/common/motor_control/esp_foc_consts.c similarity index 100% rename from src/esp_foc_consts.c rename to common/motor_control/esp_foc_consts.c diff --git a/src/esp_foc_our_sin_cos.c b/common/motor_control/esp_foc_our_sin_cos.c similarity index 100% rename from src/esp_foc_our_sin_cos.c rename to common/motor_control/esp_foc_our_sin_cos.c diff --git a/src/esp_foc_pid.c b/common/motor_control/esp_foc_pid.c similarity index 98% rename from src/esp_foc_pid.c rename to common/motor_control/esp_foc_pid.c index 83085e6..f179afc 100644 --- a/src/esp_foc_pid.c +++ b/common/motor_control/esp_foc_pid.c @@ -22,7 +22,7 @@ * SOFTWARE. */ -#include +#include int esp_foc_pid_init(struct esp_foc_pid *p, float sample_time_seconds, float integral_limit) { diff --git a/src/esp_foc_svm.c b/common/motor_control/esp_foc_svm.c similarity index 98% rename from src/esp_foc_svm.c rename to common/motor_control/esp_foc_svm.c index 47861b7..39c9766 100644 --- a/src/esp_foc_svm.c +++ b/common/motor_control/esp_foc_svm.c @@ -2,7 +2,7 @@ * Copyright (c) 2021 Teslabs Engineering S.L. * SPDX-License-Identifier: Apache-2.0 */ -#include +#include extern const float ESP_FOC_SQRT3; extern const float ESP_FOC_1_SQRT3; diff --git a/doc/images/arch.png b/doc/images/arch.png new file mode 100644 index 0000000..9ddd309 Binary files /dev/null and b/doc/images/arch.png differ diff --git a/examples/stubs/main.cpp b/examples/stubs/main.cpp deleted file mode 100644 index 0a7a080..0000000 --- a/examples/stubs/main.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include -#include -#include -#include - -struct motor_hardware_stub stub; -struct esp_foc_motor_interface *hw; -struct esp_foc_motor_control control; -int should_stop = 0; - -using namespace std; - -int main(int argc, char **argv) -{ - int err; - - err = motor_hardware_stub_init(&stub, &hw); - if(err) - cout << "failed to initialize the motor hardware err:" << err << endl; - - err = esp_foc_init_controller(&control, hw); - if(err) - cout << "failed to initialize esp foc controller err:" << err << endl; - - err = esp_foc_controller_set_speed(&control, 360.0f); - if(err) - cout << "failed to set the initial speed err" << err << endl; - - while(!should_stop) { - err = esp_foc_controller_run(&ctl); - if(err) - cout << "error when running controller err:" << err << endl; - usleep(1000); - } -} diff --git a/examples/stubs/motor_hardware_stub.c b/examples/stubs/motor_hardware_stub.c deleted file mode 100644 index 44a46bc..0000000 --- a/examples/stubs/motor_hardware_stub.c +++ /dev/null @@ -1,133 +0,0 @@ -/* - * 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. - */ -#include -#include "motor_hardware_stub.h" - -static const float pole_pairs = 12.0f; -static float erotor_angle = 0.0f; -static const float angle_step_per_ms = 0.36f; - -static int normalize_angle(float *angle) -{ - if(!angle) - return -EINVAL; - - float result = fmod(*angle, (2.0 * 3.141569f)); - if(result > 3.141569f) { - result -= 3.141569f; - } - - *angle = result; - - return 0; -} - -static int reset(struct esp_foc_motor_interface *self) -{ - if(!self) - return -EINVAL; - - return 0; -} - -static int enable(struct esp_foc_motor_interface *self) -{ - if(!self) - return -EINVAL; - - return 0; -} - -static int set_duty_cycles(struct esp_foc_motor_interface *self, float a, float b, float c) -{ - if(!self) - return -EINVAL; - - (void)a; - (void)b; - (void)c; - - return 0; -} - -static int get_rotor_angle(struct esp_foc_motor_interface *self, float *angle) -{ - if(!self || !angle) - return -EINVAL; - - *angle = erotor_angle * pole_pairs; - - return normalize_angle(angle); -} - -static int get_rotor_speed_dps(struct esp_foc_motor_interface *self, float *speed_dps) -{ - if(!self || speed_dps) - return -EINVAL; - - - //360 dps -> 1 RPS -> 1 RPM; - *speed_dps = 360.0f; - - return 0; -} - -static int align_rotor(struct esp_foc_motor_interface *self) -{ - if(!self) - return -EINVAL; - - return 0; -} - -static int fetch_sensors(struct esp_foc_motor_interface *self) -{ - if(!self) - return -EINVAL; - - erotor_angle += angle_step_per_ms; - if(e_angle > 360.0f) { - erotor_angle -= 360.0f; - } - - return 0; -} - -int motor_hardware_stub_init(struct motor_hardware_stub *stub, - struct esp_foc_motor_interface **itf) -{ - if(!stub) - return -EINVAL; - - stub->self->reset = reset; - stub->self->enable = enable; - stub->self->set_duty_cycles = set_duty_cycles; - stub->self->get_rotor_angle = get_rotor_angle; - stub->self->get_rotor_speed_dps = get_rotor_speed_dps; - stub->self->align_rotor = align_rotor; - stub->self->fetch_sensors = fetch_sensors; - - itf = stub->self; - return 0; -} \ No newline at end of file