From 8f436b405fcd1b5f1659d07d850394aeef7c86c4 Mon Sep 17 00:00:00 2001 From: Devin Marx <106412270+DevinJM3@users.noreply.github.com> Date: Fri, 21 Aug 2026 10:49:41 -0500 Subject: [PATCH 01/14] Fixed broken link on README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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) From b1a027cdd2e589ff086513f16b480d978e22d4bd Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:30 +0200 Subject: [PATCH 02/14] fix(current sense): swap the measured currents, not a variable with itself The alignment routine relabels the pins when the highest current was measured on another channel, but the corresponding sample swap was written as `_swap(c_a.b, c_a.b)` - a no-op. The polarity check that follows (`_sign(c_a.a) < 0`) therefore inspects the sample of the wrong channel and can invert the gain of the phase it just repaired, which turns the current feedback into positive feedback at low currents. Also fixed the same pattern in the 'A-(C)NC' branch, which swapped c_a.b with c_a.c while the pins swapped were A and C. Reported in the alignment audit; trigger is exactly the miswiring this feature exists to correct. --- src/common/base_classes/CurrentSense.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 85ebd2c52..97b1a833a 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 } } From 9f7065e269521b9339877380b8b22fbf354c37da Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:30 +0200 Subject: [PATCH 03/14] fix(current sense): check the polarity of the channel that was just relabelled Same defect as the BLDC alignment: when the stepper alignment decides that the measured phase A is on the other ADC channel, it swaps the pins/offsets/gains but not the samples, so the following `if (c.a < 0)` tests the sample of the channel that is *not* carrying the current (and that reads ~0). The phase A gain ends up with the wrong sign for exactly the wiring the routine is meant to correct. --- src/common/base_classes/CurrentSense.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 97b1a833a..2e8897cb6 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -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 From d1b6c4b48d2687cf89617aca010434e52695733f Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:30 +0200 Subject: [PATCH 04/14] fix(current sense): compare current magnitudes, not signed values Three flaws in the same alignment step: - the A-C branch compared `fabs(c.a) - fabs(c.c)` against a threshold without taking the absolute value of the difference, so a channel that reads *higher* than the driven phase passes the check instead of raising the error; - the B-C branch compared c.a with c.c (copy-paste from the branch above) while the message and the comment refer to phase B; - the phase-B section had the same missing fabs(). --- src/common/base_classes/CurrentSense.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/common/base_classes/CurrentSense.cpp b/src/common/base_classes/CurrentSense.cpp index 2e8897cb6..2b6e9c2ce 100644 --- a/src/common/base_classes/CurrentSense.cpp +++ b/src/common/base_classes/CurrentSense.cpp @@ -630,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{ @@ -703,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; } From 016d41549f4854e9c7c322cc06cd2ee8b844cc41 Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:31 +0200 Subject: [PATCH 05/14] fix(commander): guard the fixed-size callback arrays in add() call_list/call_ids/call_label hold 20 entries and call_count was never checked, so the 21st add() writes a function pointer, a char and a pointer past the end of the object - out-of-bounds write, memory corruption, symptoms depending on layout. --- src/communication/Commander.cpp | 2 ++ 1 file changed, 2 insertions(+) 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; From e51d988b7b93e7ebbbbc43d1e1ae61dede64010a Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:31 +0200 Subject: [PATCH 06/14] fix(hall sensor): initialise the state and don't fake a rotation at start-up The constructor left hall_state, electric_sector, electric_rotations, total_interrupts, pulse_diff, pulse_timestamp, direction, old_direction and use_interrupt uninitialised - indeterminate values for any sensor that is not in the BSS section, and wrong FOC start-up behaviour even for globals. init() then called updateState(), which compares the measured sector against that uninitialised/zeroed sector and, when the rotor happens to sit in sector 4 or 5 at power-up, interprets the difference as an electrical underflow and starts with electric_rotations = -1 (2 of the 6 power-up positions). The angle and the accumulated full rotations are offset by one electrical rotation until something resets them. Instead of relying on updateState() the first time, the current hall state is adopted directly. --- src/sensors/HallSensor.cpp | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 64c5e48c1..2f4c95dcb 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 @@ -156,11 +169,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(); From bef43123ac109d1e4b261b36dae9d1400192a980 Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:31 +0200 Subject: [PATCH 07/14] fix(magnetic sensor spi): the data mask must not be a static local `const static word data_mask` is initialised once and then reused by every MagneticSensorSPI instance, so a second sensor with a different bit resolution gets the mask (and therefore the angle) of the first one. Two sensors of different resolution on the same MCU silently read wrong angles. --- src/sensors/MagneticSensorSPI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 } From 37c1e7889bb0abf5bf6846d7c30a697d8578d16a Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:31 +0200 Subject: [PATCH 08/14] fix(magnetic sensor analog): remove the offset introduced by min_raw_count The constructor stores min_raw_count and computes cpr = max_raw_count - min_raw_count, but getSensorAngle() divided the raw reading by cpr without subtracting the minimum. The reported angle therefore has a constant offset of min_raw_count/(max-min)*2*PI - about 5 degrees for the 14..1020 range used in the examples and docs - and the span is still exactly 2*PI, which is why it looks almost right. The docs describe min_raw_count as 'the smallest expected reading' and warn that getting it wrong causes a click per revolution, i.e. it is meant to be removed. --- src/sensors/MagneticSensorAnalog.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 64f5add71c4b00e1728f50d8a136dc6cb8313ceb Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:32 +0200 Subject: [PATCH 09/14] fix(magnetic sensor i2c): mask and shift the left-aligned LSB bits correctly For a left-aligned sensor the remaining bits sit in the *upper* part of the low byte, so both the mask and the shift have to account for the unused low bits. The constructor applied the right-aligned mask (0x3F for 6 remaining bits) and then shifted the result right by 8-lsb_used, so it kept the status/parity bits and dropped the real data - the angle is wrong for every sensor configured through this constructor. The preset configurations in the header (e.g. MT6701 with lsb_mask 0xFC, lsb_shift 2) show the intended convention. --- src/sensors/MagneticSensorI2C.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) 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); From c42f102936b3678f3fd00d99cac0299a94eefa45 Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:32 +0200 Subject: [PATCH 10/14] fix(trapezoid commutation): non-centred modulation produced no output for Uq < 0 The trapezoid branches used `center = Uq` for non-centred modulation, which works for Uq > 0 but makes all three phase voltages <= 0 for Uq < 0; setPwm() clamps them to 0, so the motor gets no voltage at all in one direction. The sine/SVPWM branches handle this by shifting the phases up by their minimum, which is what non-centred modulation means for every mode. Using the same min-clamp for the trapezoid modes is exactly equivalent for Uq > 0 (every sector map contains a -1, so min = -Uq and the result is map*Uq + Uq as before) and gives the correct shifted waveform for Uq < 0. --- src/BLDCMotor.cpp | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) 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 : From fbe897452a283b2bfeda114f6693cfd8518bb95d Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:32 +0200 Subject: [PATCH 11/14] fix(open loop): store the signed shaft velocity in angleOpenloop() angleOpenloop() moves in both directions but always stored a positive shaft_velocity, while velocityOpenloop() stores the signed value. Anything using shaft_velocity (monitoring, and the back-EMF term of estimated_current torque control) therefore sees a positive speed while moving backwards. --- src/common/base_classes/FOCMotor.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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; From a3811bdd24f93ad28b238385d8982b138d4a87eb Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:32 +0200 Subject: [PATCH 12/14] fix(magnetic sensor pwm): scale the pulseIn() timeout with the PWM frequency The frequency-aware constructor documents the AS5600 PWM modes (115/230/460/920 Hz) and computes the raw counts from them, but left the read timeout at the default 1200 us. One period at 115 Hz is ~8.7 ms and the high pulse can be ~8.4 ms, so pulseIn() times out, returns 0 and the reported angle sticks at min_raw_count: a silently dead sensor for an officially supported configuration. Only 920 Hz (max pulse ~1.05 ms) fitted into the old timeout. The timeout is now 1.2 periods, i.e. ~1.2 ms at 920 Hz (compatible with the behaviour before) and ~10 ms at 115 Hz. --- src/sensors/MagneticSensorPWM.cpp | 5 +++++ 1 file changed, 5 insertions(+) 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(); } From a408f7eb2e9530457aa86f52250fbf502b1ac48a Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:33 +0200 Subject: [PATCH 13/14] fix(foc utils): restore the FLT_MIN guard in _atan2() The comment says 'inject FLT_MIN in denominator to avoid division by zero' but the term is missing (it was dropped from the ODrive original), so _atan2(0,0) is 0/0 and returns NaN instead of 0. Restores the original guard, which only affects the degenerate (0,0) case. --- src/common/foc_utils.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 From 3759fd72d01c08a0ec70b1349b1e9070cfc36d16 Mon Sep 17 00:00:00 2001 From: stijncarelsbergh Date: Mon, 5 Oct 2026 12:11:33 +0200 Subject: [PATCH 14/14] fix(hall sensor): read direction inside the critical section in getVelocity() direction is written by the interrupt handler (updateState()) and was read after interrupts() had been re-enabled, so a sector change in between mixes the old pulse timing with the new direction and produces one velocity sample with the wrong sign and magnitude. Sampled together with the pulse variables instead. --- src/sensors/HallSensor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 2f4c95dcb..18a25be86 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -143,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); } }