Merge remote-tracking branch 'origin/dev' into feat_hybridstepper_to_main

Author: askuric

README.md

@@ -10,13 +10,12 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un
 
 ## New Release
 
-v1.0.8 - Released July 2024, for Simple FOC 2.3.4 or later
+v1.0.9 - Released xxx 2024, for Simple FOC 2.3.x or later
 
 
-What's changed since 1.0.7?
-- MA735 driver thanks to [@techyrobot](https://github.com/techy-robot)
-- ESP32HWEncoder driver thanks to [@mcells](https://github.com/mcells)
-- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.8+)
+What's changed since 1.0.8?
+- AS5600 driver bugfix thanks to [@zbas](https://github.com/zbas)
+- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.9)
 
 
 ## What is included
@@ -116,6 +115,11 @@ Find out more information about the Arduino SimpleFOC project on the [docs websi
 
 ## Release History
 
+What's changed since 1.0.7?
+- MA735 driver thanks to [@techyrobot](https://github.com/techy-robot)
+- ESP32HWEncoder driver thanks to [@mcells](https://github.com/mcells)
+- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.8)
+
 What's changed since 1.0.6?
 - Improvements to LinearHall driver, thanks to dekutree
 - Fix for ESP32 compiler warning, thanks to Yannik Stradmann
@@ -126,14 +130,14 @@ What's changed since 1.0.6?
 - Refactored settings storage code
 - Refactored I2CCommander
 - New utility class for simple trapezoidal motion profiles
-- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.7+)
+- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=milestone%3A1.0.7)
 
 What's changed since 1.0.5?
 - Added STSPIN32G4 driver
 - Added STM32G4 CORDIC code, for greatly accellerated trig functions on supported MCUs
 - Added STM32FlashSettingsStorage driver, supporting STM32G4 MCUs
 - Improvements in the MT6835 sensor driver
-- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.6+)
+- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.6)
 
 What's changed since 1.0.4?
 - Added smoothing sensor by [@dekutree64](https://github.com/dekutree64)
@@ -143,7 +147,7 @@ What's changed since 1.0.4?
 - New Settings driver: SAMDNVMSettingsStorage
 - SimpleFOCRegisters abstraction, mapping SimpleFOC parameters to virtual "Registers"
 - Updated I2CCommander to use the new registers abstraction
-- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.5+)
+- Bugfixes [included](https://github.com/simplefoc/Arduino-FOC-drivers/issues?q=is%3Aissue+milestone%3A1.0.5)
 
 What's changed since 1.0.3?
 - New Comms/Input: STM32SpeedDirCommander

examples/encoders/smoothing/smoothing.ino

@@ -1,6 +1,9 @@
 /**
  *
- * Hall sensor velocity motion control example, modified to demonstrate usage of SmoothingSensor
+ * Hall sensor velocity motion control example, modified to demonstrate usage of SmoothingSensor. 
+ * The only changes are the declaration of the SmoothingSensor, passing it to motor.linkSensor 
+ * instead of the HallSensor instance, and the added Commander code to switch between the two.
+ *
  * Steps:
  * 1) Configure the motor and sensor
  * 2) Run the code
@@ -63,8 +66,6 @@ void setup() {
   sensor.enableInterrupts(doA, doB); //, doC);
   // software interrupts
   PciManager.registerListener(&listenerIndex);
-  // set SmoothingSensor phase correction for hall sensors
-  smooth.phase_correction = -_PI_6;
   // link the SmoothingSensor to the motor
   motor.linkSensor(&smooth);
 

library.properties

@@ -1,5 +1,5 @@
 name=SimpleFOCDrivers
-version=1.0.8
+version=1.0.9
 author=Simplefoc <[email protected]>
 maintainer=Simplefoc <[email protected]>
 sentence=A library of supporting drivers for SimpleFOC. Motor drivers chips, encoder chips, current sensing and supporting code.

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
+

src/encoders/as5600/AS5600.cpp

@@ -124,13 +124,13 @@ void AS5600::setZPos(uint16_t value) {
 void AS5600::setI2CAddr(uint8_t value) {
     uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CADDR, 1);
     val = (value<<1) | (val&0x01);
-    writeRegister(AS5600_REG_I2CADDR, val);
+    writeRegister(AS5600_REG_I2CADDR, val, 1);
 };
 
 void AS5600::setI2CUpdt(uint8_t value) {
     uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CUPDT, 1);
     val = (value<<1) | (val&0x01);
-    writeRegister(AS5600_REG_I2CUPDT, val);
+    writeRegister(AS5600_REG_I2CUPDT, val, 1);
 };
 
 

src/encoders/as5600/MagneticSensorAS5600.cpp

@@ -13,6 +13,6 @@ void MagneticSensorAS5600::init(TwoWire* wire) {
 
 
 float MagneticSensorAS5600::getSensorAngle() {
-    uint16_t raw = readRawAngle();
+    uint16_t raw = angle();
     return raw / AS5600_CPR * _2PI;
 };

src/encoders/linearhall/LinearHall.cpp

@@ -13,6 +13,8 @@ LinearHall::LinearHall(int _hallA, int _hallB, int _pp){
   pinA = _hallA;
   pinB = _hallB;
   pp = _pp;
+  electrical_rev = 0;
+  prev_reading = 0;
 }
 
 float LinearHall::getSensorAngle() {
@@ -84,7 +86,7 @@ void LinearHall::init(FOCMotor *motor) {
   // move one mechanical revolution forward
   for (int i = 0; i <= 2000; i++)
   {
-    float angle = _3PI_2 + _2PI * i * pp / 2000.0f;
+    float angle = _3PI_2 + _2PI * i * motor->pole_pairs / pp / 2000.0f;
     motor->setPhaseVoltage(motor->voltage_sensor_align, 0, angle);
 
     ReadLinearHalls(pinA, pinB, &lastA, &lastB);

src/encoders/smoothing/SmoothingSensor.cpp

@@ -1,12 +1,16 @@
 #include "SmoothingSensor.h"
 #include "common/foc_utils.h"
 #include "common/time_utils.h"
+#include "sensors/HallSensor.h"
 
 
 SmoothingSensor::SmoothingSensor(Sensor& s, const FOCMotor& m) : _wrapped(s), _motor(m)
 {
 }
 
+SmoothingSensor::SmoothingSensor(HallSensor& s, const FOCMotor& m) : _wrapped(s), _motor(m) {
+  phase_correction = -_PI_6;
+}
 
 void SmoothingSensor::update() {
   // Update sensor, with optional downsampling of update rate
@@ -27,8 +31,8 @@ void SmoothingSensor::update() {
 
   // Apply phase correction if needed
   if (phase_correction != 0) {
-    if (_motor.shaft_velocity < -0) angle_prev -= _motor.sensor_direction * phase_correction / _motor.pole_pairs;
-    else if (_motor.shaft_velocity > 0) angle_prev += _motor.sensor_direction * phase_correction / _motor.pole_pairs;
+    if (_motor.shaft_velocity < -0.001) angle_prev -= _motor.sensor_direction * phase_correction / _motor.pole_pairs;
+    else if (_motor.shaft_velocity > 0.001) angle_prev += _motor.sensor_direction * phase_correction / _motor.pole_pairs;
   }
 
   // Handle wraparound of the projected angle