Fix confirmed logic bugs found while auditing the library sources (13 fixes, one per commit)

README.md

@@ -101,7 +101,7 @@ This video is a bit outdated but it demonstrates the *Simple**FOC**library* basi
    - Built-in communication and monitoring via Serial, I2C, or custom protocols
 - **Cross-platform**:
    - Seamless code transfer from one microcontroller family to another 
-   - Supports multiple [MCU architectures](https://docs.simplefoc.commicrocontrollers):
+   - Supports multiple [MCU architectures](https://docs.simplefoc.com/microcontrollers):
       - Arduino: UNO R4, UNO, MEGA, DUE, Leonardo, Nano, Nano33, MKR ....
       - STM32 (Nucleo, Bluepill, B-G431B-ESC1, H7 family, etc.)
       - ESP32 (ESP32, ESP32-S2, ESP32-S3, ESP32-C3, ESP32-C6)

src/BLDCMotor.cpp

@@ -165,7 +165,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
       // centering the voltages around either
       // modulation_centered == true > driver.voltage_limit/2
       // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
-      center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
+      center = modulation_centered ? (driver->voltage_limit)/2 : 0;
 
       if(trap_120_map[sector][0]  == _HIGH_IMPEDANCE){
         Ua= center;
@@ -184,6 +184,15 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
         driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_OFF);// disable phase if possible
       }
 
+      if(!modulation_centered){
+        // non-centered modulation: shift all phases up so the lowest one is at 0,
+        // same idiom as the sine/SVPWM branches
+        float Umin = min(Ua, min(Ub, Uc));
+        Ua -= Umin;
+        Ub -= Umin;
+        Uc -= Umin;
+      }
+
     break;
 
     case FOCModulationType::Trapezoid_150 :
@@ -193,7 +202,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
       // centering the voltages around either
       // modulation_centered == true > driver.voltage_limit/2
       // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
-      center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
+      center = modulation_centered ? (driver->voltage_limit)/2 : 0;
 
       if(trap_150_map[sector][0]  == _HIGH_IMPEDANCE){
         Ua= center;
@@ -217,6 +226,15 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
         driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_ON); // enable all phases
       }
 
+      if(!modulation_centered){
+        // non-centered modulation: shift all phases up so the lowest one is at 0,
+        // same idiom as the sine/SVPWM branches
+        float Umin = min(Ua, min(Ub, Uc));
+        Ua -= Umin;
+        Ub -= Umin;
+        Uc -= Umin;
+      }
+
     break;
 
     case FOCModulationType::SinePWM :

src/common/base_classes/CurrentSense.cpp

@@ -267,7 +267,7 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool m
                 _swap(pinA, pinB);
                 _swap(offset_ia, offset_ib);
                 _swap(gain_a, gain_b);
-                _swap(c_a.b, c_a.b);
+                _swap(c_a.a, c_a.b);
                 phases_switched = true; // signal that pins have been switched
                 break;
             case 2: // phase C is the max current
@@ -297,14 +297,14 @@ int CurrentSense::alignBLDCDriver(float voltage, BLDCDriver* bldc_driver, bool m
             _swap(pinA, pinB);
             _swap(offset_ia, offset_ib);
             _swap(gain_a, gain_b);
-            _swap(c_a.b, c_a.b);
+            _swap(c_a.a, c_a.b);
             phases_switched = true; // signal that pins have been switched
         }else if(_isset(pinA) && !_isset(pinC)){
             SIMPLEFOC_DEBUG("CS: Switch A-(C)NC");
             _swap(pinA, pinC);
             _swap(offset_ia, offset_ic);
             _swap(gain_a, gain_c);
-            _swap(c_a.b, c_a.c);
+            _swap(c_a.a, c_a.c);
             phases_switched = true; // signal that pins have been switched
         }
     }
@@ -450,6 +450,7 @@ int CurrentSense::alignStepperDriver(float voltage, StepperDriver* stepper_drive
         _swap(pinA, pinB);
         _swap(offset_ia, offset_ib);
         _swap(gain_a, gain_b);
+        _swap(c.a, c.b);
         phases_switched = true; // signal that pins have been switched
     }
     // 2) check if measured current a is positive and invert if not
@@ -629,14 +630,14 @@ int CurrentSense::alignHybridDriver(float voltage, BLDCDriver* bldc_driver, bool
     if(c.a && c.c){
         // if a and mid-phase c measured
         // verify that they have almost the same magnitude
-        if((fabs(c.a) - fabs(c.c)) > 0.1f){
+        if(fabs(fabs(c.a) - fabs(c.c)) > 0.1f){
             SIMPLEFOC_DEBUG("CS: Err A-C currents not equal!");
             return 0;
         }
     }else if(c.b && c.c){
-        // if a and mid-phase c measured
+        // if b and mid-phase c measured
         // verify that they have almost the same magnitude
-        if((fabs(c.a) - fabs(c.c)) > 0.1f){
+        if(fabs(fabs(c.b) - fabs(c.c)) > 0.1f){
             SIMPLEFOC_DEBUG("CS: Err B-C currents not equal!");
             return 0;
         }else{
@@ -702,7 +703,7 @@ int CurrentSense::alignHybridDriver(float voltage, BLDCDriver* bldc_driver, bool
     if(c.b && c.c){
         // if b and mid-phase c measured
         // verify that they have almost the same magnitude
-        if((fabs(c.b) - fabs(c.c)) > 0.1f){
+        if(fabs(fabs(c.b) - fabs(c.c)) > 0.1f){
             SIMPLEFOC_DEBUG("CS: Err B-C currents not equal!");
             return 0;
         }

src/common/base_classes/FOCMotor.cpp

@@ -425,8 +425,9 @@ float FOCMotor::angleOpenloop(float target_angle){
   //                        where small position changes are no longer captured by the precision of floats
   //                        when the total position is large.
   if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){
-    shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts;
-    shaft_velocity = velocity_limit;
+    float move_direction = _sign(target_angle - shaft_angle);
+    shaft_angle += move_direction * abs( velocity_limit ) * Ts;
+    shaft_velocity = move_direction * abs( velocity_limit );
   }else{
     shaft_angle = target_angle;
     shaft_velocity = 0;

src/common/foc_utils.cpp

@@ -1,5 +1,7 @@
 #include "foc_utils.h"
 
+#include <float.h>
+
 
 // function approximating the sine calculation by using fixed size array
 // uses a 65 element lookup table and interpolation
@@ -56,7 +58,7 @@ __attribute__((weak)) float _atan2(float y, float x) {
     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));
+    float a = min(abs_x, abs_y) / (max(abs_x, abs_y) + FLT_MIN);
     // s := a * a
     float s = a * a;
     // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a

src/communication/Commander.cpp

@@ -12,6 +12,8 @@ Commander::Commander(char eol, bool echo){
 
 
 void Commander::add(char id, CommandCallback onCommand, const char* label ){
+  // guard the fixed-size callback arrays (call_list/call_ids/call_label)
+  if (call_count >= (int)(sizeof(call_list) / sizeof(call_list[0]))) return;
   call_list[call_count] = onCommand;
   call_ids[call_count] = id;
   call_label[call_count] = (char*)label;

src/sensors/HallSensor.cpp

@@ -18,6 +18,19 @@ HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){
 
   // extern pullup as default
   pullup = Pullup::USE_EXTERN;
+
+  // initialise the state variables - they would otherwise be indeterminate
+  // for sensors that do not live in the BSS section (heap/stack instances)
+  use_interrupt = false;
+  hall_state = 0;
+  electric_sector = 0;
+  electric_rotations = 0;
+  total_interrupts = 0;
+  pulse_diff = 0;
+  pulse_timestamp = _micros();
+  A_active = B_active = C_active = 0;
+  direction = Direction::UNKNOWN;
+  old_direction = Direction::UNKNOWN;
 }
 
 //  HallSensor interrupt callback functions
@@ -130,11 +143,12 @@ float HallSensor::getVelocity(){
   noInterrupts();
   long last_pulse_timestamp = pulse_timestamp;
   long last_pulse_diff = pulse_diff;
+  Direction last_direction = direction;
   interrupts();
   if (last_pulse_diff == 0 || ((long)(_micros() - last_pulse_timestamp) > last_pulse_diff*2) ) { // last velocity isn't accurate if too old
     return 0;
   } else {
-    return direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f);
+    return last_direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f);
   }
 
 }
@@ -156,11 +170,15 @@ void HallSensor::init(){
     pinMode(pinC, INPUT);
   }
 
-    // init hall_state
+  // adopt the current hall state as the starting point instead of calling
+  // updateState(): that would compare the measured sector with the zeroed
+  // one and count a spurious +-1 electric rotation for 2 of the 6 power-up
+  // positions, offsetting the angle and the full-rotation counter
   A_active = digitalRead(pinA);
   B_active = digitalRead(pinB);
   C_active = digitalRead(pinC);
-  updateState();
+  hall_state = C_active + (B_active << 1) + (A_active << 2);
+  electric_sector = ELECTRIC_SECTORS[hall_state];
 
   pulse_timestamp = _micros();
 

src/sensors/MagneticSensorAnalog.cpp

@@ -33,7 +33,7 @@ void MagneticSensorAnalog::init(){
 float MagneticSensorAnalog::getSensorAngle(){
   // raw data from the sensor
   raw_count = getRawCount();   
-  return ( (float) (raw_count) / (float)cpr) * _2PI;
+  return ( (float) (raw_count - min_raw_count) / (float)cpr) * _2PI;
 }
 
 // function reading the raw counter of the magnetic sensor

src/sensors/MagneticSensorI2C.cpp

@@ -46,11 +46,15 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution,
   _conf.msb_mask = (uint8_t)( (1 << _bits_used_msb) - 1 );
 
   uint8_t lsb_used = _bit_resolution - _bits_used_msb; // used bits in LSB
-  _conf.lsb_mask = (uint8_t)( (1 << (lsb_used)) - 1 );
-  if (!lsb_right_aligned)
+  if (!lsb_right_aligned){
+    // left aligned: the remaining bits are in the upper part of the low byte,
+    // e.g. 6 bits -> 0xFC, read with >> 2
+    _conf.lsb_mask = (uint8_t)( ((1 << (lsb_used)) - 1) << (8 - lsb_used) );
     _conf.lsb_shift = 8-lsb_used;
-  else
+  }else{
+    _conf.lsb_mask = (uint8_t)( (1 << (lsb_used)) - 1 );
     _conf.lsb_shift = 0;
+  }
   _conf.msb_shift = lsb_used;
 
   cpr = _powtwo(_bit_resolution);

src/sensors/MagneticSensorPWM.cpp

@@ -45,6 +45,11 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int freqHz, int _total_pwm
 
     min_elapsed_time = 1.0f/freqHz; // set the minimum time between two readings
 
+    // scale the blocking read timeout with the PWM frequency: the high pulse can
+    // be almost a full period long (e.g. ~8.4ms at 115Hz) while the 1200us
+    // default only covers the fastest supported frequency (920Hz)
+    timeout_us = (unsigned int)(1.2f * 1000000.0f / freqHz);
+
     // define as not set
     last_call_us = _micros();
 }

src/sensors/MagneticSensorSPI.cpp

@@ -166,7 +166,7 @@ word MagneticSensorSPI::read(word angle_register){
 
   register_value = register_value >> (1 + data_start_bit - bit_resolution);  //this should shift data to the rightmost bits of the word
 
-  const static word data_mask = 0xFFFF >> (16 - bit_resolution);
+  const word data_mask = 0xFFFF >> (16 - bit_resolution);
 
 	return register_value & data_mask;  // Return the data, stripping the non data (e.g parity) bits
 }