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.

Author: dekutree64

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

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;
 }
 

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,47 +109,25 @@ 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 ;
 }
 
 /*
   Shaft velocity calculation
   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(){

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;

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;

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;
 
 
 };