Switch angle sensors to use integer instead of floating point

Author: Moddingear

src/common/base_classes/FOCMotor.cpp

@@ -77,7 +77,11 @@ float FOCMotor::shaftVelocity() {
 float FOCMotor::electricalAngle(){
   // if no sensor linked return previous value ( for open loop )
   if(!sensor) return electrical_angle;
-  return  _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle()  - zero_electric_angle );
+  return  _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() 
+    #ifdef INTEGER_ANGLE
+    * _2PI / sensor->getStepsPerRevolution()
+    #endif
+    - zero_electric_angle );
 }
 
 /**
@@ -289,8 +293,16 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
        * We then report the one closest to the actual value. This could be useful if the zero search method is not reliable enough (eg. high pole count).
       */
 
-      float estimated_zero_electric_angle_A = _normalizeAngle(  (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle);
-      float estimated_zero_electric_angle_B = _normalizeAngle(  (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle + _PI);
+      float estimated_zero_electric_angle_A = _normalizeAngle(  (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle()
+        #ifdef INTEGER_ANGLE
+        * _2PI / sensor->getStepsPerRevolution()
+        #endif
+        - d_electrical_angle);
+      float estimated_zero_electric_angle_B = _normalizeAngle(  (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle()
+        #ifdef INTEGER_ANGLE
+        * _2PI / sensor->getStepsPerRevolution()
+        #endif
+        - d_electrical_angle + _PI);
       float estimated_zero_electric_angle = 0.0f;
       if (fabsf(estimated_zero_electric_angle_A - zero_electric_angle) < fabsf(estimated_zero_electric_angle_B - zero_electric_angle))
       {

src/common/base_classes/Sensor.cpp

@@ -5,48 +5,69 @@
 
 
 void Sensor::update() {
-    float val = getSensorAngle();
+    angle_type val = getSensorAngle();
     if (val<0) // sensor angles are strictly non-negative. Negative values are used to signal errors.
         return; // TODO signal error, e.g. via a flag and counter
     angle_prev_ts = _micros();
-    float d_angle = val - angle_prev;
-    // if overflow happened track it as full rotation
-    if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 
-    angle_prev = val;
+    setAngleContinuous(val);
 }
 
 
  /** get current angular velocity (rad/s) */
 float Sensor::getVelocity() {
     // calculate sample time
     // if timestamps were unsigned, we could get rid of this section, unsigned overflow handles it correctly
+    #ifdef INTEGER_ANGLE
+    angle_type Ts = angle_prev_ts - vel_angle_prev_ts;
+    #else
     float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f;
+    #endif
+    #if 0
     if (Ts < 0.0f) {    // handle micros() overflow - we need to reset vel_angle_prev_ts
         vel_angle_prev = angle_prev;
+        #ifndef INTEGER_ANGLE
         vel_full_rotations = full_rotations;
+        #endif
         vel_angle_prev_ts = angle_prev_ts;
         return velocity;
     }
+    #endif
     if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small
 
     // Calculate change in angle. Handles `full_rotations` integer wrap-arounds,
     // and avoids float precision loss issues by keeping numbers small.
-    float delta_angle = angle_prev - vel_angle_prev;
+    angle_type delta_angle = angle_prev - vel_angle_prev;
+    #ifndef INTEGER_ANGLE
     const int32_t delta_full_rotations = full_rotations - vel_full_rotations;
     if (delta_full_rotations) {
         delta_angle += delta_full_rotations * _2PI;
     }
+    #endif
 
+    #ifdef INTEGER_ANGLE
+    if (abs(delta_angle) > 0)
+    {
+        velocity = delta_angle * 1000000 * _2PI / (Ts * steps_per_revolution);
+    }
+    else
+    {
+        velocity = 0.0f;
+    }
+    
+    #else
     // floating point equality checks are bad, so instead we check that the angle change is very small
     if (fabsf(delta_angle) > 1e-8f) {
         velocity = delta_angle / Ts;
     } else {
         velocity = 0.0f;
     }
+    #endif
 
     // Always advance the velocity reference sample to avoid stale deltas/time windows.
     vel_angle_prev = angle_prev;
+    #ifndef INTEGER_ANGLE
     vel_full_rotations = full_rotations;
+    #endif
     vel_angle_prev_ts = angle_prev_ts;
 
     return velocity;
@@ -68,30 +89,63 @@ void Sensor::init() {
 }
 
 
-float Sensor::getMechanicalAngle() {
+angle_type Sensor::getMechanicalAngle() {
     return angle_prev;
 }
 
 
 
 float Sensor::getAngle(){
+    #ifdef INTEGER_ANGLE
+    return angle_prev / (float)steps_per_revolution * _2PI;
+    #else
     return (float)full_rotations * _2PI + angle_prev;
+    #endif
 }
 
 
 
 double Sensor::getPreciseAngle() {
+    #ifdef INTEGER_ANGLE
+    return angle_prev / (double)steps_per_revolution * _2PI;
+    #else
     return (double)full_rotations * (double)_2PI + (double)angle_prev;
+    #endif
 }
 
 
 
 int32_t Sensor::getFullRotations() {
+    #ifdef INTEGER_ANGLE
+    return angle_prev/steps_per_revolution;
+    #else
     return full_rotations;
+    #endif
 }
 
 
 
 int Sensor::needsSearch() {
     return 0; // default false
 }
+
+void Sensor::setAngleContinuous(angle_type sensor_angle)
+{
+    #ifdef INTEGER_ANGLE
+    angle_type d_angle = sensor_angle - sensor_angle_prev;
+    if(abs(d_angle) > (steps_per_revolution/2) ) 
+    {
+        d_angle += d_angle > 0 ? -steps_per_revolution : steps_per_revolution;
+    }
+    angle_prev += d_angle;
+    sensor_angle_prev = sensor_angle;
+    #else
+    angle_type d_angle = sensor_angle - angle_prev;
+    if(abs(d_angle) > (0.8f*_2PI) ) 
+    {
+        full_rotations += ( d_angle > 0 ) ? -1 : 1; 
+    }
+    angle_prev = sensor_angle;
+    #endif
+
+}

src/common/base_classes/Sensor.h

@@ -21,6 +21,14 @@ enum Pullup : uint8_t {
     USE_EXTERN = 0x01  //!< Use external pullups
 };
 
+#define INTEGER_ANGLE
+
+#ifdef INTEGER_ANGLE
+using angle_type = int;
+#else
+using angle_type = float;
+#endif
+
 /**
  * Sensor abstract class defintion
  * 
@@ -43,13 +51,15 @@ enum Pullup : uint8_t {
  */
 class Sensor{
 	friend class SmoothingSensor;
+    friend class CalibratedSensor;
+    friend class HysteresisSensor;
     public:
         /**
          * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with
          * the hardware. Base implementation uses the values returned by update() so that 
          * the same values are returned until update() is called again.
          */
-        virtual float getMechanicalAngle();
+        virtual angle_type getMechanicalAngle();
 
         /**
          * Get current position (in rad) including full rotations and shaft angle.
@@ -108,6 +118,10 @@ class Sensor{
          */
         float min_elapsed_time = 0.000100f; // default is 100 microseconds, or 10kHz
 
+        #ifdef INTEGER_ANGLE
+        int getStepsPerRevolution() const {return steps_per_revolution;};
+        #endif
+
     protected:
         /** 
          * Get current shaft angle from the sensor hardware, and 
@@ -117,7 +131,8 @@ class Sensor{
          * Calling this method directly does not update the base-class internal fields.
          * Use update() when calling from outside code.
          */
-        virtual float getSensorAngle()=0;
+        virtual angle_type 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
@@ -126,14 +141,26 @@ class Sensor{
          */
         virtual void init();
 
+        /**
+         * Set angle_prev and full_rotations from a sensor angle, handling sensor overflows as revolutions
+         */
+        void setAngleContinuous(angle_type sensor_angle);
+
         // velocity calculation variables
         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
+        angle_type angle_prev=0.0f; // result of last call to getSensorAngle(), used for full rotations and velocity
+        unsigned long angle_prev_ts=0; // timestamp of last call to getAngle, 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
+        unsigned long vel_angle_prev_ts=0; // last velocity calculation timestamp
+        #ifdef INTEGER_ANGLE
+    private:
+        angle_type sensor_angle_prev = 0;
+    protected:
+        angle_type steps_per_revolution = 2;
+        #else
         int32_t full_rotations=0; // full rotation tracking
         int32_t vel_full_rotations=0; // previous full rotation value for velocity calculation
+        #endif
 };
 
 #endif

src/sensors/Encoder.cpp

@@ -19,6 +19,9 @@ Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){
   pulse_timestamp = 0;
 
   cpr = _ppr;
+  #ifdef INTEGER_ANGLE
+  steps_per_revolution = cpr;
+  #endif
   A_active = 0;
   B_active = 0;
   I_active = 0;
@@ -106,16 +109,25 @@ void Encoder::update() {
   angle_prev_ts = pulse_timestamp;
   long copy_pulse_counter = pulse_counter;
   interrupts();
+  #ifdef INTEGER_ANGLE
+  setAngleContinuous(copy_pulse_counter%steps_per_revolution);
+  #else
   // 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);
+  #endif
+
 }
 
 /*
 	Shaft angle calculation
 */
-float Encoder::getSensorAngle(){
+angle_type Encoder::getSensorAngle(){
+  #ifdef INTEGER_ANGLE
+  return angle_prev % steps_per_revolution;
+  #else
   return _2PI * (pulse_counter) / ((float)cpr);
+  #endif
 }
 
 

src/sensors/Encoder.h

@@ -60,7 +60,7 @@ class Encoder: public Sensor{
 
     // Abstract functions of the Sensor class implementation
     /** get current angle (rad) */
-    float getSensorAngle() override;
+    angle_type getSensorAngle() override;
     /**  get current angular velocity (rad/s) */
     float getVelocity() override;
     virtual void update() override;

src/sensors/GenericSensor.cpp

@@ -6,7 +6,7 @@
   - readCallback - pointer to the function which reads the sensor angle.
 */
 
-GenericSensor::GenericSensor(float (*readCallback)(), void (*initCallback)()){
+GenericSensor::GenericSensor(angle_type (*readCallback)(), void (*initCallback)()){
   // if function provided add it to the 
   if(readCallback != nullptr) this->readCallback = readCallback;
   if(initCallback != nullptr) this->initCallback = initCallback;
@@ -21,6 +21,6 @@ void GenericSensor::init(){
 /*
 	Shaft angle calculation
 */
-float GenericSensor::getSensorAngle(){
+angle_type GenericSensor::getSensorAngle(){
   return this->readCallback();
 }

src/sensors/GenericSensor.h

@@ -14,16 +14,16 @@ class GenericSensor: public Sensor{
      * @param readCallback pointer to the function reading the sensor angle
      * @param initCallback pointer to the function initialising the sensor
     */
-    GenericSensor(float (*readCallback)() = nullptr, void (*initCallback)() = nullptr);
+    GenericSensor(angle_type (*readCallback)() = nullptr, void (*initCallback)() = nullptr);
 
-    float (*readCallback)() = nullptr; //!< function pointer to sensor reading
+    angle_type (*readCallback)() = nullptr; //!< function pointer to sensor reading
     void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation
 
     void init() override;
 
     // Abstract functions of the Sensor class implementation
     /** get current angle (rad) */
-    float getSensorAngle() override;
+    angle_type getSensorAngle() override;
 
 };
 

src/sensors/HallSensor.cpp

@@ -15,6 +15,9 @@ HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){
 
   // hall has 6 segments per electrical revolution
   cpr = _pp * 6;
+  #ifdef INTEGER_ANGLE
+  steps_per_revolution = cpr;
+  #endif
 
   // extern pullup as default
   pullup = Pullup::USE_EXTERN;
@@ -108,8 +111,7 @@ void HallSensor::update() {
   long last_electric_rotations = electric_rotations;
   int8_t last_electric_sector = electric_sector;
   if (use_interrupt) 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);
+  setAngleContinuous((last_electric_rotations * 6 + last_electric_sector) % cpr);
 }
 
 
@@ -118,8 +120,12 @@ void HallSensor::update() {
 	Shaft angle calculation
   TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost
 */
-float HallSensor::getSensorAngle() {
+angle_type HallSensor::getSensorAngle() {
+  #ifdef INTEGER_ANGLE
+  return electric_rotations * 6 + electric_sector;
+  #else
   return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ;
+  #endif
 }
 
 /*

src/sensors/HallSensor.h

@@ -57,7 +57,7 @@ class HallSensor: public Sensor{
     /** Interrupt-safe update */
     void update() override;
     /** get current angle (rad) */
-    float getSensorAngle() override;
+    angle_type getSensorAngle() override;
     /**  get current angular velocity (rad/s) */
     float getVelocity() override;