arch: refactor espFoC on top of the Zephyr RTOS

This refactor also changes the espFoC to be a motor control application for esp32 SoCs instead of a library. Signed-off-by: Felipe Neves <[email protected]>
uLipe · Nov 4, 2024 · 8c73c39 · 8c73c39
1 parent b602c07
commit 8c73c39
Show file tree

Hide file tree

Showing 28 changed files with 1,420 additions and 232 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
diff --git a/README.md b/README.md
@@ -3,27 +3,43 @@
 ![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!);
-* Uses openAMP to offload motor control tasks to one of the core, and leave the other for communication;
-* support UART and CAN communication;
+* Easy to wire motor using common drivers and I2C encoders out there!
+* Uses ESP32 AMP solution for Zephyr RTOS to split execution;
+* App CPU runs the motor control firmware;
+* Pro CPU runs communication and exposes a thread for user application;
+* Interaction between the firmwares are doing by a custom IPC protocol;
+* Planned support for UART and CAN communication;
 
 ## Limitations:
 
-* Support for esp32 and esp32s3 only;
-* Requires and rotor position sensor, for example, incremental encoder.
+* Support only for espressif SoC that are dual-core and have FPU;
+* Once sensored support, requires an I2C magnetic encoder sensor.
 
 ## Getting started:
 
-* Just clone this project on most convenient folder;
+* Just clone this project on most convenient folder then:
+
+```
+ $ west build -palways -besp32s3_devkitm/esp32s3/procpu /path/to/espFoC
+```
+
+* To flash:
+```
+ $ west flash
+```
 
 ## Typical wiring:
 

diff --git a/app/CMakeLists.txt b/app/CMakeLists.txt
@@ -0,0 +1,41 @@
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.20.0)
+
+set(REMOTE_ZEPHYR_DIR ${CMAKE_CURRENT_BINARY_DIR}/espfoc_remote-prefix/src/espfoc_remote-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)
+
+FILE(GLOB ipc ${MOTOR_CONTROL_SRC}/ipc/*.c)
+
+set_source_files_properties(${REMOTE_ZEPHYR_DIR}/zephyr.c PROPERTIES GENERATED TRUE)
+target_sources(app PRIVATE espfoc_app_core/esp_foc_app_shell.c
+                           espfoc_app_core/esp_foc_main.c
+                           espfoc_app_core/esp_foc_user.c
+                           ${ipc}
+                           ${REMOTE_ZEPHYR_DIR}/zephyr.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_procpu.overlay b/app/boards/esp32s3_devkitm_procpu.overlay
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2023 Felipe Neves <[email protected]>
+ *
+ * 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/app/espfoc_app_core/esp_foc_app_shell.c b/app/espfoc_app_core/esp_foc_app_shell.c
@@ -0,0 +1,147 @@
+/*
+ * 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 <stdlib.h>
+#include <zephyr/kernel.h>
+#include <zephyr/device.h>
+#include <zephyr/shell/shell.h>
+#include <espFoC/ipc/esp_foc_ipc.h>
+
+static int cmd_set_position(const struct shell *shell, size_t argc, char **argv)
+{
+    struct motor_command command;
+
+	if (argc != 2) {
+		return -EINVAL;
+	}
+
+	command.position_mdeg = (int32_t)(strtof(argv[1], NULL) * 1000.0f);
+	command.command_mask = MOTOR_CMD_POSITION_MASK;
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+static int cmd_set_speed(const struct shell *shell, size_t argc, char **argv)
+{
+	struct motor_command command;
+
+	if (argc != 2) {
+		return -EINVAL;
+	}
+
+	command.speed_mdps = (int32_t)(strtof(argv[1], NULL) * 1000.0f);
+	command.command_mask = MOTOR_CMD_SPEED_MASK;
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+static int cmd_enable(const struct shell *shell, size_t argc, char **argv)
+{
+	struct motor_command command;
+
+	if (argc != 1) {
+		return -EINVAL;
+	}
+
+	command.command_mask = MOTOR_CMD_SHUTDOWN_MASK;
+	command.power_state = 1;
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+static int cmd_disable(const struct shell *shell, size_t argc, char **argv)
+{
+	struct motor_command command;
+
+	if (argc != 1) {
+		return -EINVAL;
+	}
+
+	command.command_mask = MOTOR_CMD_SHUTDOWN_MASK;
+	command.power_state = 1;
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+static int cmd_set_gains_pid_vel(const struct shell *shell, size_t argc, char **argv)
+{
+    struct motor_command command;
+
+	if (argc != 4) {
+		return -EINVAL;
+	}
+
+	command.command_mask = MOTOR_CMD_SPD_PID_MASK;
+	command.pid_gains[0] = (int32_t)(strtof(argv[1], NULL) * 1e+6f);
+	command.pid_gains[1] = (int32_t)(strtof(argv[2], NULL) * 1e+6f);
+	command.pid_gains[2] = (int32_t)(strtof(argv[3], NULL) * 1e+6f);
+
+	return esp_foc_ipc_send_command(&command);
+
+}
+
+static int cmd_set_gains_pid_pos(const struct shell *shell, size_t argc, char **argv)
+{
+    struct motor_command command;
+
+	if (argc != 4) {
+		return -EINVAL;
+	}
+
+	command.command_mask = MOTOR_CMD_POS_PID_MASK;
+	command.pid_gains[0] = (int32_t)(strtof(argv[1], NULL) * 1e+6f);
+	command.pid_gains[1] = (int32_t)(strtof(argv[2], NULL) * 1e+6f);
+	command.pid_gains[2] = (int32_t)(strtof(argv[3], NULL) * 1e+6f);
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+static int cmd_set_maintenance_pwm(const struct shell *shell, size_t argc, char **argv)
+{
+    struct motor_command command;
+
+	if (argc != 4) {
+		return -EINVAL;
+	}
+
+	command.command_mask = (MOTOR_CMD_MAITENANCE_MODE | MOTOR_CMD_MAITENANCE_PWMS);
+	command.pwms[0] = (int32_t)(strtof(argv[1], NULL) * 1e+6f);
+	command.pwms[1] = (int32_t)(strtof(argv[2], NULL) * 1e+6f);
+	command.pwms[2] = (int32_t)(strtof(argv[3], NULL) * 1e+6f);
+
+	return esp_foc_ipc_send_command(&command);
+}
+
+SHELL_STATIC_SUBCMD_SET_CREATE(
+	esp_foc,
+	SHELL_CMD(set_position, NULL, "sets position in degrees", cmd_set_position),
+	SHELL_CMD(set_speed, NULL, "sets speed in degrees per second", cmd_set_speed),
+	SHELL_CMD(enable, NULL, "enable motor driver", cmd_enable),
+	SHELL_CMD(disable, NULL, "disable motor driver", cmd_disable),
+	SHELL_CMD(set_gains_pid_vel, NULL, "set pids velocity", cmd_set_gains_pid_vel),
+	SHELL_CMD(set_gains_pid_pos, NULL, "set pids speed", cmd_set_gains_pid_pos),
+	SHELL_CMD(set_maintenance_pwm, NULL, "set pwms maitenance mode", cmd_set_maintenance_pwm),
+	SHELL_SUBCMD_SET_END
+	);
+
+SHELL_CMD_REGISTER(esp_foc, &esp_foc, "espFoC Basic shell commands", NULL);
diff --git a/app/espfoc_app_core/esp_foc_main.c b/app/espfoc_app_core/esp_foc_main.c
@@ -0,0 +1,57 @@
+/*
+ * 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 <stdio.h>
+#include <zephyr/kernel.h>
+#include <espFoC/ipc/esp_foc_ipc.h>
+
+static void esp_foc_motor_on_state (const struct motor_state *cmd);
+ESP_FOC_DEFINE_IPC_CALLBACK(state_callback, NULL, esp_foc_motor_on_state);
+
+static void esp_foc_motor_on_state (const struct motor_state *cmd)
+{
+    printk("espFoC Motor core sent state \n");
+}
+
+int esp_foc_early_init(void)
+{
+    esp_foc_ipc_init();
+    esp_foc_ipc_register_callback(&state_callback);
+    return 0;
+}
+SYS_INIT(esp_foc_early_init, POST_KERNEL, CONFIG_APPLICATION_INIT_PRIORITY);
+
+int main (void)
+{
+    /**TODO use the main application to implement espfoc commander
+     * Through UART, SPI, I2C and CAN, last one is the priority
+     */
+    printk("espFoC is running in the %s\n", CONFIG_BOARD);
+}
+
+__weak void esp_foc_user_entry(void)
+{
+    printk("Weak user entry of the espFoC \n");
+}
+K_THREAD_DEFINE(foc_tid, 4096, esp_foc_user_entry, NULL, NULL, NULL, 1, 0, 0);
diff --git a/app/espfoc_app_core/esp_foc_user.c b/app/espfoc_app_core/esp_foc_user.c
@@ -0,0 +1,35 @@
+/*
+ * 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 <stdio.h>
+#include <zephyr/kernel.h>
+#include <espFoC/ipc/esp_foc_ipc.h>
+
+void esp_foc_user_entry(void)
+{
+    /* Add your user code here! */
+    /* Use esp foc ipc to interact with the motor control firmware
+     * That runs on the other core.
+     */
+}
diff --git a/app/espfoc_motor_core/CMakeLists.txt 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)
+
+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})