foc: fix the bias on the sine pwm modulation

and introduce the space vector PWM modulation.

Signed-off-by: Felipe Neves <[email protected]>

Author: uLipe

CMakeLists.txt

@@ -1,7 +1,7 @@
 idf_build_get_property(target IDF_TARGET)
 
 set(requires driver esp_adc freertos esp_system esp_timer)
-set(srcs "esp_foc.c" "esp_foc_scope.c" "esp_foc_consts.c" "esp_foc_sine_cosine.c")
+set(srcs "esp_foc.c" "esp_foc_scope.c" "esp_foc_consts.c" "esp_foc_sine_cosine.c" "esp_foc_sqrtf.c")
 set(includes "include" "drivers")
 
 list(APPEND srcs   "drivers/inverter_3pwm_ledc.c"

Kconfig

@@ -39,6 +39,14 @@ menu "espFoC Settings"
         int "Size of scope buffer in frames"
         default 1536
 
+    config ESP_FOC_USE_SINE_PWM
+        bool "Use Sinewave modulation for PWM, if not it will use the SVPWM"
+        default y
+
+    config ESP_FOC_COMP_THI
+        bool "Enable third harmonic injection compensation on SVPWM"
+        depends on !ESP_FOC_USE_SINE_PWM
+        default n
 endmenu
 
 

drivers/inverter_3pwm_ledc.c

@@ -102,20 +102,20 @@ IRAM_ATTR static void set_voltages(esp_foc_inverter_t *self,
     esp_foc_ledc_inverter *obj =
         __containerof(self, esp_foc_ledc_inverter, interface);
 
-    if(v_u > obj->dc_link_voltage) {
-        v_u = obj->dc_link_voltage;
+    if(v_u > 1.0f) {
+        v_u = 1.0f;
     } else if (v_u < 0.0f) {
         v_u = 0.0f;
     }
 
-    if(v_v > obj->dc_link_voltage) {
-        v_v = obj->dc_link_voltage;
+    if(v_v > 1.0f) {
+        v_v = 1.0f;
     } else if (v_v < 0.0f) {
         v_v = 0.0f;
     }
 
-    if(v_w > obj->dc_link_voltage) {
-        v_w = obj->dc_link_voltage;
+    if(v_w > 1.0f) {
+        v_w = 1.0f;
     } else if (v_w < 0.0f) {
         v_w = 0.0f;
     }
@@ -266,7 +266,8 @@ esp_foc_inverter_t *inverter_3pwm_ledc_new(ledc_channel_t ch_u,
     }
 
     ledc[port].hw = LEDC_LL_GET_HW();
-    ledc[port].voltage_to_duty_ratio = LEDC_RESOLUTION_STEPS / ledc[port].dc_link_voltage;
+
+    ledc[port].voltage_to_duty_ratio = LEDC_RESOLUTION_STEPS;
 
     return &ledc[port].interface;
 }

drivers/rotor_sensor_as5600.c

@@ -12,7 +12,7 @@
  *
  * 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
@@ -21,7 +21,7 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
  */
- 
+
 #include <math.h>
 #include "espFoC/rotor_sensor_as5600.h"
 #include "driver/gpio.h"
@@ -33,7 +33,7 @@
 #define AS5600_SLAVE_ADDR 0x36
 #define AS5600_ANGLE_REGISTER_H 0x0E
 #define AS5600_PULSES_PER_REVOLUTION 4096.0f
-#define AS5600_READING_MASK 0xFFF 
+#define AS5600_READING_MASK 0xFFF
 
 static const char *tag = "ROTOR_SENSOR_AS5600";
 
@@ -50,7 +50,7 @@ static bool i2c_bus_configured = false;
 DRAM_ATTR static esp_foc_as5600_t rotor_sensors[CONFIG_NOOF_AXIS];
 static const float encoder_wrap_value = AS5600_PULSES_PER_REVOLUTION * 0.95f;
 
-IRAM_ATTR static uint16_t read_angle_sensor(int i2c_port) 
+IRAM_ATTR static uint16_t read_angle_sensor(int i2c_port)
 {
     uint8_t write_buffer = AS5600_ANGLE_REGISTER_H;
     uint8_t read_buffer[2] = {0,0};
@@ -65,7 +65,7 @@ IRAM_ATTR static uint16_t read_angle_sensor(int i2c_port)
                                         read_buffer,
                                         2,
                                         portMAX_DELAY);
- 
+
     } while (err != ESP_OK);
 
     raw = read_buffer[0];
@@ -114,7 +114,7 @@ IRAM_ATTR static float read_counts(esp_foc_rotor_sensor_t *self)
     float delta = (float)raw - obj->previous;
 
     if(fabs(delta) >= encoder_wrap_value) {
-        obj->accumulated = (delta < 0.0f) ? 
+        obj->accumulated = (delta < 0.0f) ?
             obj->accumulated + AS5600_PULSES_PER_REVOLUTION :
                 obj->accumulated - AS5600_PULSES_PER_REVOLUTION;
     }

esp_foc.c

@@ -115,18 +115,6 @@ static inline void esp_foc_torque_control_loop(esp_foc_axis_t *axis)
                                         esp_foc_low_pass_filter_update(
                                             &axis->current_filters[1], axis->i_d.raw)) +
                                             axis->target_u_d.raw;
-
-    if(axis->u_q.raw > (axis->dc_link_voltage * 0.4)) {
-        axis->u_q.raw = (axis->dc_link_voltage * 0.4);
-    } else if (axis->u_q.raw < -(axis->dc_link_voltage * 0.4)){
-        axis->u_q.raw = -(axis->dc_link_voltage * 0.4);
-    }
-
-    if(axis->u_d.raw > (axis->dc_link_voltage * 0.4)) {
-        axis->u_d.raw = (axis->dc_link_voltage * 0.4);
-    } else if (axis->u_d.raw < -(axis->dc_link_voltage * 0.4)){
-        axis->u_d.raw = -(axis->dc_link_voltage * 0.4);
-    }
 }
 
 IRAM_ATTR static void esp_foc_control_loop(void *arg)
@@ -142,7 +130,9 @@ IRAM_ATTR static void esp_foc_control_loop(void *arg)
                     axis->u_q.raw,
                     &axis->u_u.raw,
                     &axis->u_v.raw,
-                    &axis->u_w.raw);
+                    &axis->u_w.raw,
+                    axis->biased_dc_link_voltage,
+                    axis->dc_link_to_normalized);
 
     axis->inverter_driver->set_voltages(axis->inverter_driver,
                                         axis->u_u.raw,
@@ -211,14 +201,30 @@ esp_foc_err_t esp_foc_initialize_axis(esp_foc_axis_t *axis,
         return ESP_FOC_ERR_INVALID_ARG;
     }
 
+#ifdef CONFIG_ESP_FOC_CUSTOM_MATH
+    extern void esp_foc_fast_init_sqrt_table(void);
+    esp_foc_fast_init_sqrt_table();
+#endif
+
     axis->inverter_driver = inverter;
     axis->rotor_sensor_driver = rotor;
     axis->isensor_driver = isensor;
 
     axis->dc_link_voltage =
         axis->inverter_driver->get_dc_link_voltage(axis->inverter_driver);
+
+#ifdef CONFIG_ESP_FOC_USE_SINE_PWM
+    axis->biased_dc_link_voltage = axis->dc_link_voltage / 4.0f;
+    axis->dc_link_to_normalized = 1.0f / axis->dc_link_voltage;
+#else
+    axis->biased_dc_link_voltage = (axis->dc_link_voltage * ESP_FOC_SQRT3_TWO) / 2.0f;
+    axis->dc_link_to_normalized = 1.0f / (axis->dc_link_voltage * ESP_FOC_SQRT3_TWO);
+#endif
     axis->inverter_driver->set_voltages(axis->inverter_driver, 0.0, 0.0, 0.0);
+
     ESP_LOGI(tag,"inverter dc-link voltage: %f[V]", axis->dc_link_voltage);
+    ESP_LOGI(tag, "FoC max voltage: %f",  axis->biased_dc_link_voltage);
+    ESP_LOGI(tag, "FoC normalizer scale: %f",  axis->dc_link_to_normalized);
 
     axis->dt = 0.0;
     axis->last_timestamp = 0.0;
@@ -341,10 +347,10 @@ esp_foc_err_t esp_foc_align_axis(esp_foc_axis_t *axis)
                                         0.0f);
 
     axis->inverter_driver->enable(axis->inverter_driver);
-    esp_foc_sleep_ms(2000);
+    esp_foc_sleep_ms(100);
 
     axis->inverter_driver->set_voltages(axis->inverter_driver,
-                                        0.1 * (axis->dc_link_voltage/2.0f),
+                                        0.1,
                                         0.0f,
                                         0.0f);
 
@@ -379,16 +385,16 @@ IRAM_ATTR esp_foc_err_t esp_foc_set_target_voltage(esp_foc_axis_t *axis,
         return ESP_FOC_ERR_AXIS_INVALID_STATE;
     }
 
-    if(uq.raw > (axis->dc_link_voltage * 0.4)) {
-        uq.raw = (axis->dc_link_voltage * 0.4);
-    } else if (uq.raw < -(axis->dc_link_voltage * 0.4)){
-        uq.raw = -(axis->dc_link_voltage * 0.4);
+    if(uq.raw > (axis->biased_dc_link_voltage)) {
+        uq.raw = (axis->biased_dc_link_voltage);
+    } else if (uq.raw < -(axis->biased_dc_link_voltage)){
+        uq.raw = -(axis->biased_dc_link_voltage);
     }
 
-    if(ud.raw > (axis->dc_link_voltage * 0.4)) {
-        ud.raw = (axis->dc_link_voltage * 0.4);
-    } else if (ud.raw < -(axis->dc_link_voltage * 0.4)){
-        ud.raw = -(axis->dc_link_voltage * 0.4);
+    if(ud.raw > (axis->biased_dc_link_voltage)) {
+        ud.raw = (axis->biased_dc_link_voltage);
+    } else if (ud.raw < -(axis->biased_dc_link_voltage)){
+        ud.raw = -(axis->biased_dc_link_voltage);
     }
 
     esp_foc_critical_enter();
@@ -464,6 +470,8 @@ esp_foc_err_t esp_foc_test_motor(esp_foc_inverter_t *inverter,
                                 esp_foc_rotor_sensor_t *rotor,
                                 esp_foc_motor_control_settings_t settings)
 {
+    float norm, bias;
+
     if(inverter == NULL) {
         ESP_LOGE(tag, "invalid inverter driver!");
         return ESP_FOC_ERR_INVALID_ARG;
@@ -488,6 +496,14 @@ esp_foc_err_t esp_foc_test_motor(esp_foc_inverter_t *inverter,
                             0.0f);
     esp_foc_sleep_ms(250);
 
+#ifdef CONFIG_ESP_FOC_USE_SINE_PWM
+    bias = inverter->get_dc_link_voltage(inverter) / 2.0f;
+    norm = 1.0f / inverter->get_dc_link_voltage(inverter);
+#else
+    bias = (inverter->get_dc_link_voltage(inverter) * ESP_FOC_SQRT3_TWO) / 2.0f;
+    norm = 1.0f / (inverter->get_dc_link_voltage(inverter) * ESP_FOC_SQRT3_TWO);
+#endif
+
     /* Turn motor in one direction */
     for(float i = 0.0f; i < 2 * M_PI * settings.motor_pole_pairs; i += 0.05) {
         esp_foc_u_voltage u;
@@ -503,7 +519,9 @@ esp_foc_err_t esp_foc_test_motor(esp_foc_inverter_t *inverter,
                 0.2f,
                 &u.raw,
                 &v.raw,
-                &w.raw);
+                &w.raw,
+                bias,
+                norm);
 
         inverter->set_voltages(inverter,
                                 u.raw,
@@ -528,7 +546,9 @@ esp_foc_err_t esp_foc_test_motor(esp_foc_inverter_t *inverter,
                 0.2f,
                 &u.raw,
                 &v.raw,
-                &w.raw);
+                &w.raw,
+                bias,
+                norm);
 
         inverter->set_voltages(inverter,
                                 u.raw,

esp_foc_consts.c

@@ -12,7 +12,7 @@
  *
  * 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
@@ -21,7 +21,7 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
  */
- 
+
 #include <math.h>
 
 #ifdef CONFIG_ESP_FOC_CUSTOM_MATH
@@ -30,10 +30,11 @@ const float ESP_FOC_FAST_2PI = ESP_FOC_FAST_PI * 2.0f;
 const float ESP_FOC_SIN_COS_APPROX_B = 4.0f / ESP_FOC_FAST_PI;
 const float ESP_FOC_SIN_COS_APPROX_C = -4.0f / (ESP_FOC_FAST_PI * ESP_FOC_FAST_PI);
 const float ESP_FOC_SIN_COS_APPROX_P = 0.225f;
-const float ESP_FOC_SIN_COS_APPROX_D = ESP_FOC_FAST_PI/2.0f; 
+const float ESP_FOC_SIN_COS_APPROX_D = ESP_FOC_FAST_PI/2.0f;
 #endif
 
 const float ESP_FOC_CLARKE_K1 = 2.0/3.0f;
 const float ESP_FOC_CLARKE_K2 = 1.0/3.0f;
 const float ESP_FOC_CLARKE_PARK_SQRT3 = 1.73205080757f;
 const float ESP_FOC_CLARKE_K3 = 2.0f / ESP_FOC_CLARKE_PARK_SQRT3;
+const float ESP_FOC_SQRT3_TWO = ESP_FOC_CLARKE_PARK_SQRT3 / 2.0f;

esp_foc_sine_cosine.c

@@ -24,6 +24,7 @@
 
 #include <math.h>
 #include <stdint.h>
+#include "esp_attr.h"
 
 static const float sine_table[4096] = {
       0.000000f,   0.001534f,   0.003068f,   0.004602f,   0.006136f,   0.007670f,   0.009204f,   0.010738f,
@@ -1058,14 +1059,14 @@ static const float cosine_table[4096] = {
 const static float full2pi = M_PI * 2.0f;
 const static float index_scale = (4096.0f / full2pi);
 
-float esp_foc_fast_sine(float angle)
+IRAM_ATTR float esp_foc_fast_sine(float angle)
 {
     uint16_t index = (uint16_t)(angle * index_scale);
     index &= (4096 - 1);
     return sine_table[index];
 }
 
-float esp_foc_fast_cosine(float angle)
+IRAM_ATTR float esp_foc_fast_cosine(float angle)
 {
     uint16_t index = (uint16_t)(angle * index_scale);
     index &= (4096 - 1);

esp_foc_sqrtf.c

@@ -0,0 +1,79 @@
+/*
+ * 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 <stdint.h>
+#include <math.h>
+#include "esp_attr.h"
+
+#define SQRT_TABLE_BITS 12
+#define SQRT_TABLE_SIZE (1 << SQRT_TABLE_BITS)
+
+static float sqrt_table[SQRT_TABLE_SIZE];
+
+void esp_foc_fast_init_sqrt_table(void)
+{
+    for (int i = 0; i < SQRT_TABLE_SIZE; i++) {
+        float x = 1.0f + 3.0f * ((float)i / (SQRT_TABLE_SIZE - 1));
+        sqrt_table[i] = sqrtf(x);
+    }
+}
+
+IRAM_ATTR float esp_foc_fast_sqrt(float x)
+{
+    if (x <= 0.0f) {
+        return 0.0f;
+    }
+
+    union {
+        float f;
+        uint32_t i;
+    } u = { x };
+
+    int e = (u.i >> 23) & 0xff;
+    int exponent = e - 127;
+    float m = 1.0f + (u.i & 0x7fffff) / 8388608.0f;
+    int odd = exponent & 1;
+
+    if (odd) {
+        m *= 2.0f;
+    }
+
+    float t = (m - 1.0f) / 3.0f;
+    float pos = t * (SQRT_TABLE_SIZE - 1);
+    int index = (int) pos;
+
+    if (index >= SQRT_TABLE_SIZE - 1) {
+        index = SQRT_TABLE_SIZE - 2;
+    }
+
+    float frac = pos - index;
+    float sqrt_m = sqrt_table[index] + frac * (sqrt_table[index + 1] - sqrt_table[index]);
+
+    if (odd) {
+        sqrt_m *= 0.707106781186547524f;
+    }
+
+    int new_exp = exponent >> 1;
+    return ldexpf(sqrt_m, new_exp);
+}