Merge branch 'dev' into foc_refactoring

.github/workflows/ccpp.yml

@@ -58,15 +58,15 @@ jobs:
             sketch-names: single_full_control_example.ino
 
           - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2
-            platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json
-            sketch-names: bldc_driver_3pwm_standalone.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone 
+            platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json
+            sketch-names: bldc_driver_3pwm_standalone.ino,stepper_driver_2pwm_standalone.ino,stepper_driver_4pwm_standalone.ino
 
           - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3
-            platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json
+            platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json
             sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino
 
           - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 
-            platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json
+            platform-url: https://espressif.github.io/arduino-esp32/package_esp32_index.json
             sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino
 
           - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples

.github/workflows/teensy.yml

@@ -0,0 +1,41 @@
+name: PlatformIO - Teensy build
+
+on: [push]
+
+jobs:
+  build:
+
+    runs-on: ubuntu-latest        
+    steps:
+      - uses: actions/checkout@v3
+      - uses: actions/cache@v3
+        with:
+          path: |
+            ~/.cache/pip
+            ~/.platformio/.cache
+          key: ${{ runner.os }}-pio
+      - uses: actions/setup-python@v4
+        with:
+          python-version: '3.9'
+      - name: Install PlatformIO Core
+        run: pip install --upgrade platformio
+
+      - name: PIO Run Teensy 4
+        run: pio ci --lib="." --board=teensy41 --board=teensy40
+        env:
+          PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino
+
+      - name: PIO Run Teensy 4
+        run: pio ci --lib="." --board=teensy41 --board=teensy40
+        env:
+          PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino
+
+      - name: PIO Run Teensy 3
+        run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36
+        env:
+          PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy3/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino
+
+      - name: PIO Run Teensy 3
+        run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36
+        env:
+          PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy3/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino

README.md

@@ -1,7 +1,8 @@
 # SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library** <br> 
 ### A Cross-Platform FOC implementation for BLDC and Stepper motors<br> based on the Arduino IDE and PlatformIO 
 
-![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg)
+![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) [![PlatformIO - Teensy build](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml/badge.svg)](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml) 
+
 ![GitHub release (latest by date)](https://img.shields.io/github/v/release/simplefoc/arduino-foc)
 ![GitHub Release Date](https://img.shields.io/github/release-date/simplefoc/arduino-foc?color=blue)
 ![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev)
@@ -24,20 +25,13 @@ Therefore this is an attempt to:
    - *Medium-power* BLDC driver (<30Amps): [Arduino <span class="simple">Simple<b>FOC</b>PowerShield</span> ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield).
    - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller)
 
-> NEW RELEASE 📢 : <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.3.1
-> - Support for Arduino UNO R4 Minima (Renesas R7FA4M1 MCU - note UNO R4 WiFi is not yet supported)
-> - Support setting PWM polarity on ESP32 (thanks to [@mcells](https://github.com/mcells))
-> - Expose I2C errors in MagneticSensorI2C (thanks to [@padok](https://github.com/padok))
-> - Improved default trig functions (sine, cosine) - faster, smaller
-> - Overridable trig functions - plug in your own optimized versions
-> - Bugfix: microseconds overflow in velocity mode [#287](https://github.com/simplefoc/Arduino-FOC/issues/287)
-> - Bugfix: KV initialization ([5fc3128](https://github.com/simplefoc/Arduino-FOC/commit/5fc3128d282b65c141ca486327c6235089999627))
-> - And more bugfixes - see the [complete list of 2.3.1 fixes here](https://github.com/simplefoc/Arduino-FOC/issues?q=is%3Aissue+milestone%3A2.3.1_Release)
-> - Change: simplify initFOC() API ([d57d32d](https://github.com/simplefoc/Arduino-FOC/commit/d57d32dd8715dbed4e476469bc3de0c052f1d531). [5231e5e](https://github.com/simplefoc/Arduino-FOC/commit/5231e5e1d044b0cc33ede67664b6ef2f9d0a8cdf), [10c5b87](https://github.com/simplefoc/Arduino-FOC/commit/10c5b872672cab72df16ddd738bbf09bcce95d28))
-> - Change: check for linked driver in currentsense and exit gracefully ([5ef4d9d](https://github.com/simplefoc/Arduino-FOC/commit/5ef4d9d5a92e03da0dd5af7f624243ab30f1b688))
-> - Compatibility with newest versions of Arduino framework for STM32, Renesas, ESP32, Atmel SAM, Atmel AVR, nRF52 and RP2040
-
-## Arduino *SimpleFOClibrary* v2.3.1
+> NEW RELEASE 📢 : <span class="simple">Simple<span class="foc">FOC</span>library</span> v2.3.2
+> - Improved [space vector modulation code](https://github.com/simplefoc/Arduino-FOC/pull/309) thanks to [@Candas1](https://github.com/Candas1)
+> - Bugfix for teensy3.2 - [#321](https://github.com/simplefoc/Arduino-FOC/pull/321)
+> - Added teensy3/4 compile to the github CI using platformio
+> - And more bugfixes - see the [complete list of 2.3.2 fixes here](https://github.com/simplefoc/Arduino-FOC/issues?q=is%3Aissue+milestone%3A2.3.2_Release)
+
+## Arduino *SimpleFOClibrary* v2.3.2
 
 <p align="">
 <a href="https://youtu.be/Y5kLeqTc6Zk">

keywords.txt

@@ -133,6 +133,7 @@ sensor_direction	KEYWORD2
 sensor_offset	KEYWORD2
 zero_electric_angle	KEYWORD2
 verbose	KEYWORD2
+verboseMode	KEYWORD1
 decimal_places	KEYWORD2
 phase_resistance	KEYWORD2
 phase_inductance	KEYWORD2

library.properties

@@ -1,5 +1,5 @@
 name=Simple FOC
-version=2.3.1
+version=2.3.2
 author=Simplefoc <info@simplefoc.com>
 maintainer=Simplefoc <info@simplefoc.com>
 sentence=A library demistifying FOC for BLDC motors

src/BLDCMotor.cpp

@@ -422,7 +422,7 @@ void BLDCMotor::move(float new_target) {
       shaft_angle_sp = target;
       // calculate velocity set point
       shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
-      shaft_angle_sp = _constrain(shaft_angle_sp,-velocity_limit, velocity_limit);
+      shaft_velocity_sp = _constrain(shaft_velocity_sp,-velocity_limit, velocity_limit);
       // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
       // if torque controlled through voltage
@@ -561,6 +561,8 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
 
       center = driver->voltage_limit/2;
       if (foc_modulation == FOCModulationType::SpaceVectorPWM){
+        // discussed here: https://community.simplefoc.com/t/embedded-world-2023-stm32-cordic-co-processor/3107/165?u=candas1
+        // a bit more info here: https://microchipdeveloper.com/mct5001:which-zsm-is-best
         // Midpoint Clamp
         float Umin = min(Ua, min(Ub, Uc));
         float Umax = max(Ua, max(Ub, Uc));

src/StepperMotor.cpp

@@ -134,7 +134,7 @@ int StepperMotor::alignSensor() {
   SIMPLEFOC_DEBUG("MOT: Align sensor.");
 
   // if unknown natural direction
-  if(!_isset(sensor_direction)){
+  if(sensor_direction == Direction::UNKNOWN){
     // check if sensor needs zero search
     if(sensor->needsSearch()) exit_flag = absoluteZeroSearch();
     // stop init if not found index
@@ -311,7 +311,7 @@ void StepperMotor::move(float new_target) {
       shaft_angle_sp = target;
       // calculate velocity set point
       shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
-      shaft_angle_sp = _constrain(shaft_angle_sp, -velocity_limit, velocity_limit);
+      shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit);
       // calculate the torque command
       current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
       // if torque controlled through voltage
@@ -440,4 +440,4 @@ float StepperMotor::angleOpenloop(float target_angle){
   open_loop_timestamp = now_us;
 
   return Uq;
-}
+}

src/common/base_classes/Sensor.cpp

@@ -6,6 +6,8 @@
 
 void Sensor::update() {
     float val = getSensorAngle();
+    if (val<0) // sensor angles are strictly non-negative. Negative values are used to signal errors.
+        return; // TODO signal error, e.g. via a flag and counter
     angle_prev_ts = _micros();
     float d_angle = val - angle_prev;
     // if overflow happened track it as full rotation

src/common/foc_utils.cpp

@@ -44,6 +44,33 @@ __attribute__((weak)) void _sincos(float a, float* s, float* c){
   *c = _cos(a);
 }
 
+// fast_atan2 based on https://math.stackexchange.com/a/1105038/81278
+// Via Odrive project
+// https://github.com/odriverobotics/ODrive/blob/master/Firmware/MotorControl/utils.cpp
+// This function is MIT licenced, copyright Oskar Weigl/Odrive Robotics
+// The origin for Odrive atan2 is public domain. Thanks to Odrive for making
+// it easy to borrow.
+__attribute__((weak)) float _atan2(float y, float x) {
+    // a := min (|x|, |y|) / max (|x|, |y|)
+    float abs_y = fabsf(y);
+    float abs_x = fabsf(x);
+    // inject FLT_MIN in denominator to avoid division by zero
+    float a = min(abs_x, abs_y) / (max(abs_x, abs_y));
+    // s := a * a
+    float s = a * a;
+    // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
+    float r =
+        ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
+    // if |y| > |x| then r := 1.57079637 - r
+    if (abs_y > abs_x) r = 1.57079637f - r;
+    // if x < 0 then r := 3.14159274 - r
+    if (x < 0.0f) r = 3.14159274f - r;
+    // if y < 0 then r := -r
+    if (y < 0.0f) r = -r;
+
+    return r;
+  }
+
 
 // normalizing radian angle to [0,2PI]
 __attribute__((weak)) float _normalizeAngle(float angle){
@@ -60,14 +87,10 @@ float _electricalAngle(float shaft_angle, int pole_pairs) {
 // https://reprap.org/forum/read.php?147,219210
 // https://en.wikipedia.org/wiki/Fast_inverse_square_root
 __attribute__((weak)) float _sqrtApprox(float number) {//low in fat
-  // float x;
-  // const float f = 1.5F; // better precision
-
-  // x = number * 0.5F;
-  float y = number;
-  long i = * ( long * ) &y;
-  i = 0x5f375a86 - ( i >> 1 );
-  y = * ( float * ) &i;
-  // y = y * ( f - ( x * y * y ) ); // better precision
-  return number * y;
+  union {
+    float    f;
+    uint32_t i;
+  } y = { .f = number };
+  y.i = 0x5f375a86 - ( y.i >> 1 );
+  return number * y.f;
 }

src/common/foc_utils.h

@@ -85,6 +85,11 @@ float _cos(float a);
  */
 void _sincos(float a, float* s, float* c);
 
+/**
+ * Function approximating atan2 
+ * 
+ */
+float _atan2(float y, float x);
 
 /**
  * normalizing radian angle to [0,2PI]

src/communication/Commander.cpp

@@ -11,10 +11,10 @@ Commander::Commander(char eol, bool echo){
 }
 
 
-void Commander::add(char id, CommandCallback onCommand, char* label ){
+void Commander::add(char id, CommandCallback onCommand, const char* label ){
   call_list[call_count] = onCommand;
   call_ids[call_count] = id;
-  call_label[call_count] = label;
+  call_label[call_count] = (char*)label;
   call_count++;
 }
 

src/communication/Commander.h

@@ -91,7 +91,7 @@ class Commander
      * @param onCommand  - function pointer void function(char*)
      * @param label      - string label to be displayed when scan command sent
      */
-    void add(char id , CommandCallback onCommand, char* label = nullptr);
+    void add(char id , CommandCallback onCommand, const char* label = nullptr);
 
     // printing variables
     VerboseMode verbose = VerboseMode::user_friendly; //!< flag signaling that the commands should output user understanable text

src/communication/StepDirListener.h

@@ -5,11 +5,6 @@
 #include "../common/foc_utils.h"
 
 
-#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) && !defined(ARDUINO_UNOR4_WIFI) && !defined(ARDUINO_UNOR4_MINIMA) && !defined(NRF52_SERIES) && !defined(ARDUINO_ARCH_MEGAAVR)
-#define PinStatus int
-#endif
-
-
 /**
  * Step/Dir listenner class for easier interraction with this communication interface.
  */
@@ -53,7 +48,7 @@ class StepDirListener
     int pin_step; //!< step pin
     int pin_dir; //!< direction pin
     long count; //!< current counter value - should be set to 0 for homing
-    PinStatus polarity = RISING; //!< polarity of the step pin
+    decltype(RISING) polarity = RISING; //!< polarity of the step pin
 
   private:
     float* attached_variable = nullptr; //!< pointer to the attached variable 

src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp

@@ -6,7 +6,8 @@
 #include "freertos/task.h"
 #include "rom/ets_sys.h"
 #include "esp_attr.h"
-#include "esp_intr.h"
+//#include "esp_intr.h" deprecated
+#include "esp_intr_alloc.h"
 #include "soc/rtc_io_reg.h"
 #include "soc/rtc_cntl_reg.h"
 #include "soc/sens_reg.h"

src/current_sense/hardware_specific/esp32/esp32_mcu.cpp

@@ -121,6 +121,8 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){
     mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL);  //Set ISR Handler
 }
 
+static void IRAM_ATTR mcpwm0_isr_handler(void*) __attribute__ ((unused));
+
 // Read currents when interrupt is triggered
 static void IRAM_ATTR mcpwm0_isr_handler(void*){
   // // high side
@@ -139,6 +141,7 @@ static void IRAM_ATTR mcpwm0_isr_handler(void*){
   // MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0;
 }
 
+static void IRAM_ATTR mcpwm1_isr_handler(void*) __attribute__ ((unused));
 
 // Read currents when interrupt is triggered
 static void IRAM_ATTR mcpwm1_isr_handler(void*){

src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp

@@ -110,6 +110,9 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p
   MX_ADC1_Init(&hadc1);
   MX_ADC2_Init(&hadc2);
 
+  HAL_ADCEx_Calibration_Start(&hadc1,ADC_SINGLE_ENDED);
+  HAL_ADCEx_Calibration_Start(&hadc2,ADC_SINGLE_ENDED);
+
   MX_DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1);
   MX_DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2);
 

src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp

@@ -66,6 +66,10 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){
   }
   // set the trigger output event
   LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
+
+  // Start the adc calibration
+  HAL_ADCEx_Calibration_Start(cs_params->adc_handle);
+
   // start the adc 
   HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle);
 

src/drivers/hardware_specific/renesas/renesas.cpp

@@ -144,7 +144,7 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool
   uint8_t timer_channel = GET_CHANNEL(pinCfgs[0]);
   // check its not used
   if (channel_used[timer_channel]) {
-    SIMPLEFOC_DEBUG("DRV: channel in use");
+    SIMPLEFOC_DEBUG("DRV: channel in use: ", timer_channel);
     return false;
   }
 
@@ -207,6 +207,10 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool
   t->timer_cfg.cycle_end_irq = FSP_INVALID_VECTOR;
 
   t->ext_cfg.p_pwm_cfg = &(t->pwm_cfg);
+  t->ext_cfg.capture_a_ipl = BSP_IRQ_DISABLED;
+  t->ext_cfg.capture_a_irq = FSP_INVALID_VECTOR;
+  t->ext_cfg.capture_b_ipl = BSP_IRQ_DISABLED;
+  t->ext_cfg.capture_b_irq = FSP_INVALID_VECTOR;
   t->pwm_cfg.trough_ipl = BSP_IRQ_DISABLED;
   t->pwm_cfg.trough_irq = FSP_INVALID_VECTOR;
   t->pwm_cfg.poeg_link = GPT_POEG_LINK_POEG0;
@@ -258,26 +262,17 @@ bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool
     t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (active_high ? 0x00 : 0x10);
     t->ext_cfg.gtior_setting.gtior_b.obdflt = active_high ? 0x00 : 0x01;
   }
-
-  // lets stop the timer in case its running
-  if (GPT_OPEN == t->ctrl.open) {
-    if (R_GPT_Stop(&(t->ctrl)) != FSP_SUCCESS) {
-      SIMPLEFOC_DEBUG("DRV: timer stop failed");
-      return false;
-    }
-  }
-
   memset(&(t->ctrl), 0, sizeof(gpt_instance_ctrl_t));
   err = R_GPT_Open(&(t->ctrl),&(t->timer_cfg));
   if ((err != FSP_ERR_ALREADY_OPEN) && (err != FSP_SUCCESS)) {
     SIMPLEFOC_DEBUG("DRV: open failed");
     return false;
   }
-  #if defined(SIMPLEFOC_RESENSAS_DEBUG)
   if (err == FSP_ERR_ALREADY_OPEN) {
     SimpleFOCDebug::println("DRV: timer already open");
+    return false;
   }
-  #endif
+
   err = R_GPT_PeriodSet(&(t->ctrl), t->timer_cfg.period_counts);
   if (err != FSP_SUCCESS) {
     SIMPLEFOC_DEBUG("DRV: period set failed");