Merge pull request #195 from runger1101001/velocity_change

change getVelocity() to remember velocity

Author: runger1101001

src/common/base_classes/Sensor.cpp

@@ -2,7 +2,7 @@
 #include "../foc_utils.h"
 #include "../time_utils.h"
 
-// TODO add an init method to make the startup smoother by initializing internal variables to current values rather than 0
+
 
 void Sensor::update() {
     float val = getSensorAngle();
@@ -18,17 +18,17 @@ void Sensor::update() {
 float Sensor::getVelocity() {
     // calculate sample time
     float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;
-    // quick fix for strange cases (micros overflow)
-    if(Ts <= 0) Ts = 1e-3f;
-    // velocity calculation
-    float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;    
-    // save variables for future pass
+    if (Ts<minDeltaT) return velocity; // don't update velocity if deltaT is too small
+
+    velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;
     vel_angle_prev = angle_prev;
     vel_full_rotations = full_rotations;
     vel_angle_prev_ts = angle_prev_ts;
-    return vel;
+    return velocity;
 }
 
+
+
 void Sensor::init() {
     // initialize all the internal variables of Sensor to ensure a "smooth" startup (without a 'jump' from zero)
     getSensorAngle(); // call once

src/common/base_classes/Sensor.h

@@ -101,6 +101,12 @@ class Sensor{
          * 1 - ecoder with index (with index not found yet)
          */
         virtual int needsSearch();
+
+        /**
+         * Minimum time between updates to velocity. If time elapsed is lower than this, the velocity is not updated.
+         */
+        float minDeltaT = 0.000100; // default is 100 microseconds, or 10kHz
+
     protected:
         /** 
          * Get current shaft angle from the sensor hardware, and 
@@ -120,9 +126,10 @@ class Sensor{
         virtual void init();
 
         // velocity calculation variables
-        float angle_prev=0; // result of last call to getSensorAngle(), used for full rotations and velocity
+        float velocity=0.0f;
+        float angle_prev=0.0f; // result of last call to getSensorAngle(), used for full rotations and velocity
         long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity
-        float vel_angle_prev=0; // angle at last call to getVelocity, used for velocity
+        float vel_angle_prev=0.0f; // angle at last call to getVelocity, used for velocity
         long vel_angle_prev_ts=0; // last velocity calculation timestamp
         int32_t full_rotations=0; // full rotation tracking
         int32_t vel_full_rotations=0; // previous full rotation value for velocity calculation