add a (optional) init method to sensor base class

Author: runger1101001

src/common/base_classes/Sensor.cpp

@@ -30,6 +30,19 @@ float Sensor::getVelocity() {
     return vel;
 }
 
+void Sensor::init() {
+    // initialize all the internal variables of Sensor to ensure a "smooth" startup (without a 'jump' from zero)
+    getSensorAngle(); // call once
+    delayMicroseconds(1);
+    vel_angle_prev = getSensorAngle(); // call again
+    vel_angle_prev_ts = _micros();
+    delay(1);
+    getSensorAngle(); // call once
+    delayMicroseconds(1);
+    angle_prev = getSensorAngle(); // call again
+    angle_prev_ts = _micros();
+}
+
 
 float Sensor::getShaftAngle() {
     return angle_prev;
@@ -57,4 +70,4 @@ int32_t Sensor::getFullRotations() {
 
 int Sensor::needsSearch() {
     return 0; // default false
-}
+}

src/common/base_classes/Sensor.h

@@ -113,6 +113,13 @@ class Sensor{
          * Use updateSensor() when calling from outside code.
          */
         virtual float getSensorAngle()=0;
+        /**
+         * Call Sensor::init() from your sensor subclass's init method if you want smoother startup
+         * The base class init() method calls getSensorAngle() several times to initialize the internal fields
+         * to current values, ensuring there is no discontinuity ("jump from zero") during the first calls
+         * to sensor.getAngle() and sensor.getVelocity()
+         */
+        virtual void init();
 
         // velocity calculation variables
         float angle_prev=0; // result of last call to getSensorAngle(), used for full rotations and velocity