Clean up

Author: Candas1

src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp

@@ -24,10 +24,6 @@ void MXLEMMINGObserverSensor::update() {
   // Update sensor, with optional downsampling of update rate
   if (sensor_cnt++ < sensor_downsample) return;
 
-  // Estimate the BEMF and exit if it's below the threshold 
-  float bemf = _motor.voltage.q - _motor.phase_resistance * _motor.current.q; 
-  if (abs(bemf) < bemf_threshold) return;
-
   sensor_cnt = 0;
 
   // read current phase currents
@@ -39,9 +35,9 @@ void MXLEMMINGObserverSensor::update() {
   // get current timestamp
   long now_us = _micros();
   // calculate the sample time from last call
-  float Ts = (now_us - angle_prev_ts) * 1e-6f;
+  float dt = (now_us - angle_prev_ts) * 1e-6f;
   // quick fix for strange cases (micros overflow + timestamp not defined)
-  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+  if(dt <= 0 || dt > 0.5f) dt = 1e-3f;
 
   // This work deviates slightly from the BSD 3 clause licence.
   // The work here is entirely original to the MESC FOC project, and not based
@@ -53,10 +49,13 @@ void MXLEMMINGObserverSensor::update() {
   // to David Molony as the original author must be noted.
 
   // MXLEMMING Flux Observer
-  flux_alpha = _constrain( flux_alpha + (_motor.Ualpha - _motor.phase_resistance * ABcurrent.alpha) * Ts -
-        _motor.phase_inductance * (ABcurrent.alpha - i_alpha_prev),-flux_linkage, flux_linkage);
-  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);
+  float resistive_term_a =  _motor.phase_resistance * ABcurrent.alpha;
+  float resistive_term_b =  _motor.phase_resistance * ABcurrent.beta;
+  float inductive_term_a = _motor.phase_inductance * (ABcurrent.alpha - i_alpha_prev);
+  float inductive_term_b = _motor.phase_inductance * (ABcurrent.beta  - i_beta_prev);
+
+  flux_alpha = _constrain( flux_alpha + (_motor.Ualpha - resistive_term_a) * dt - inductive_term_a ,-flux_linkage, flux_linkage);
+  flux_beta  = _constrain( flux_beta  + (_motor.Ubeta  - resistive_term_b) * dt - inductive_term_b ,-flux_linkage, flux_linkage);
 
   // Calculate electrical angle
   electrical_angle = _normalizeAngle(_atan2(flux_beta,flux_alpha));

src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h

@@ -37,7 +37,6 @@ class MXLEMMINGObserverSensor : public Sensor
     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
 
   protected:    
     const FOCMotor& _motor;

src/encoders/MXLEMMING_observer/README.md

@@ -16,6 +16,8 @@ It will not track the position if any of those parameters are missing.
 
 The KV rating and pole pairs parameters are used to derive the motor flux linkage which is key for the flux observer to run well.
 ```
+#include <encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h>
+
 BLDCMotor motor = BLDCMotor(15, 0.1664, 17.0, 0.00036858); // Hoverboard Motor
 MXLEMMINGObserverSensor sensor = MXLEMMINGObserverSensor(motor);
 ```
@@ -31,10 +33,6 @@ motor.sensor_direction= Direction::CW;
 motor.zero_electric_angle = 0;
 ```
 
-### Bemf Threshold
-The sensor also has a bemf_threshold parameter (0 by default) that prevents the flux observer from tracking if the estimated bemf is not high enough.
-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 flux observer is calculating the electrical angle directly, but SimpleFOC needs to derive the electrical angle from the sensor angle for the FOC calculation