Merge pull request #48 from Candas1/flux_observer

MXLemming flux observer

src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.cpp

@@ -0,0 +1,104 @@
+#include "MXLEMMINGObserverSensor.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+
+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)){
+    flux_linkage = 60 / ( _SQRT3 * _PI * _motor.KV_rating * _motor.pole_pairs * 2);
+  }
+}
+
+
+void MXLEMMINGObserverSensor::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;
+
+  sensor_cnt = 0;
+
+  // read current phase currents
+  PhaseCurrent_s current = _motor.current_sense->getPhaseCurrents();
+
+  // 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 dt = (now_us - angle_prev_ts) * 1e-6f;
+  // quick fix for strange cases (micros overflow + timestamp not defined)
+  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
+  // 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.
+
+  // MXLEMMING Flux Observer
+  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));
+
+  // Electrical angle difference
+  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
+  float full_rotation = _2PI * _motor.pole_pairs;
+  if(abs(angle_track) > full_rotation){
+    if (angle_track>0){
+      full_rotations += 1;
+      angle_track -= full_rotation;
+    }else{
+      full_rotations -= 1;
+      angle_track += full_rotation;
+    }
+  }
+  angle_prev = angle_track /_motor.pole_pairs;
+
+  // Store Previous values
+  i_alpha_prev = ABcurrent.alpha;
+  i_beta_prev = ABcurrent.beta;
+  angle_prev_ts = now_us;
+  electrical_angle_prev = electrical_angle;
+
+}
+
+void MXLEMMINGObserverSensor::init(){
+  this->Sensor::init(); // call base class
+}
+
+/*
+	Shaft angle calculation
+*/
+float MXLEMMINGObserverSensor::getSensorAngle(){
+  return 0;
+}

src/encoders/MXLEMMING_observer/MXLEMMINGObserverSensor.h

@@ -0,0 +1,46 @@
+#ifndef MXLEMMING_OBSERVER_SENSOR_H
+#define MXLEMMING_OBSERVER_SENSOR_H
+
+#include "Arduino.h"
+#include "common/base_classes/FOCMotor.h"
+#include "common/base_classes/Sensor.h"
+
+/**
+  
+*/
+
+class MXLEMMINGObserverSensor : public Sensor
+{
+  public:
+    /**
+    MXLEMMINGObserverSensor class constructor
+    @param m  Motor that the MXLEMMINGObserverSensor will be linked to
+    */
+    MXLEMMINGObserverSensor(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_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 = 0; // Electrical angle
+    float electrical_angle_prev = 0; // Previous electrical angle
+    float angle_track = 0; // Total Electrical angle
+
+  protected:    
+    const FOCMotor& _motor;
+
+};
+
+#endif

src/encoders/MXLEMMING_observer/README.md

@@ -0,0 +1,39 @@
+# MXLEMMING Observer Sensor
+
+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 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 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);
+```
+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;
+```
+
+### 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
+