sensor precision fix, part 2, sensor changes

Author: runger1101001

src/sensors/Encoder.cpp

@@ -102,8 +102,25 @@ void Encoder::handleIndex() {
 	Shaft angle calculation
 */
 float Encoder::getAngle(){
+  return  _2PI * (pulse_counter % (int)cpr);
+}
+
+float Encoder::getPosition(){
   return  _2PI * (pulse_counter) / ((float)cpr);
 }
+double Encoder::getPrecisePosition(){
+  return  _2PI * (pulse_counter) / ((double)cpr);
+}
+int32_t Encoder::getFullRotations(){
+  return  pulse_counter / (int)cpr;
+}
+float Encoder::getShaftAngle(){
+  return  getAngle();
+}
+
+
+
+
 /*
   Shaft velocity calculation
   function using mixed time and frequency measurement technique

src/sensors/Encoder.h

@@ -60,9 +60,14 @@ class Encoder: public Sensor{
 
     // Abstract functions of the Sensor class implementation
     /** get current angle (rad) */
+    float getShaftAngle() override;
     float getAngle() override;
     /**  get current angular velocity (rad/s) */
     float getVelocity() override;
+    float getPosition() override;
+    double getPrecisePosition() override;
+    int32_t getFullRotations() override;
+
     /**
      * returns 0 if it does need search for absolute zero
      * 0 - encoder without index 

src/sensors/HallSensor.cpp

@@ -96,22 +96,47 @@ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) {
 	Shaft angle calculation
 */
 float HallSensor::getAngle() {
-  return ((electric_rotations * 6 + electric_sector) / cpr) * _2PI ;
+  return getShaftAngle();
 }
 
 /*
   Shaft velocity calculation
   function using mixed time and frequency measurement technique
 */
 float HallSensor::getVelocity(){
-  if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old
+  if (pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old
     return 0;
   } else {
-    return direction * (_2PI / cpr) / (pulse_diff / 1000000.0);
+    return direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0);
   }
 
 }
 
+
+
+float HallSensor::getShaftAngle() {
+  return (float)((electric_rotations * 6 + electric_sector) % cpr) * _2PI ;
+}
+
+
+float HallSensor::getPosition() {
+  return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ;
+}
+
+
+double HallSensor::getPrecisePosition() {
+  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(){
@@ -149,4 +174,3 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){
   if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE);
   if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE);
 }
-

src/sensors/HallSensor.h

@@ -50,13 +50,17 @@ class HallSensor: public Sensor{
 
     // HallSensor configuration
     Pullup pullup; //!< Configuration parameter internal or external pullups
-    float cpr;//!< HallSensor cpr number
+    int cpr;//!< HallSensor cpr number
 
     // Abstract functions of the Sensor class implementation
     /** get current angle (rad) */
+    float getShaftAngle() override;
     float getAngle() override;
     /**  get current angular velocity (rad/s) */
     float getVelocity() override;
+    float getPosition() override;
+    double getPrecisePosition() override;
+    int32_t getFullRotations() override;
 
     // whether last step was CW (+1) or CCW (-1).  
     Direction direction;
@@ -77,7 +81,7 @@ class HallSensor: public Sensor{
     Direction decodeDirection(int oldState, int newState);
     void updateState();
 
-    volatile long pulse_timestamp;//!< last impulse timestamp in us
+    volatile unsigned long pulse_timestamp;//!< last impulse timestamp in us
     volatile int A_active; //!< current active states of A channel
     volatile int B_active; //!< current active states of B channel
     volatile int C_active; //!< current active states of C channel

src/sensors/MagneticSensorAnalog.cpp

@@ -23,47 +23,15 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun
 
 
 void MagneticSensorAnalog::init(){
-
-	// velocity calculation init
-	angle_prev = 0;
-	velocity_calc_timestamp = _micros(); 
-
-	// full rotations tracking number
-	full_rotation_offset = 0;
-	raw_count_prev = getRawCount();  
+	raw_count = getRawCount();  
 }
 
 //  Shaft angle calculation
 //  angle is in radians [rad]
-float MagneticSensorAnalog::getAngle(){
+float MagneticSensorAnalog::getShaftAngle(){
   // raw data from the sensor
-  raw_count = getRawCount(); 
-
-  int delta = raw_count - raw_count_prev;
-  // if overflow happened track it as full rotation
-  if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; 
-  
-  float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI;
-
-  // calculate velocity here 
-  long now = _micros();
-  float Ts = ( now - velocity_calc_timestamp)*1e-6;
-  // quick fix for strange cases (micros overflow)
-  if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; 
-  velocity = (angle - angle_prev)/Ts;
-
-  // save variables for future pass
-  raw_count_prev = raw_count;
-  angle_prev = angle;
-  velocity_calc_timestamp = now;
-
-  return angle;
-}
-
-// Shaft velocity calculation
-float MagneticSensorAnalog::getVelocity(){
-  // TODO: Refactor?: to avoid angle being called twice, velocity is pre-calculted during getAngle
-  return velocity;
+  raw_count = getRawCount();   
+  return ( (float) (raw_count) / (float)cpr) * _2PI;
 }
 
 // function reading the raw counter of the magnetic sensor

src/sensors/MagneticSensorAnalog.h

@@ -29,10 +29,7 @@ class MagneticSensorAnalog: public Sensor{
 
     // implementation of abstract functions of the Sensor class
     /** get current angle (rad) */
-    float getAngle() override;
-    /** get current angular velocity (rad/s) **/
-    float getVelocity() override;
- 
+    float getShaftAngle() override;
     /** raw count (typically in range of 0-1023), useful for debugging resolution issues */
     int raw_count;
 
@@ -48,16 +45,6 @@ class MagneticSensorAnalog: public Sensor{
      */
     int getRawCount();
 
-    // total angle tracking variables
-    float full_rotation_offset; //!<number of full rotations made
-    int raw_count_prev; //!< angle in previous position calculation step
-
-    // velocity calculation variables
-    float angle_prev; //!< angle in previous velocity calculation step
-    long velocity_calc_timestamp; //!< last velocity calculation timestamp
-    float velocity;
-    
-
 };
 
 

src/sensors/MagneticSensorPWM.cpp

@@ -26,47 +26,16 @@ void MagneticSensorPWM::init(){
 
     // initial hardware
     pinMode(pinPWM, INPUT);
-
-    // velocity calculation init
-    angle_prev = 0;
-    velocity_calc_timestamp = _micros();
-
-    // full rotations tracking number
-    full_rotation_offset = 0;
-    raw_count_prev = getRawCount();
+    raw_count = getRawCount();
 }
 
 // get current angle (rad) 
-float MagneticSensorPWM::getAngle(){
-
+float MagneticSensorPWM::getShaftAngle(){
     // raw data from sensor
     raw_count = getRawCount();
-    
-    int delta = raw_count - raw_count_prev;
-    // if overflow happened track it as full rotation
-    if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI;
-
-    float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI;
-
-    // calculate velocity here
-    long now = _micros();
-    float Ts = (now - velocity_calc_timestamp)*1e-6;
-    // quick fix for strange cases (micros overflow)
-    if(Ts <= 0 || Ts > 0.5) Ts = 1e-3;
-    velocity = (angle - angle_prev)/Ts;
-
-    // save variables for future pass
-    raw_count_prev = raw_count;
-    angle_prev = angle;
-    velocity_calc_timestamp = now;
-
-    return angle;
+    return( (float) (raw_count) / (float)cpr) * _2PI;
 }
 
-//  get velocity (rad/s)
-float MagneticSensorPWM::getVelocity(){
-    return velocity;
-}
 
 // read the raw counter of the magnetic sensor
 int MagneticSensorPWM::getRawCount(){

src/sensors/MagneticSensorPWM.h

@@ -23,9 +23,7 @@ class MagneticSensorPWM: public Sensor{
     int pinPWM;
 
     // get current angle (rad)
-    float getAngle() override;
-    // get current angular velocity (rad/s)
-    float getVelocity() override;
+    float getShaftAngle() override;
 
     // pwm handler
     void handlePWM();
@@ -50,15 +48,6 @@ class MagneticSensorPWM: public Sensor{
      */
     int getRawCount();
 
-    // total angle tracking variables
-    float full_rotation_offset; //!<number of full rotations made
-    int raw_count_prev; //!< angle in previous position calculation step
-
-    // velocity calculation variables
-    float angle_prev; //!< angle in previous velocity calculation step
-    long velocity_calc_timestamp; //!< last velocity calculation timestamp
-    float velocity;
-
     // time tracking variables
     unsigned long last_call_us;
     // unsigned long pulse_length_us;