diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml
index 5c50bb1e..6c396ea9 100644
--- a/.github/workflows/ccpp.yml
+++ b/.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
diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml
new file mode 100644
index 00000000..9fc88b9a
--- /dev/null
+++ b/.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
diff --git a/README.md b/README.md
index 747b0f34..75475bec 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,8 @@
# SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library**
### A Cross-Platform FOC implementation for BLDC and Stepper motors
based on the Arduino IDE and PlatformIO
-
+ [](https://github.com/simplefoc/Arduino-FOC/actions/workflows/teensy.yml)
+



@@ -24,20 +25,20 @@ Therefore this is an attempt to:
- *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](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 📢 : SimpleFOClibrary 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 📢 : SimpleFOClibrary 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 stepper motor initialization
+> - Bugfix for current sensing when only 2 phase currents available - please re-check your current sense PID tuning
+> - Bugfix for teensy3.2 - [#321](https://github.com/simplefoc/Arduino-FOC/pull/321)
+> - Added teensy3/4 compile to the github CI using platformio
+> - Fix compile issues with recent versions of ESP32 framework
+> - Add ADC calibration on STM32 MCUs
+> - Bugfix for crash when using ADC2 on ESP32s - [thanks to @mcells](https://github.com/simplefoc/Arduino-FOC/pull/346)
+> - Bugfix for renesas PWM on UNO R4 WiFi - [thanks to @facchinm](https://github.com/simplefoc/Arduino-FOC/pull/322)
+> - And more bugfixes - see the complete list of 2.3.2 [fixes and PRs](https://github.com/simplefoc/Arduino-FOC/milestone/9?closed=1)
+
+
+## Arduino *SimpleFOClibrary* v2.3.2
diff --git a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino
index 51ac990e..ab8e3bba 100644
--- a/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino
+++ b/examples/hardware_specific_examples/B_G431B_ESC1/B_G431B_ESC1.ino
@@ -96,6 +96,7 @@ void setup() {
void loop() {
// main FOC algorithm function
+ motor.loopFOC();
// Motion control function
motor.move();
diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino
index a5b72f82..2b4cf6f1 100644
--- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino
+++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino
@@ -29,7 +29,7 @@
// Motor instance
BLDCMotor motor = BLDCMotor(11);
-BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INH_A, INH_B,INH_B, INH_C,INL_C, EN_GATE);
+BLDCDriver6PWM driver = BLDCDriver6PWM(INH_A,INL_A, INH_B,INL_B, INH_C,INL_C, EN_GATE);
// encoder instance
Encoder encoder = Encoder(2, 3, 8192);
diff --git a/keywords.txt b/keywords.txt
index 1f200e52..d8fd3e73 100644
--- a/keywords.txt
+++ b/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
diff --git a/library.properties b/library.properties
index 69043875..7242d57c 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
name=Simple FOC
-version=2.3.1
+version=2.3.2
author=Simplefoc
maintainer=Simplefoc
sentence=A library demistifying FOC for BLDC motors
diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp
index b72728bc..8dc2f5cd 100644
--- a/src/BLDCMotor.cpp
+++ b/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
@@ -545,6 +545,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
break;
case FOCModulationType::SinePWM :
+ case FOCModulationType::SpaceVectorPWM :
// Sinusoidal PWM modulation
// Inverse Park + Clarke transformation
_sincos(angle_el, &_sa, &_ca);
@@ -553,107 +554,34 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq;
Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq;
- // center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
- center = driver->voltage_limit/2;
// Clarke transform
- Ua = Ualpha + center;
- Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta + center;
- Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center;
+ Ua = Ualpha;
+ Ub = -0.5f * Ualpha + _SQRT3_2 * Ubeta;
+ Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta;
+
+ 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));
+ center -= (Umax+Umin) / 2;
+ }
if (!modulation_centered) {
float Umin = min(Ua, min(Ub, Uc));
Ua -= Umin;
Ub -= Umin;
Uc -= Umin;
+ }else{
+ Ua += center;
+ Ub += center;
+ Uc += center;
}
break;
- case FOCModulationType::SpaceVectorPWM :
- // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
- // https://www.youtube.com/watch?v=QMSWUMEAejg
-
- // the algorithm goes
- // 1) Ualpha, Ubeta
- // 2) Uout = sqrt(Ualpha^2 + Ubeta^2)
- // 3) angle_el = atan2(Ubeta, Ualpha)
- //
- // equivalent to 2) because the magnitude does not change is:
- // Uout = sqrt(Ud^2 + Uq^2)
- // equivalent to 3) is
- // angle_el = angle_el + atan2(Uq,Ud)
-
- float Uout;
- // a bit of optitmisation
- if(Ud){ // only if Ud and Uq set
- // _sqrt is an approx of sqrt (3-4% error)
- Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit;
- // angle normalisation in between 0 and 2pi
- // only necessary if using _sin and _cos - approximation functions
- angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
- }else{// only Uq available - no need for atan2 and sqrt
- Uout = Uq / driver->voltage_limit;
- // angle normalisation in between 0 and 2pi
- // only necessary if using _sin and _cos - approximation functions
- angle_el = _normalizeAngle(angle_el + _PI_2);
- }
- // find the sector we are in currently
- sector = floor(angle_el / _PI_3) + 1;
- // calculate the duty cycles
- float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout;
- float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout;
- // two versions possible
- float T0 = 0; // pulled to 0 - better for low power supply voltage
- if (modulation_centered) {
- T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2
- }
-
- // calculate the duty cycles(times)
- float Ta,Tb,Tc;
- switch(sector){
- case 1:
- Ta = T1 + T2 + T0/2;
- Tb = T2 + T0/2;
- Tc = T0/2;
- break;
- case 2:
- Ta = T1 + T0/2;
- Tb = T1 + T2 + T0/2;
- Tc = T0/2;
- break;
- case 3:
- Ta = T0/2;
- Tb = T1 + T2 + T0/2;
- Tc = T2 + T0/2;
- break;
- case 4:
- Ta = T0/2;
- Tb = T1+ T0/2;
- Tc = T1 + T2 + T0/2;
- break;
- case 5:
- Ta = T2 + T0/2;
- Tb = T0/2;
- Tc = T1 + T2 + T0/2;
- break;
- case 6:
- Ta = T1 + T2 + T0/2;
- Tb = T0/2;
- Tc = T1 + T0/2;
- break;
- default:
- // possible error state
- Ta = 0;
- Tb = 0;
- Tc = 0;
- }
-
- // calculate the phase voltages and center
- Ua = Ta*driver->voltage_limit;
- Ub = Tb*driver->voltage_limit;
- Uc = Tc*driver->voltage_limit;
- break;
-
}
// set the voltages in driver
diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp
index 491a20ee..7831f0bf 100644
--- a/src/StepperMotor.cpp
+++ b/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;
-}
\ No newline at end of file
+}
diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp
index 609162c2..217e8a67 100644
--- a/src/common/base_classes/CurrentSense.cpp
+++ b/src/common/base_classes/CurrentSense.cpp
@@ -16,12 +16,12 @@ float CurrentSense::getDCCurrent(float motor_electrical_angle){
// if only two measured currents
i_alpha = current.a;
i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
- }if(!current.a){
+ }else if(!current.a){
// if only two measured currents
float a = -current.c - current.b;
i_alpha = a;
i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
- }if(!current.b){
+ }else if(!current.b){
// if only two measured currents
float b = -current.a - current.c;
i_alpha = current.a;
@@ -62,12 +62,12 @@ DQCurrent_s CurrentSense::getFOCCurrents(float angle_el){
// if only two measured currents
i_alpha = current.a;
i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
- }if(!current.a){
+ }else if(!current.a){
// if only two measured currents
float a = -current.c - current.b;
i_alpha = a;
i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
- }if(!current.b){
+ }else if(!current.b){
// if only two measured currents
float b = -current.a - current.c;
i_alpha = current.a;
diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp
index a80fa438..b0a8f4db 100644
--- a/src/common/base_classes/Sensor.cpp
+++ b/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
diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp
index 4cb09863..233bd246 100644
--- a/src/common/foc_utils.cpp
+++ b/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;
}
diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h
index 0efe3b59..abdeebf8 100644
--- a/src/common/foc_utils.h
+++ b/src/common/foc_utils.h
@@ -79,6 +79,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]
diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp
index ee6be9f9..d2822b04 100644
--- a/src/communication/Commander.cpp
+++ b/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++;
}
diff --git a/src/communication/Commander.h b/src/communication/Commander.h
index 91e3dc45..4ec2b281 100644
--- a/src/communication/Commander.h
+++ b/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
diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h
index f9691fd3..3627b5e7 100644
--- a/src/communication/StepDirListener.h
+++ b/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
diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp
index 807c387d..57ffdfb5 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp
+++ b/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"
@@ -153,9 +154,8 @@ bool IRAM_ATTR __adcStart(uint8_t pin){
}
if(channel > 9){
- channel -= 10;
CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
- SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
+ SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S);
SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
} else {
CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
@@ -224,9 +224,8 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin)
__analogInit();
if(channel > 9){
- channel -= 10;
CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
- SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
+ SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << (channel - 10)), SENS_SAR2_EN_PAD_S);
SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
} else {
CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
@@ -256,4 +255,4 @@ uint16_t IRAM_ATTR adcRead(uint8_t pin)
}
-#endif
\ No newline at end of file
+#endif
diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp
index 3df9dff6..2057463c 100644
--- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp
+++ b/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*){
diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp
index 3e10bbca..8456759c 100644
--- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp
+++ b/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);
diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp
index 8daa7eef..51b494f4 100644
--- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp
+++ b/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);
diff --git a/src/drivers/hardware_specific/renesas/renesas.cpp b/src/drivers/hardware_specific/renesas/renesas.cpp
index f90a4c56..2c9c9b22 100644
--- a/src/drivers/hardware_specific/renesas/renesas.cpp
+++ b/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");
diff --git a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp
index 3a7b5de5..fe145273 100644
--- a/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp
+++ b/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp
@@ -3,7 +3,6 @@
// if defined
// - Teensy 3.0 MK20DX128
// - Teensy 3.1/3.2 MK20DX256
-// - Teensy 3.5 MK20DX128
// - Teensy LC MKL26Z64
// - Teensy 3.5 MK64FX512
// - Teensy 3.6 MK66FX1M0
@@ -14,7 +13,7 @@
#pragma message("SimpleFOC: compiling for Teensy 3.x")
#pragma message("")
-// pin definition from https://github.com/PaulStoffregen/cores/blob/286511f3ec849a6c9e0ec8b73ad6a2fada52e44c/teensy3/pins_teensy.c
+// pin definition from https://github.com/PaulStoffregen/cores/blob/286511f3ec849a6c9e0ec8b73ad6a2fada52e44c/teensy3/pins_teensy.c#L627
#if defined(__MK20DX128__)
#define FTM0_CH0_PIN 22
#define FTM0_CH1_PIN 23
@@ -116,7 +115,7 @@ int _findTimer( const int Ah, const int Al, const int Bh, const int Bl, const i
}
}
- #ifdef FTM3_SC // if the board has FTM3 timer
+#ifdef FTM3_CH0_PIN // if the board has FTM3 timer
if((Ah == FTM3_CH0_PIN && Al == FTM3_CH1_PIN) ||
(Ah == FTM3_CH2_PIN && Al == FTM3_CH3_PIN) ||
(Ah == FTM3_CH4_PIN && Al == FTM3_CH5_PIN) ){
@@ -134,7 +133,7 @@ int _findTimer( const int Ah, const int Al, const int Bh, const int Bl, const i
}
}
}
- #endif
+#endif
#ifdef SIMPLEFOC_TEENSY_DEBUG
SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Pins not on timers FTM0 or FTM3!");
diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp
index 38767d59..d9e5ec83 100644
--- a/src/sensors/HallSensor.cpp
+++ b/src/sensors/HallSensor.cpp
@@ -53,7 +53,6 @@ void HallSensor::updateState() {
hall_state = new_hall_state;
int8_t new_electric_sector = ELECTRIC_SECTORS[hall_state];
- static Direction old_direction;
if (new_electric_sector - electric_sector > 3) {
//underflow
direction = Direction::CCW;
diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h
index ad50c7d5..78e1b416 100644
--- a/src/sensors/HallSensor.h
+++ b/src/sensors/HallSensor.h
@@ -62,6 +62,7 @@ class HallSensor: public Sensor{
// whether last step was CW (+1) or CCW (-1).
Direction direction;
+ Direction old_direction;
void attachSectorCallback(void (*onSectorChange)(int a) = nullptr);