Merge pull request #276 from dekutree64/dev

Fix for issue #270

Author: runger1101001

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,23 @@ void Encoder::handleIndex() {
 }
 
 
+// Sensor update function. Safely copy volatile interrupt variables into Sensor base class state variables.
 void Encoder::update() {
-    // do nothing for Encoder
+  // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed
+  noInterrupts();
+  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);
 }
 
 /*
 	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 +125,11 @@ int32_t Encoder::getFullRotations(){
   function using mixed time and frequency measurement technique
 */
 float Encoder::getVelocity(){
+  // 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
@@ -139,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
@@ -161,7 +160,7 @@ float Encoder::getVelocity(){
   prev_timestamp_us = timestamp_us;
   // save velocity calculation variables
   prev_Th = Th;
-  prev_pulse_counter = pulse_counter;
+  prev_pulse_counter = copy_pulse_counter;
   return velocity;
 }
 

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;
 
     /**

src/sensors/HallSensor.cpp

@@ -94,8 +94,16 @@ 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() {
+  // Copy volatile variables in minimal-duration blocking section to ensure no interrupts are missed
+  noInterrupts();
+  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);
 }
 
 
@@ -104,47 +112,27 @@ 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(){
+  noInterrupts();
+  long last_pulse_timestamp = pulse_timestamp;
   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
+  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 {
-    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;
+    return direction * (_2PI / (float)cpr) / (last_pulse_diff / 1000000.0f);
   }
 
 }
 
-
-
-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;
 
 
 };