From 389285fc53c49720ec17c99eaaf0efc049c650e4 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Tue, 17 Oct 2023 05:54:37 +0200
Subject: [PATCH 01/20] Initial commit

---
 .../flux_observer/FluxObserverSensor.cpp      | 93 +++++++++++++++++++
 .../flux_observer/FluxObserverSensor.h        | 44 +++++++++
 src/encoders/flux_observer/README.md          |  1 +
 3 files changed, 138 insertions(+)
 create mode 100644 src/encoders/flux_observer/FluxObserverSensor.cpp
 create mode 100644 src/encoders/flux_observer/FluxObserverSensor.h
 create mode 100644 src/encoders/flux_observer/README.md

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/flux_observer/FluxObserverSensor.cpp
new file mode 100644
index 0000000..bc8755b
--- /dev/null
+++ b/src/encoders/flux_observer/FluxObserverSensor.cpp
@@ -0,0 +1,93 @@
+#include "FluxObserverSensor.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+
+FluxObserverSensor::FluxObserverSensor(const FOCMotor& m) : _motor(m)
+{
+}
+
+
+void FluxObserverSensor::update() {
+  // Current sense is required for the observer
+  if (!_motor.current_sense) return;
+  
+  // Update sensor, with optional downsampling of update rate
+  if(sensor_cnt++ < sensor_downsample) return;
+  
+  sensor_cnt = 0;
+
+  // read current phase currents
+  PhaseCurrent_s current = _motor.current_sense->getPhaseCurrents();
+
+  // calculate clarke transform
+  float i_alpha, i_beta;
+  if(!current.c){
+      // if only two measured currents
+      i_alpha = current.a;  
+      i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
+  }if(!current.a){
+      // if only two measured currents
+      float a = -current.c - current.b;
+      i_alpha = a;  
+      i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
+  }if(!current.b){
+      // if only two measured currents
+      float b = -current.a - current.c;
+      i_alpha = current.a;  
+      i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b;
+  } else {
+      // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
+      float mid = (1.f/3) * (current.a + current.b + current.c);
+      float a = current.a - mid;
+      float b = current.b - mid;
+      i_alpha = a;
+      i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
+  }
+
+  // Flux linkage observer    
+  float now = _micros();
+  float Ts = ( now - angle_prev_ts) * 1e-6f; 
+  flux_a = _constrain( flux_a + (_motor.ABVoltage.alpha - _motor.phase_resistance * i_alpha) * Ts -
+        _motor.phase_inductance * (i_alpha - i_alpha_prev),-_motor.flux_linkage, _motor.flux_linkage);
+  flux_b = _constrain( flux_b + (_motor.ABVoltage.beta  - _motor.phase_resistance * i_beta)  * Ts -
+        _motor.phase_inductance * (i_beta  - i_beta_prev) ,-_motor.flux_linkage, _motor.flux_linkage);
+  
+  // Calculate angle
+  float electrical_angle = _normalizeAngle(_atan2(flux_b,flux_a));
+
+  // Electrical angle difference
+  float d_electrical_angle = electrical_angle - electrical_angle_prev;
+  //if(abs(d_electrical_angle) > _PI ) full_electrical_rotations += ( d_electrical_angle > 0 ) ? -1 : 1;
+  if(abs(d_electrical_angle) > _PI ){
+    if (d_electrical_angle > 0){
+      full_electrical_rotations -= 1;
+      d_electrical_angle += _2PI;
+    }else{
+      full_electrical_rotations += 1;
+      d_electrical_angle -= _2PI;
+    }
+  }
+
+  // Mechanical angles
+  angle_prev += d_electrical_angle /_motor.pole_pairs;
+  full_rotations += full_electrical_rotations / _motor.pole_pairs;
+
+  // Store Previous values
+  i_alpha_prev = i_alpha;
+  i_beta_prev = i_beta;
+  angle_prev_ts = now;
+  electrical_angle_prev = electrical_angle;
+
+}
+
+void FluxObserverSensor::init(){
+  this->Sensor::init(); // call base class
+}
+
+/*
+	Shaft angle calculation
+*/
+float FluxObserverSensor::getSensorAngle(){
+  return 0;//return this->Sensor::getSensorAngle(); // call base class 
+}
\ No newline at end of file
diff --git a/src/encoders/flux_observer/FluxObserverSensor.h b/src/encoders/flux_observer/FluxObserverSensor.h
new file mode 100644
index 0000000..b13392c
--- /dev/null
+++ b/src/encoders/flux_observer/FluxObserverSensor.h
@@ -0,0 +1,44 @@
+#ifndef FLUX_OBSERVER_SENSOR_H
+#define FLUX_OBSERVER_SENSOR_H
+
+#include "Arduino.h"
+#include "common/base_classes/FOCMotor.h"
+#include "common/base_classes/Sensor.h"
+
+/**
+  
+*/
+
+class FluxObserverSensor : public Sensor
+{
+  public:
+    /**
+    FluxObserverSensor class constructor
+    @param m  Motor that the FluxObserverSensor will be linked to
+    */
+    FluxObserverSensor(const FOCMotor& m);
+    void update() override;
+ 
+    void init() override;
+
+    // Abstract functions of the Sensor class implementation
+    /** get current angle (rad) */
+    float getSensorAngle() override;
+
+    
+    // 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_a = 0;
+    float flux_b = 0;
+    float i_alpha_prev = 0;
+    float i_beta_prev = 0;
+    float electrical_angle_prev = 0;
+    float full_electrical_rotations = 0;
+
+  protected:    
+    const FOCMotor& _motor;
+
+};
+
+#endif
diff --git a/src/encoders/flux_observer/README.md b/src/encoders/flux_observer/README.md
new file mode 100644
index 0000000..ad94334
--- /dev/null
+++ b/src/encoders/flux_observer/README.md
@@ -0,0 +1 @@
+# Flux Observer Sensor
\ No newline at end of file

From a881a7cc7c0a63e8db1fff5d6f9ea58e6bb1ba48 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Wed, 18 Oct 2023 06:38:51 +0200
Subject: [PATCH 02/20] Fix from charizardrekt

---
 .../flux_observer/FluxObserverSensor.cpp      | 22 ++++++++++++++-----
 .../flux_observer/FluxObserverSensor.h        |  1 +
 2 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/flux_observer/FluxObserverSensor.cpp
index bc8755b..be593cb 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.cpp
+++ b/src/encoders/flux_observer/FluxObserverSensor.cpp
@@ -58,16 +58,28 @@ void FluxObserverSensor::update() {
 
   // Electrical angle difference
   float d_electrical_angle = electrical_angle - electrical_angle_prev;
-  //if(abs(d_electrical_angle) > _PI ) full_electrical_rotations += ( d_electrical_angle > 0 ) ? -1 : 1;
-  if(abs(d_electrical_angle) > _PI ){
+
+  if(abs(d_electrical_angle) > _2PI * 0.9 ){ //change the 0.9 factor based on sample rate can also just use _PI for simplicity 
     if (d_electrical_angle > 0){
-      full_electrical_rotations -= 1;
+      d_electrical_angle -= _2PI;
+    }else{
       d_electrical_angle += _2PI;
+    }
+  }
+
+  angle_track += d_electrical_angle;
+
+  // Count full rotations
+  if(abs(angle_track) > _2PI * _motor.pole_pairs){
+    if (angle_track > 0){
+      full_rotations -= 1;
+      angle_track-=_2PI;
     }else{
-      full_electrical_rotations += 1;
-      d_electrical_angle -= _2PI;
+      full_rotations += 1;
+      angle_track+=_2PI;
     }
   }
+  angle_prev = angle_track /_motor.pole_pairs;
 
   // Mechanical angles
   angle_prev += d_electrical_angle /_motor.pole_pairs;
diff --git a/src/encoders/flux_observer/FluxObserverSensor.h b/src/encoders/flux_observer/FluxObserverSensor.h
index b13392c..08ba892 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.h
+++ b/src/encoders/flux_observer/FluxObserverSensor.h
@@ -35,6 +35,7 @@ class FluxObserverSensor : public Sensor
     float i_beta_prev = 0;
     float electrical_angle_prev = 0;
     float full_electrical_rotations = 0;
+    float angle_track = 0;
 
   protected:    
     const FOCMotor& _motor;

From a87a72276283cedd6e016ab9a9055df39e5f5a16 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Wed, 18 Oct 2023 21:40:04 +0200
Subject: [PATCH 03/20] Fix

---
 .../flux_observer/FluxObserverSensor.cpp      | 42 +++++++++----------
 .../flux_observer/FluxObserverSensor.h        |  1 +
 2 files changed, 21 insertions(+), 22 deletions(-)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/flux_observer/FluxObserverSensor.cpp
index be593cb..5aa35a2 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.cpp
+++ b/src/encoders/flux_observer/FluxObserverSensor.cpp
@@ -57,34 +57,32 @@ void FluxObserverSensor::update() {
   float electrical_angle = _normalizeAngle(_atan2(flux_b,flux_a));
 
   // Electrical angle difference
-  float d_electrical_angle = electrical_angle - electrical_angle_prev;
-
-  if(abs(d_electrical_angle) > _2PI * 0.9 ){ //change the 0.9 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 = 0;
+  if (first){
+    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;
+      }
     }
   }
-
   angle_track += d_electrical_angle;
 
-  // Count full rotations
-  if(abs(angle_track) > _2PI * _motor.pole_pairs){
-    if (angle_track > 0){
-      full_rotations -= 1;
-      angle_track-=_2PI;
-    }else{
-      full_rotations += 1;
-      angle_track+=_2PI;
-    }
+  // Mechanical angles
+  if(angle_track > _2PI * _motor.pole_pairs){
+    full_rotations += 1;
+    angle_track -= _2PI * _motor.pole_pairs;
+  }else if (angle_track < 0){
+    full_rotations -= 1;
+    angle_track += _2PI * _motor.pole_pairs;
   }
   angle_prev = angle_track /_motor.pole_pairs;
-
-  // Mechanical angles
-  angle_prev += d_electrical_angle /_motor.pole_pairs;
-  full_rotations += full_electrical_rotations / _motor.pole_pairs;
-
+  
   // Store Previous values
   i_alpha_prev = i_alpha;
   i_beta_prev = i_beta;
diff --git a/src/encoders/flux_observer/FluxObserverSensor.h b/src/encoders/flux_observer/FluxObserverSensor.h
index 08ba892..2ab86d4 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.h
+++ b/src/encoders/flux_observer/FluxObserverSensor.h
@@ -36,6 +36,7 @@ class FluxObserverSensor : public Sensor
     float electrical_angle_prev = 0;
     float full_electrical_rotations = 0;
     float angle_track = 0;
+    int8_t first = 1;
 
   protected:    
     const FOCMotor& _motor;

From e5013e5957ad8d6083ac5de5b2f3106817f3d8c6 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Thu, 19 Oct 2023 18:43:50 +0200
Subject: [PATCH 04/20] Fix

---
 .../flux_observer/FluxObserverSensor.cpp      | 26 ++++++++++++++-----
 src/encoders/flux_observer/README.md          |  4 ++-
 2 files changed, 22 insertions(+), 8 deletions(-)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/flux_observer/FluxObserverSensor.cpp
index 5aa35a2..a15e4fa 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.cpp
+++ b/src/encoders/flux_observer/FluxObserverSensor.cpp
@@ -45,6 +45,16 @@ void FluxObserverSensor::update() {
       i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
   }
 
+
+  // This work deviates slightly from the BSD 3 clause licence.
+  // The work here is entirely original to the MESC FOC project, and not based
+  // on any appnotes, or borrowed from another project. This work is free to
+  // use, as granted in BSD 3 clause, with the exception that this note must
+  // be included in where this code is implemented/modified to use your
+  // variable names, structures containing variables or other minor
+  // rearrangements in place of the original names I have chosen, and credit
+  // to David Molony as the original author must be noted.
+
   // Flux linkage observer    
   float now = _micros();
   float Ts = ( now - angle_prev_ts) * 1e-6f; 
@@ -73,13 +83,15 @@ void FluxObserverSensor::update() {
   }
   angle_track += d_electrical_angle;
 
-  // Mechanical angles
-  if(angle_track > _2PI * _motor.pole_pairs){
-    full_rotations += 1;
-    angle_track -= _2PI * _motor.pole_pairs;
-  }else if (angle_track < 0){
-    full_rotations -= 1;
-    angle_track += _2PI * _motor.pole_pairs;
+  // Mechanical angle and full_rotations
+  if(abs(angle_track) > _2PI * _motor.pole_pairs){
+    if (angle_track>0){
+      full_rotations += 1;
+      angle_track -= _2PI * _motor.pole_pairs;
+    }else{
+      full_rotations -= 1;
+      angle_track += _2PI * _motor.pole_pairs;
+    }
   }
   angle_prev = angle_track /_motor.pole_pairs;
   
diff --git a/src/encoders/flux_observer/README.md b/src/encoders/flux_observer/README.md
index ad94334..ad46882 100644
--- a/src/encoders/flux_observer/README.md
+++ b/src/encoders/flux_observer/README.md
@@ -1 +1,3 @@
-# Flux Observer Sensor
\ No newline at end of file
+# Flux Observer Sensor
+
+[MESC Book](https://davidmolony.github.io/MESC_Firmware/operation/CONTROL.html#the-sensorless-observer)
\ No newline at end of file

From f58725c30e0b354960e079186bc4cebe74502455 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Sat, 21 Oct 2023 15:41:40 +0200
Subject: [PATCH 05/20] Update

---
 .../flux_observer/FluxObserverSensor.cpp      | 21 +++++++++++++------
 .../flux_observer/FluxObserverSensor.h        | 12 +++++------
 2 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/flux_observer/FluxObserverSensor.cpp
index a15e4fa..2449e84 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.cpp
+++ b/src/encoders/flux_observer/FluxObserverSensor.cpp
@@ -5,6 +5,10 @@
 
 FluxObserverSensor::FluxObserverSensor(const FOCMotor& m) : _motor(m)
 {
+  // Derive Flux linkage from KV_rating and pole_pairs
+  if (_isset(_motor.pole_pairs) && _isset(_motor.KV_rating)){
+    flux_linkage = 60 / ( _sqrt(3) * _PI * (_motor.KV_rating/_SQRT2) * (_motor.pole_pairs * 2));
+  }
 }
 
 
@@ -12,6 +16,11 @@ void FluxObserverSensor::update() {
   // Current sense is required for the observer
   if (!_motor.current_sense) return;
   
+  // Exit if one of the parameter needed for the flux observer is 0
+  if ((_motor.phase_inductance == 0) ||
+      (_motor.phase_resistance == 0) ||
+      (flux_linkage == 0)) return;
+
   // Update sensor, with optional downsampling of update rate
   if(sensor_cnt++ < sensor_downsample) return;
   
@@ -58,13 +67,13 @@ void FluxObserverSensor::update() {
   // Flux linkage observer    
   float now = _micros();
   float Ts = ( now - angle_prev_ts) * 1e-6f; 
-  flux_a = _constrain( flux_a + (_motor.ABVoltage.alpha - _motor.phase_resistance * i_alpha) * Ts -
-        _motor.phase_inductance * (i_alpha - i_alpha_prev),-_motor.flux_linkage, _motor.flux_linkage);
-  flux_b = _constrain( flux_b + (_motor.ABVoltage.beta  - _motor.phase_resistance * i_beta)  * Ts -
-        _motor.phase_inductance * (i_beta  - i_beta_prev) ,-_motor.flux_linkage, _motor.flux_linkage);
+  flux_alpha = _constrain( flux_alpha + (_motor.ABVoltage.alpha - _motor.phase_resistance * i_alpha) * Ts -
+        _motor.phase_inductance * (i_alpha - i_alpha_prev),-flux_linkage, flux_linkage);
+  flux_beta  = _constrain( flux_beta  + (_motor.ABVoltage.beta  - _motor.phase_resistance * i_beta)  * Ts -
+        _motor.phase_inductance * (i_beta  - i_beta_prev) ,-flux_linkage, flux_linkage);
   
   // Calculate angle
-  float electrical_angle = _normalizeAngle(_atan2(flux_b,flux_a));
+  float electrical_angle = _normalizeAngle(atan2(flux_beta,flux_alpha));
 
   // Electrical angle difference
   float d_electrical_angle = 0;
@@ -111,5 +120,5 @@ void FluxObserverSensor::init(){
 	Shaft angle calculation
 */
 float FluxObserverSensor::getSensorAngle(){
-  return 0;//return this->Sensor::getSensorAngle(); // call base class 
+  return 0;
 }
\ No newline at end of file
diff --git a/src/encoders/flux_observer/FluxObserverSensor.h b/src/encoders/flux_observer/FluxObserverSensor.h
index 2ab86d4..cd487a5 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.h
+++ b/src/encoders/flux_observer/FluxObserverSensor.h
@@ -29,12 +29,12 @@ class FluxObserverSensor : 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_a = 0;
-    float flux_b = 0;
-    float i_alpha_prev = 0;
-    float i_beta_prev = 0;
-    float electrical_angle_prev = 0;
-    float full_electrical_rotations = 0;
+    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 electrical_angle_prev = 0; // Previous electrical angle
     float angle_track = 0;
     int8_t first = 1;
 

From eb776975d37bf360a3c8cd5bc63769a1383af512 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Sat, 21 Oct 2023 16:40:49 +0200
Subject: [PATCH 06/20] Use Ualpha and Ubeta

---
 src/encoders/flux_observer/FluxObserverSensor.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/flux_observer/FluxObserverSensor.cpp
index 2449e84..f0354f0 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.cpp
+++ b/src/encoders/flux_observer/FluxObserverSensor.cpp
@@ -67,9 +67,9 @@ void FluxObserverSensor::update() {
   // Flux linkage observer    
   float now = _micros();
   float Ts = ( now - angle_prev_ts) * 1e-6f; 
-  flux_alpha = _constrain( flux_alpha + (_motor.ABVoltage.alpha - _motor.phase_resistance * i_alpha) * Ts -
+  flux_alpha = _constrain( flux_alpha + (_motor.Ualpha - _motor.phase_resistance * i_alpha) * Ts -
         _motor.phase_inductance * (i_alpha - i_alpha_prev),-flux_linkage, flux_linkage);
-  flux_beta  = _constrain( flux_beta  + (_motor.ABVoltage.beta  - _motor.phase_resistance * i_beta)  * Ts -
+  flux_beta  = _constrain( flux_beta  + (_motor.Ubeta  - _motor.phase_resistance * i_beta)  * Ts -
         _motor.phase_inductance * (i_beta  - i_beta_prev) ,-flux_linkage, flux_linkage);
   
   // Calculate angle

From c3feef0ce3b1a51377b0f3adb88ea6bfd450bfd2 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Mon, 23 Oct 2023 22:07:10 +0200
Subject: [PATCH 07/20] Bemf Threshold

---
 src/encoders/flux_observer/FluxObserverSensor.cpp | 13 +++++++++----
 src/encoders/flux_observer/FluxObserverSensor.h   |  5 +++--
 2 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/flux_observer/FluxObserverSensor.cpp
index f0354f0..0fa415a 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.cpp
+++ b/src/encoders/flux_observer/FluxObserverSensor.cpp
@@ -22,8 +22,13 @@ void FluxObserverSensor::update() {
       (flux_linkage == 0)) return;
 
   // Update sensor, with optional downsampling of update rate
-  if(sensor_cnt++ < sensor_downsample) return;
-  
+  if (sensor_cnt++ < sensor_downsample) return;
+
+  // Close to zero speed the flux observer can resonate
+  // 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
@@ -54,7 +59,6 @@ void FluxObserverSensor::update() {
       i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
   }
 
-
   // This work deviates slightly from the BSD 3 clause licence.
   // The work here is entirely original to the MESC FOC project, and not based
   // on any appnotes, or borrowed from another project. This work is free to
@@ -73,11 +77,12 @@ void FluxObserverSensor::update() {
         _motor.phase_inductance * (i_beta  - i_beta_prev) ,-flux_linkage, flux_linkage);
   
   // Calculate angle
-  float electrical_angle = _normalizeAngle(atan2(flux_beta,flux_alpha));
+  float 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{
diff --git a/src/encoders/flux_observer/FluxObserverSensor.h b/src/encoders/flux_observer/FluxObserverSensor.h
index cd487a5..a02f044 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.h
+++ b/src/encoders/flux_observer/FluxObserverSensor.h
@@ -35,8 +35,9 @@ class FluxObserverSensor : public Sensor
     float i_alpha_prev = 0; // Previous Alpha current
     float i_beta_prev = 0; // Previous Beta current
     float electrical_angle_prev = 0; // Previous electrical angle
-    float angle_track = 0;
-    int8_t first = 1;
+    float angle_track = 0; // Total Electrical angle
+    float bemf_threshold = 0.5; // 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;

From 25e7db7073f804a1b0a821dbd0d2cd89b980468d Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Mon, 23 Oct 2023 22:10:29 +0200
Subject: [PATCH 08/20] Regular atan2

---
 src/encoders/flux_observer/FluxObserverSensor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/flux_observer/FluxObserverSensor.cpp
index 0fa415a..bd77d99 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.cpp
+++ b/src/encoders/flux_observer/FluxObserverSensor.cpp
@@ -77,7 +77,7 @@ void FluxObserverSensor::update() {
         _motor.phase_inductance * (i_beta  - i_beta_prev) ,-flux_linkage, flux_linkage);
   
   // Calculate angle
-  float electrical_angle = _normalizeAngle(_atan2(flux_beta,flux_alpha));
+  float electrical_angle = _normalizeAngle(atan2(flux_beta,flux_alpha));
 
   // Electrical angle difference
   float d_electrical_angle = 0;

From 1a273451a1ec8a67146edfe90e9311b7c81fd6ab Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Sun, 12 Nov 2023 20:33:23 +0100
Subject: [PATCH 09/20] Fix for Clarke transform and faster atan2

---
 src/encoders/flux_observer/FluxObserverSensor.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/flux_observer/FluxObserverSensor.cpp
index bd77d99..1d79dc4 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.cpp
+++ b/src/encoders/flux_observer/FluxObserverSensor.cpp
@@ -40,12 +40,12 @@ void FluxObserverSensor::update() {
       // if only two measured currents
       i_alpha = current.a;  
       i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
-  }if(!current.a){
+  }else if(!current.a){
       // if only two measured currents
       float a = -current.c - current.b;
       i_alpha = a;  
       i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
-  }if(!current.b){
+  }else if(!current.b){
       // if only two measured currents
       float b = -current.a - current.c;
       i_alpha = current.a;  
@@ -77,7 +77,7 @@ void FluxObserverSensor::update() {
         _motor.phase_inductance * (i_beta  - i_beta_prev) ,-flux_linkage, flux_linkage);
   
   // Calculate angle
-  float electrical_angle = _normalizeAngle(atan2(flux_beta,flux_alpha));
+  float electrical_angle = _normalizeAngle(_atan2(flux_beta,flux_alpha));
 
   // Electrical angle difference
   float d_electrical_angle = 0;

From bb0ed89d3d5a8452d924b1c54674d2b1fd830c38 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Sat, 2 Dec 2023 18:08:14 +0100
Subject: [PATCH 10/20] Remove sqrt2 for kv and use getABcurrents function

---
 .../flux_observer/FluxObserverSensor.cpp      | 38 ++++---------------
 1 file changed, 8 insertions(+), 30 deletions(-)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/flux_observer/FluxObserverSensor.cpp
index 1d79dc4..546828d 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.cpp
+++ b/src/encoders/flux_observer/FluxObserverSensor.cpp
@@ -7,7 +7,7 @@ FluxObserverSensor::FluxObserverSensor(const FOCMotor& m) : _motor(m)
 {
   // Derive Flux linkage from KV_rating and pole_pairs
   if (_isset(_motor.pole_pairs) && _isset(_motor.KV_rating)){
-    flux_linkage = 60 / ( _sqrt(3) * _PI * (_motor.KV_rating/_SQRT2) * (_motor.pole_pairs * 2));
+    flux_linkage = 60 / ( _SQRT3 * _PI * _motor.KV_rating * _motor.pole_pairs * 2);
   }
 }
 
@@ -35,29 +35,7 @@ void FluxObserverSensor::update() {
   PhaseCurrent_s current = _motor.current_sense->getPhaseCurrents();
 
   // calculate clarke transform
-  float i_alpha, i_beta;
-  if(!current.c){
-      // if only two measured currents
-      i_alpha = current.a;  
-      i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
-  }else if(!current.a){
-      // if only two measured currents
-      float a = -current.c - current.b;
-      i_alpha = a;  
-      i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
-  }else if(!current.b){
-      // if only two measured currents
-      float b = -current.a - current.c;
-      i_alpha = current.a;  
-      i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b;
-  } else {
-      // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
-      float mid = (1.f/3) * (current.a + current.b + current.c);
-      float a = current.a - mid;
-      float b = current.b - mid;
-      i_alpha = a;
-      i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
-  }
+  ABCurrent_s ABcurrent = _motor.current_sense->getABCurrents(current);
 
   // This work deviates slightly from the BSD 3 clause licence.
   // The work here is entirely original to the MESC FOC project, and not based
@@ -71,10 +49,10 @@ void FluxObserverSensor::update() {
   // Flux linkage observer    
   float now = _micros();
   float Ts = ( now - angle_prev_ts) * 1e-6f; 
-  flux_alpha = _constrain( flux_alpha + (_motor.Ualpha - _motor.phase_resistance * i_alpha) * Ts -
-        _motor.phase_inductance * (i_alpha - i_alpha_prev),-flux_linkage, flux_linkage);
-  flux_beta  = _constrain( flux_beta  + (_motor.Ubeta  - _motor.phase_resistance * i_beta)  * Ts -
-        _motor.phase_inductance * (i_beta  - i_beta_prev) ,-flux_linkage, flux_linkage);
+  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);
   
   // Calculate angle
   float electrical_angle = _normalizeAngle(_atan2(flux_beta,flux_alpha));
@@ -110,8 +88,8 @@ void FluxObserverSensor::update() {
   angle_prev = angle_track /_motor.pole_pairs;
   
   // Store Previous values
-  i_alpha_prev = i_alpha;
-  i_beta_prev = i_beta;
+  i_alpha_prev = ABcurrent.alpha;
+  i_beta_prev = ABcurrent.beta;
   angle_prev_ts = now;
   electrical_angle_prev = electrical_angle;
 

From 2079b916ecf2a1d25ef0b879362b717bdad091a3 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Tue, 23 Jan 2024 20:39:06 +0100
Subject: [PATCH 11/20] Fix bemf threshold

---
 src/encoders/flux_observer/FluxObserverSensor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/flux_observer/FluxObserverSensor.cpp
index 546828d..2add3a0 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.cpp
+++ b/src/encoders/flux_observer/FluxObserverSensor.cpp
@@ -27,7 +27,7 @@ void FluxObserverSensor::update() {
   // Close to zero speed the flux observer can resonate
   // 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;
+  if (abs(bemf) < bemf_threshold) return;
 
   sensor_cnt = 0;
 

From 202ba0c4ae11558f6391c4bdff1e130a8735e178 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Fri, 7 Jun 2024 10:52:16 +0200
Subject: [PATCH 12/20] Update FluxObserverSensor.h

Make bemf threshold 0 by default
---
 src/encoders/flux_observer/FluxObserverSensor.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.h b/src/encoders/flux_observer/FluxObserverSensor.h
index a02f044..e7e49f0 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.h
+++ b/src/encoders/flux_observer/FluxObserverSensor.h
@@ -36,7 +36,7 @@ class FluxObserverSensor : public Sensor
     float i_beta_prev = 0; // Previous Beta current
     float electrical_angle_prev = 0; // Previous electrical angle
     float angle_track = 0; // Total Electrical angle
-    float bemf_threshold = 0.5; // Bemf voltage amplitude when the flux observer should start tracking
+    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:    

From b9842710460c6641f4f6f59478a03216409b233d Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Fri, 7 Jun 2024 11:07:39 +0200
Subject: [PATCH 13/20] Update README.md

Some documentations
---
 src/encoders/flux_observer/README.md | 38 +++++++++++++++++++++++++++-
 1 file changed, 37 insertions(+), 1 deletion(-)

diff --git a/src/encoders/flux_observer/README.md b/src/encoders/flux_observer/README.md
index ad46882..07cdf8d 100644
--- a/src/encoders/flux_observer/README.md
+++ b/src/encoders/flux_observer/README.md
@@ -1,3 +1,39 @@
 # Flux Observer Sensor
 
-[MESC Book](https://davidmolony.github.io/MESC_Firmware/operation/CONTROL.html#the-sensorless-observer)
\ No newline at end of file
+The MXLEMMING has been ported from the MESC Firmware, it's also the default Flux observer in Vesc.
+The [MESC book](https://davidmolony.github.io/MESC_Firmware/operation/CONTROL.html#the-sensorless-observer) explains the math behind the flux observer.
+
+It's a simple solution for sensorless motor control only using phase currents and motor parameters, if tracking the position at low speed and when not driving the motor is not relevant.
+
+### Motor Parameters:
+The MXLEMMING Flux Observer needs the following motor parameters to be set:
+- phase resistance
+- KV rating
+- phase inductance
+- pole pairs
+
+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 ([code](https://github.com/Candas1/Arduino-FOC-drivers/blob/202ba0c4ae11558f6391c4bdff1e130a8735e178/src/encoders/flux_observer/FluxObserverSensor.cpp#L10)) which is key for the flux observer to run well.
+```
+BLDCMotor motor = BLDCMotor(15, 0.1664, 17.0, 0.00036858); // Hoverboard Motor
+```
+flux_linkage parameter can be adjusted from the code.
+
+### Current Sense
+The current sense is required as this flux observer only relies on phase currents.
+
+### Sensor Alignment
+The flux observer sensor doesn't need sensor alignment.
+```
+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 ([code](https://github.com/Candas1/Arduino-FOC-drivers/blob/202ba0c4ae11558f6391c4bdff1e130a8735e178/src/encoders/flux_observer/FluxObserverSensor.cpp#L29)).
+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.
+

From df7209e0eaafb9f0e7b925deb5699b005e660b60 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Mon, 1 Jul 2024 08:46:19 +0000
Subject: [PATCH 14/20] Rename the sensor

---
 .../MXLEMMINGObserverSensor.cpp}                     | 11 +++++------
 .../MXLEMMINGObserverSensor.h}                       | 12 ++++++------
 .../{flux_observer => MXLEMMING_observer}/README.md  |  8 ++++----
 3 files changed, 15 insertions(+), 16 deletions(-)
 rename src/encoders/{flux_observer/FluxObserverSensor.cpp => MXLEMMING_observer/MXLEMMINGObserverSensor.cpp} (92%)
 rename src/encoders/{flux_observer/FluxObserverSensor.h => MXLEMMING_observer/MXLEMMINGObserverSensor.h} (81%)
 rename src/encoders/{flux_observer => MXLEMMING_observer}/README.md (88%)

diff --git a/src/encoders/flux_observer/FluxObserverSensor.cpp b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
similarity index 92%
rename from src/encoders/flux_observer/FluxObserverSensor.cpp
rename to src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
index 2add3a0..70ca01b 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.cpp
+++ b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
@@ -1,9 +1,9 @@
-#include "FluxObserverSensor.h"
+#include "MXLEMMINGObserverSensor.h"
 #include "common/foc_utils.h"
 #include "common/time_utils.h"
 
 
-FluxObserverSensor::FluxObserverSensor(const FOCMotor& m) : _motor(m)
+MXLEMMINGObserverSensor::MXLEMMINGObserverSensor(const FOCMotor& m) : _motor(m)
 {
   // Derive Flux linkage from KV_rating and pole_pairs
   if (_isset(_motor.pole_pairs) && _isset(_motor.KV_rating)){
@@ -12,7 +12,7 @@ FluxObserverSensor::FluxObserverSensor(const FOCMotor& m) : _motor(m)
 }
 
 
-void FluxObserverSensor::update() {
+void MXLEMMINGObserverSensor::update() {
   // Current sense is required for the observer
   if (!_motor.current_sense) return;
   
@@ -24,7 +24,6 @@ void FluxObserverSensor::update() {
   // Update sensor, with optional downsampling of update rate
   if (sensor_cnt++ < sensor_downsample) return;
 
-  // Close to zero speed the flux observer can resonate
   // 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;
@@ -95,13 +94,13 @@ void FluxObserverSensor::update() {
 
 }
 
-void FluxObserverSensor::init(){
+void MXLEMMINGObserverSensor::init(){
   this->Sensor::init(); // call base class
 }
 
 /*
 	Shaft angle calculation
 */
-float FluxObserverSensor::getSensorAngle(){
+float MXLEMMINGObserverSensor::getSensorAngle(){
   return 0;
 }
\ No newline at end of file
diff --git a/src/encoders/flux_observer/FluxObserverSensor.h b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h
similarity index 81%
rename from src/encoders/flux_observer/FluxObserverSensor.h
rename to src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h
index e7e49f0..7ec9a4d 100644
--- a/src/encoders/flux_observer/FluxObserverSensor.h
+++ b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h
@@ -1,5 +1,5 @@
-#ifndef FLUX_OBSERVER_SENSOR_H
-#define FLUX_OBSERVER_SENSOR_H
+#ifndef MXLEMMING_OBSERVER_SENSOR_H
+#define MXLEMMING_OBSERVER_SENSOR_H
 
 #include "Arduino.h"
 #include "common/base_classes/FOCMotor.h"
@@ -9,14 +9,14 @@
   
 */
 
-class FluxObserverSensor : public Sensor
+class MXLEMMINGObserverSensor : public Sensor
 {
   public:
     /**
-    FluxObserverSensor class constructor
-    @param m  Motor that the FluxObserverSensor will be linked to
+    MXLEMMINGObserverSensor class constructor
+    @param m  Motor that the MXLEMMINGObserverSensor will be linked to
     */
-    FluxObserverSensor(const FOCMotor& m);
+    MXLEMMINGObserverSensor(const FOCMotor& m);
     void update() override;
  
     void init() override;
diff --git a/src/encoders/flux_observer/README.md b/src/encoders/MXLEMMING_observer/README.md
similarity index 88%
rename from src/encoders/flux_observer/README.md
rename to src/encoders/MXLEMMING_observer/README.md
index 07cdf8d..aee1616 100644
--- a/src/encoders/flux_observer/README.md
+++ b/src/encoders/MXLEMMING_observer/README.md
@@ -1,12 +1,12 @@
-# Flux Observer Sensor
+# MXLEMMING Observer Sensor
 
-The MXLEMMING has been ported from the MESC Firmware, it's also the default Flux observer in Vesc.
-The [MESC book](https://davidmolony.github.io/MESC_Firmware/operation/CONTROL.html#the-sensorless-observer) explains the math behind the flux observer.
+The MXLEMMING Obserser has been ported from the MESC Firmware, it's also the default Flux observer in Vesc firmware.
+The [MESC book](https://davidmolony.github.io/MESC_Firmware/operation/CONTROL.html#the-sensorless-observer) explains the math behind this flux observer.
 
 It's a simple solution for sensorless motor control only using phase currents and motor parameters, if tracking the position at low speed and when not driving the motor is not relevant.
 
 ### Motor Parameters:
-The MXLEMMING Flux Observer needs the following motor parameters to be set:
+The MXLEMMING Observer needs the following motor parameters to be set:
 - phase resistance
 - KV rating
 - phase inductance

From 031a4dd97dd9899469491e667398b986d7d02e1f Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Mon, 1 Jul 2024 09:01:23 +0000
Subject: [PATCH 15/20] Update readme

---
 src/encoders/MXLEMMING_observer/README.md | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/encoders/MXLEMMING_observer/README.md b/src/encoders/MXLEMMING_observer/README.md
index aee1616..6f8ad4b 100644
--- a/src/encoders/MXLEMMING_observer/README.md
+++ b/src/encoders/MXLEMMING_observer/README.md
@@ -14,9 +14,10 @@ The MXLEMMING Observer needs the following motor parameters to be set:
 
 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 ([code](https://github.com/Candas1/Arduino-FOC-drivers/blob/202ba0c4ae11558f6391c4bdff1e130a8735e178/src/encoders/flux_observer/FluxObserverSensor.cpp#L10)) which is key for the flux observer to run well.
+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.
 ```
 BLDCMotor motor = BLDCMotor(15, 0.1664, 17.0, 0.00036858); // Hoverboard Motor
+MXLEMMINGObserverSensor sensor = MXLEMMINGObserverSensor(motor);
 ```
 flux_linkage parameter can be adjusted from the code.
 

From 4f42deb3c85dac30b14ed8a56ee8a623d90778b7 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Tue, 16 Jul 2024 20:09:08 +0200
Subject: [PATCH 16/20] Update MXLEMMINGObserverSensor.cpp

Fix for overflow
---
 src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
index 70ca01b..2ee5134 100644
--- a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
+++ b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
@@ -46,7 +46,7 @@ void MXLEMMINGObserverSensor::update() {
   // to David Molony as the original author must be noted.
 
   // Flux linkage observer    
-  float now = _micros();
+  int32_t now = _micros();
   float Ts = ( now - angle_prev_ts) * 1e-6f; 
   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);
@@ -103,4 +103,4 @@ void MXLEMMINGObserverSensor::init(){
 */
 float MXLEMMINGObserverSensor::getSensorAngle(){
   return 0;
-}
\ No newline at end of file
+}

From 946df15538f07dece171ef38d530c9b5165ce355 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Wed, 17 Jul 2024 19:38:47 +0200
Subject: [PATCH 17/20] Update README.md

remove link to code
---
 src/encoders/MXLEMMING_observer/README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/encoders/MXLEMMING_observer/README.md b/src/encoders/MXLEMMING_observer/README.md
index 6f8ad4b..b3d4512 100644
--- a/src/encoders/MXLEMMING_observer/README.md
+++ b/src/encoders/MXLEMMING_observer/README.md
@@ -32,7 +32,7 @@ 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 ([code](https://github.com/Candas1/Arduino-FOC-drivers/blob/202ba0c4ae11558f6391c4bdff1e130a8735e178/src/encoders/flux_observer/FluxObserverSensor.cpp#L29)).
+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:

From 9045e6cd44e487904fb346dffc30c1a1c95ce2b4 Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Wed, 17 Jul 2024 21:37:29 +0200
Subject: [PATCH 18/20] Update MXLEMMINGObserverSensor.cpp

Update time calculation
---
 .../MXLEMMINGObserverSensor.cpp                   | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

diff --git a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
index 2ee5134..98d7aa5 100644
--- a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
+++ b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
@@ -36,6 +36,13 @@ void MXLEMMINGObserverSensor::update() {
   // calculate clarke transform
   ABCurrent_s ABcurrent = _motor.current_sense->getABCurrents(current);
 
+  // get current timestamp
+  long now_us = _micros();
+  // calculate the sample time from last call
+  float Ts = (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;
+
   // This work deviates slightly from the BSD 3 clause licence.
   // The work here is entirely original to the MESC FOC project, and not based
   // on any appnotes, or borrowed from another project. This work is free to
@@ -44,10 +51,8 @@ void MXLEMMINGObserverSensor::update() {
   // variable names, structures containing variables or other minor
   // rearrangements in place of the original names I have chosen, and credit
   // to David Molony as the original author must be noted.
-
-  // Flux linkage observer    
-  int32_t now = _micros();
-  float Ts = ( now - angle_prev_ts) * 1e-6f; 
+  
+  // 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 -
@@ -89,7 +94,7 @@ void MXLEMMINGObserverSensor::update() {
   // Store Previous values
   i_alpha_prev = ABcurrent.alpha;
   i_beta_prev = ABcurrent.beta;
-  angle_prev_ts = now;
+  angle_prev_ts = now_us;
   electrical_angle_prev = electrical_angle;
 
 }

From 0010b2c5418138822dd66bc1cd173e4f8dbb732c Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Sat, 20 Jul 2024 14:44:28 +0000
Subject: [PATCH 19/20] Cleanup

---
 .../MXLEMMINGObserverSensor.cpp               | 32 ++++++++-----------
 .../MXLEMMINGObserverSensor.h                 | 12 +++----
 src/encoders/MXLEMMING_observer/README.md     |  3 +-
 3 files changed, 21 insertions(+), 26 deletions(-)

diff --git a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
index 98d7aa5..1676c55 100644
--- a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
+++ b/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;
-}
+}
\ No newline at end of file
diff --git a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h
index 7ec9a4d..52ded05 100644
--- a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h
+++ b/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
\ No newline at end of file
diff --git a/src/encoders/MXLEMMING_observer/README.md b/src/encoders/MXLEMMING_observer/README.md
index b3d4512..d7bc520 100644
--- a/src/encoders/MXLEMMING_observer/README.md
+++ b/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
 

From 4a091f0bfcb0e391a4e33c21bfebcd6b45f1a6ea Mon Sep 17 00:00:00 2001
From: Candas1 <candascraig@hotmail.com>
Date: Sat, 27 Jul 2024 20:49:09 +0200
Subject: [PATCH 20/20] Clean up

---
 .../MXLEMMINGObserverSensor.cpp               | 19 +++++++++----------
 .../MXLEMMINGObserverSensor.h                 |  1 -
 src/encoders/MXLEMMING_observer/README.md     |  6 ++----
 3 files changed, 11 insertions(+), 15 deletions(-)

diff --git a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
index 1676c55..cf872d1 100644
--- a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp
+++ b/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));
diff --git a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h b/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h
index 52ded05..e5ba28c 100644
--- a/src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h
+++ b/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;
diff --git a/src/encoders/MXLEMMING_observer/README.md b/src/encoders/MXLEMMING_observer/README.md
index d7bc520..29a06b5 100644
--- a/src/encoders/MXLEMMING_observer/README.md
+++ b/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