diff --git a/README.md b/README.md index 9bb5a80bb..2881db2d0 100644 --- a/README.md +++ b/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) diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index bc765967d..5180afc8f 100644 --- a/src/BLDCMotor.cpp +++ b/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 : diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 85ebd2c52..2b6e9c2ce 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/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; } diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index a193229a4..72dd71e75 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/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; diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 7ae372f78..3cb88141a 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -1,5 +1,7 @@ #include "foc_utils.h" +#include + // 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 diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index 1f2de371a..b3f1eb5ce 100644 --- a/src/communication/Commander.cpp +++ b/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; diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 64c5e48c1..18a25be86 100644 --- a/src/sensors/HallSensor.cpp +++ b/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(); diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index d4adad600..416983923 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/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 diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 9298413a2..a9de26994 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/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); diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 04b7cc75d..e53c46c10 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/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(); } diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index baaab2de4..e12cbffff 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/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 }