From 5e35fb8d0d330cab75c40994012aac307d9c4636 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Mon, 22 May 2023 01:24:37 -0500 Subject: [PATCH 1/3] Fix for issue #270 Overhaul of interrupt-based sensors to eliminate bad readings due to interrupts modifying variables in the middle of get functions. All sensor classes are now required to use the base class state variables, and do their angle reading once per frame in the update() function rather than using the virtual get functions to return real-time updated readings. I have chosen to make an exception for getVelocity() and allow it to access volatile variables as well, although it may be better to merge it into update() instead. Interrupts should be disabled/enabled as necessary in the update() and getVelocity() functions, and preferably nowhere else. The Arduino library provides no mechanism to restore the previous state of interrupts, so use of the unconditional enable should be kept to as few locations as possible so we can be reasonably sure that it won't be called at a time when interrupts should remain disabled. Additional changes to specific sensors: HallSensor: Removed the max_velocity variable because it was a quick fix for bad velocity readings that were coming from this interrupt problem, which should no longer occur. Also changed the condition for "velocity too old" from (_micros() - pulse_timestamp) > pulse_diff to (_micros() - pulse_timestamp) > pulse_diff*2, because any deceleration would cause inappropriate reporting of zero velocity. MagneticSensorPWM: Unrelated to the interrupt problem, timestamp is now saved from the rising edge of the PWM pulse because that's when the angle was sampled, and communicating it takes a significant and variable amount of time. This gives more accurate velocity calculations, and will allow extrapolating accurate angles using the new SmoothingSensor class. --- src/common/base_classes/Sensor.h | 1 + src/sensors/Encoder.cpp | 28 ++++++++---------- src/sensors/HallSensor.cpp | 47 ++++++++++--------------------- src/sensors/HallSensor.h | 6 ++-- src/sensors/MagneticSensorPWM.cpp | 17 ++++++++++- src/sensors/MagneticSensorPWM.h | 4 +++ 6 files changed, 50 insertions(+), 53 deletions(-) diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index b8834e8e..c77eb911 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -42,6 +42,7 @@ enum Pullup : uint8_t { * */ class Sensor{ + friend class SmoothingSensor; public: /** * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 96057d82..2e4a70da 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -99,29 +99,21 @@ void Encoder::handleIndex() { } +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void Encoder::update() { - // do nothing for Encoder + noInterrupts(); + // TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost + full_rotations = pulse_counter / (int)cpr; + angle_prev = _2PI * ((pulse_counter) % ((int)cpr)) / ((float)cpr); + angle_prev_ts = pulse_timestamp; + interrupts(); } /* Shaft angle calculation */ float Encoder::getSensorAngle(){ - return getAngle(); -} -// TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost -float Encoder::getMechanicalAngle(){ - return _2PI * ((pulse_counter) % ((int)cpr)) / ((float)cpr); -} - -float Encoder::getAngle(){ - return _2PI * (pulse_counter) / ((float)cpr); -} -double Encoder::getPreciseAngle(){ - return _2PI * (pulse_counter) / ((double)cpr); -} -int32_t Encoder::getFullRotations(){ - return pulse_counter / (int)cpr; + return _2PI * (pulse_counter) / ((float)cpr); } @@ -131,6 +123,8 @@ int32_t Encoder::getFullRotations(){ function using mixed time and frequency measurement technique */ float Encoder::getVelocity(){ + // Make sure no interrupts modify the state variables in the middle of these calculations + noInterrupts(); // timestamp long timestamp_us = _micros(); // sampling time calculation @@ -162,6 +156,8 @@ float Encoder::getVelocity(){ // save velocity calculation variables prev_Th = Th; prev_pulse_counter = pulse_counter; + // Re-enable interrupts (ideally this would restore to the previous state rather than unconditionally enabling) + interrupts(); return velocity; } diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 7d1d9f64..32ed54e2 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -94,8 +94,13 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { -float HallSensor::getSensorAngle() { - return getAngle(); +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. +void HallSensor::update() { + noInterrupts(); + angle_prev = ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ; + full_rotations = (int32_t)((electric_rotations * 6 + electric_sector) / cpr); + angle_prev_ts = pulse_timestamp; + interrupts(); } @@ -104,8 +109,8 @@ float HallSensor::getSensorAngle() { Shaft angle calculation TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost */ -float HallSensor::getMechanicalAngle() { - return ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ; +float HallSensor::getSensorAngle() { + return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; } /* @@ -113,38 +118,16 @@ float HallSensor::getMechanicalAngle() { function using mixed time and frequency measurement technique */ float HallSensor::getVelocity(){ - long last_pulse_diff = pulse_diff; - if (last_pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > last_pulse_diff) ) { // last velocity isn't accurate if too old - return 0; - } else { - float vel = direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f); - // quick fix https://github.com/simplefoc/Arduino-FOC/issues/192 - if(vel < -velocity_max || vel > velocity_max) vel = 0.0f; //if velocity is out of range then make it zero - return vel; - } - + noInterrupts(); + float vel = 0; + if (pulse_diff != 0 && ((long)(_micros() - pulse_timestamp) < pulse_diff*2) ) // last velocity isn't accurate if too old + vel = direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f); + interrupts(); + return vel; } -float HallSensor::getAngle() { - return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; -} - - -double HallSensor::getPreciseAngle() { - return ((double)(electric_rotations * 6 + electric_sector) / (double)cpr) * (double)_2PI ; -} - - -int32_t HallSensor::getFullRotations() { - return (int32_t)((electric_rotations * 6 + electric_sector) / cpr); -} - - - - - // HallSensor initialisation of the hardware pins // and calculation variables void HallSensor::init(){ diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index d6b4493a..ad50c7d5 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -53,14 +53,12 @@ class HallSensor: public Sensor{ int cpr;//!< HallSensor cpr number // Abstract functions of the Sensor class implementation + /** Interrupt-safe update */ + void update() override; /** get current angle (rad) */ float getSensorAngle() override; - float getMechanicalAngle() override; - float getAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - double getPreciseAngle() override; - int32_t getFullRotations() override; // whether last step was CW (+1) or CCW (-1). Direction direction; diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 02798ad3..3b0cd9aa 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -56,10 +56,21 @@ void MagneticSensorPWM::init(){ // initial hardware pinMode(pinPWM, INPUT); raw_count = getRawCount(); + pulse_timestamp = _micros(); this->Sensor::init(); // call base class init } +// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. +void MagneticSensorPWM::update() { + if (is_interrupt_based) + noInterrupts(); + Sensor::update(); + angle_prev_ts = pulse_timestamp; // Timestamp of actual sample, before the time-consuming PWM communication + if (is_interrupt_based) + interrupts(); +} + // get current angle (rad) float MagneticSensorPWM::getSensorAngle(){ // raw data from sensor @@ -73,6 +84,7 @@ float MagneticSensorPWM::getSensorAngle(){ // read the raw counter of the magnetic sensor int MagneticSensorPWM::getRawCount(){ if (!is_interrupt_based){ // if it's not interrupt based read the value in a blocking way + pulse_timestamp = _micros(); // ideally this should be done right at the rising edge of the pulse pulse_length_us = pulseIn(pinPWM, HIGH); } return pulse_length_us; @@ -84,7 +96,10 @@ void MagneticSensorPWM::handlePWM() { unsigned long now_us = _micros(); // if falling edge, calculate the pulse length - if (!digitalRead(pinPWM)) pulse_length_us = now_us - last_call_us; + if (!digitalRead(pinPWM)) { + pulse_length_us = now_us - last_call_us; + pulse_timestamp = last_call_us; // angle was sampled at the rising edge of the pulse, so use that timestamp + } // save the currrent timestamp for the next call last_call_us = now_us; diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index 77e82459..48492c84 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -32,6 +32,9 @@ class MagneticSensorPWM: public Sensor{ void init(); int pinPWM; + + // Interrupt-safe update + void update() override; // get current angle (rad) float getSensorAngle() override; @@ -62,6 +65,7 @@ class MagneticSensorPWM: public Sensor{ // time tracking variables unsigned long last_call_us; // unsigned long pulse_length_us; + unsigned long pulse_timestamp; }; From 98a10e4e96e0473784997f4ebfbac8d3d3c780da Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Mon, 22 May 2023 03:48:40 -0500 Subject: [PATCH 2/3] Reduced interrupt blocking time Changed interrupt-based sensors to only copy volatile variables during interrupt-blocking sections, and do computations with them after re-enabling interrupts. --- src/sensors/Encoder.cpp | 21 ++++++++++++--------- src/sensors/HallSensor.cpp | 21 +++++++++++++-------- 2 files changed, 25 insertions(+), 17 deletions(-) diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 2e4a70da..fa4b8e73 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -101,12 +101,14 @@ void Encoder::handleIndex() { // Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void Encoder::update() { + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed noInterrupts(); - // TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost - full_rotations = pulse_counter / (int)cpr; - angle_prev = _2PI * ((pulse_counter) % ((int)cpr)) / ((float)cpr); angle_prev_ts = pulse_timestamp; + long copy_pulse_counter = pulse_counter; interrupts(); + // TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost + full_rotations = copy_pulse_counter / (int)cpr; + angle_prev = _2PI * ((copy_pulse_counter) % ((int)cpr)) / ((float)cpr); } /* @@ -123,8 +125,11 @@ float Encoder::getSensorAngle(){ function using mixed time and frequency measurement technique */ float Encoder::getVelocity(){ - // Make sure no interrupts modify the state variables in the middle of these calculations + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed noInterrupts(); + long copy_pulse_counter = pulse_counter; + long copy_pulse_timestamp = pulse_timestamp; + interrupts(); // timestamp long timestamp_us = _micros(); // sampling time calculation @@ -133,8 +138,8 @@ float Encoder::getVelocity(){ if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // time from last impulse - float Th = (timestamp_us - pulse_timestamp) * 1e-6f; - long dN = pulse_counter - prev_pulse_counter; + float Th = (timestamp_us - copy_pulse_timestamp) * 1e-6f; + long dN = copy_pulse_counter - prev_pulse_counter; // Pulse per second calculation (Eq.3.) // dN - impulses received @@ -155,9 +160,7 @@ float Encoder::getVelocity(){ prev_timestamp_us = timestamp_us; // save velocity calculation variables prev_Th = Th; - prev_pulse_counter = pulse_counter; - // Re-enable interrupts (ideally this would restore to the previous state rather than unconditionally enabling) - interrupts(); + prev_pulse_counter = copy_pulse_counter; return velocity; } diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 32ed54e2..38767d59 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -96,11 +96,14 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { // Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables. void HallSensor::update() { + // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed noInterrupts(); - angle_prev = ((float)((electric_rotations * 6 + electric_sector) % cpr) / (float)cpr) * _2PI ; - full_rotations = (int32_t)((electric_rotations * 6 + electric_sector) / cpr); angle_prev_ts = pulse_timestamp; + long last_electric_rotations = electric_rotations; + int8_t last_electric_sector = electric_sector; interrupts(); + angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ; + full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr); } @@ -119,14 +122,16 @@ float HallSensor::getSensorAngle() { */ float HallSensor::getVelocity(){ noInterrupts(); - float vel = 0; - if (pulse_diff != 0 && ((long)(_micros() - pulse_timestamp) < pulse_diff*2) ) // last velocity isn't accurate if too old - vel = direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f); + long last_pulse_timestamp = pulse_timestamp; + long last_pulse_diff = pulse_diff; interrupts(); - return vel; -} - + 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); + } +} // HallSensor initialisation of the hardware pins // and calculation variables From b03f8a91dfc78cedbaabcf88cc70e6f65d064165 Mon Sep 17 00:00:00 2001 From: dekutree64 <54822734+dekutree64@users.noreply.github.com> Date: Mon, 22 May 2023 04:41:17 -0500 Subject: [PATCH 3/3] Forgot to remove function headers --- src/sensors/Encoder.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index 91427a83..af6a5ab6 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -61,12 +61,8 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ float getSensorAngle() override; - float getMechanicalAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; - float getAngle() override; - double getPreciseAngle() override; - int32_t getFullRotations() override; virtual void update() override; /**