Cleanup

Author: Candas1

src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp

@@ -58,35 +58,29 @@ void MXLEMMINGObserverSensor::update() {
   flux_beta  = _constrain( flux_beta  + (_motor.Ubeta  - _motor.phase_resistance * ABcurrent.beta)  * Ts -
         _motor.phase_inductance * (ABcurrent.beta  - i_beta_prev) ,-flux_linkage, flux_linkage);
 
-  // Calculate angle
-  float electrical_angle = _normalizeAngle(_atan2(flux_beta,flux_alpha));
+  // Calculate electrical angle
+  electrical_angle = _normalizeAngle(_atan2(flux_beta,flux_alpha));
 
   // Electrical angle difference
-  float d_electrical_angle = 0;
-  if (first){
-    // Skip angle difference calculation the first time
-    first = 0;
-    d_electrical_angle = electrical_angle;
-  }else{
-    d_electrical_angle = electrical_angle - electrical_angle_prev;
-    if(abs(d_electrical_angle) > _2PI * 0.8 ){ //change the  factor based on sample rate can also just use _PI for simplicity 
-      if (d_electrical_angle > 0){
-        d_electrical_angle -= _2PI;
-      }else{
-        d_electrical_angle += _2PI;
-      }
+  float d_electrical_angle = electrical_angle - electrical_angle_prev;
+  if(abs(d_electrical_angle) > _2PI * 0.8 ){ //change the  factor based on sample rate can also just use _PI for simplicity 
+    if (d_electrical_angle > 0){
+      d_electrical_angle -= _2PI;
+    }else{
+      d_electrical_angle += _2PI;
     }
   }
   angle_track += d_electrical_angle;
 
   // Mechanical angle and full_rotations
-  if(abs(angle_track) > _2PI * _motor.pole_pairs){
+  float full_rotation = _2PI * _motor.pole_pairs;
+  if(abs(angle_track) > full_rotation){
     if (angle_track>0){
       full_rotations += 1;
-      angle_track -= _2PI * _motor.pole_pairs;
+      angle_track -= full_rotation;
     }else{
       full_rotations -= 1;
-      angle_track += _2PI * _motor.pole_pairs;
+      angle_track += full_rotation;
     }
   }
   angle_prev = angle_track /_motor.pole_pairs;
@@ -108,4 +102,4 @@ void MXLEMMINGObserverSensor::init(){
 */
 float MXLEMMINGObserverSensor::getSensorAngle(){
   return 0;
-}
+}

src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h

@@ -29,19 +29,19 @@ class MXLEMMINGObserverSensor : public Sensor
     // For sensors with slow communication, use these to poll less often
     unsigned int sensor_downsample = 0; // parameter defining the ratio of downsampling for sensor update
     unsigned int sensor_cnt = 0; // counting variable for downsampling
-    float flux_alpha = 0; // Flux Alpha 
-    float flux_beta = 0; // Flux Beta
+    float flux_alpha   = 0; // Flux Alpha 
+    float flux_beta    = 0; // Flux Beta
     float flux_linkage = 0; // Flux linkage, calculated based on KV and pole number
     float i_alpha_prev = 0; // Previous Alpha current
-    float i_beta_prev = 0; // Previous Beta current
+    float i_beta_prev  = 0; // Previous Beta current
+    float electrical_angle = 0; // Electrical angle
     float electrical_angle_prev = 0; // Previous electrical angle
     float angle_track = 0; // Total Electrical angle
     float bemf_threshold = 0; // Bemf voltage amplitude when the flux observer should start tracking
-    int8_t first = 1; // To skip angle difference calculation the first time
-
+    
   protected:    
     const FOCMotor& _motor;
 
 };
 
-#endif
+#endif

src/encoders/MXLEMMING_observer/README.md

@@ -36,5 +36,6 @@ The sensor also has a bemf_threshold parameter (0 by default) that prevents the
 This can help when starting the motor as the flux observer is not good at tracking the position at low speed.
 
 ### To do:
-The Clarke transform is running both in the loopFOC and in the sensor update now, it can be remove from the sensor when the Alpha and Beta currents will be persisted as a BLDCMotor member.
+- The Clarke transform is running both in the loopFOC and in the sensor update now, it can be remove from the sensor when the Alpha and Beta currents will be persisted as a BLDCMotor member
+- The flux observer is calculating the electrical angle directly, but SimpleFOC needs to derive the electrical angle from the sensor angle for the FOC calculation