FEAT merge with dev

Author: askuric

keywords.txt

@@ -18,6 +18,7 @@ StepperDriver	KEYWORD1
 PIDController	KEYWORD1    
 LowPassFilter	KEYWORD1   
 InlineCurrentSense	KEYWORD1   
+LowsideCurrentSense	KEYWORD1   
 CurrentSense	KEYWORD1   
 Commander	KEYWORD1   
 StepDirListener	KEYWORD1   
@@ -48,6 +49,9 @@ LPF_current_q	KEYWORD2
 LPF_current_d	KEYWORD2
 P_angle	KEYWORD2
 LPF_angle	KEYWORD2
+lpf_a	KEYWORD2
+lpf_b	KEYWORD2
+lpf_c	KEYWORD2
 
 MotionControlType	KEYWORD1
 TorqueControlType	KEYWORD1

src/common/defaults.h

@@ -43,4 +43,7 @@
 // align voltage
 #define DEF_VOLTAGE_SENSOR_ALIGN 3.0f //!< default voltage for sensor and motor zero alignemt
 // low pass filter velocity
-#define DEF_VEL_FILTER_Tf 0.005f //!< default velocity filter time constant
+#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant
+
+// current sense default parameters
+#define DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf 0.0f  //!< default currnet sense per phase low pass filter time constant 

src/current_sense/InlineCurrentSense.cpp

@@ -50,9 +50,9 @@ void InlineCurrentSense::calibrateOffsets(){
 // read all three phase currents (if possible 2 or 3)
 PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){
     PhaseCurrent_s current;
-    current.a = (_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps
-    current.b = (_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps
-    current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps
+    current.a = lpf_a(_readADCVoltageInline(pinA) - offset_ia)*gain_a;// amps
+    current.b = lpf_b(_readADCVoltageInline(pinB) - offset_ib)*gain_b;// amps
+    current.c = (!_isset(pinC)) ? 0 : lpf_c(_readADCVoltageInline(pinC) - offset_ic)*gain_c; // amps
     return current;
 }
 // Function synchronizing current sense with motor driver.

src/current_sense/InlineCurrentSense.h

@@ -4,7 +4,9 @@
 #include "Arduino.h"
 #include "../common/foc_utils.h"
 #include "../common/time_utils.h"
+#include "../common/defaults.h"
 #include "../common/base_classes/CurrentSense.h"
+#include "../common/lowpass_filter.h"
 #include "hardware_api.h"
 
 
@@ -33,8 +35,13 @@ class InlineCurrentSense: public CurrentSense{
     float gain_b; //!< phase B gain
     float gain_c; //!< phase C gain
 
-  private:
+    // per phase low pass fileters
+    LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current A low pass filter
+    LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current B low pass filter
+    LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current C low pass filter
 
+  private:
+  
     // hardware variables
   	int pinA; //!< pin A analog pin for current measurement
   	int pinB; //!< pin B analog pin for current measurement

src/current_sense/LowsideCurrentSense.cpp

@@ -27,13 +27,15 @@ void LowsideCurrentSense::init(){
     calibrateOffsets();
 }
 // Function finding zero offsets of the ADC
-void LowsideCurrentSense::calibrateOffsets(){
+void LowsideCurrentSense::calibrateOffsets(){    
+    const int calibration_rounds = 1000;
+
     // find adc offset = zero current voltage
-    offset_ia =0;
-    offset_ib= 0;
-    offset_ic= 0;
+    offset_ia = 0;
+    offset_ib = 0;
+    offset_ic = 0;
     // read the adc voltage 1000 times ( arbitrary number )
-    for (int i = 0; i < 1000; i++) {
+    for (int i = 0; i < calibration_rounds; i++) {
         _startADC3PinConversionLowSide();
         offset_ia += _readADCVoltageLowSide(pinA);
         offset_ib += _readADCVoltageLowSide(pinB);
@@ -47,12 +49,11 @@ void LowsideCurrentSense::calibrateOffsets(){
 }
 
 // read all three phase currents (if possible 2 or 3)
-PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){
     PhaseCurrent_s current;
     _startADC3PinConversionLowSide();
-    current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps
-    current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps
-    current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps
+    current.a = lpf_a(_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps
+    current.b = lpf_b(_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps
+    current.c = (!_isset(pinC)) ? 0 : lpf_b(_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps
     return current;
 }
 // Function synchronizing current sense with motor driver.

src/current_sense/LowsideCurrentSense.h

@@ -4,8 +4,10 @@
 #include "Arduino.h"
 #include "../common/foc_utils.h"
 #include "../common/time_utils.h"
+#include "../common/defaults.h"
 #include "../common/base_classes/CurrentSense.h"
 #include "../common/base_classes/FOCMotor.h"
+#include "../common/lowpass_filter.h"
 #include "hardware_api.h"
 
 
@@ -34,6 +36,11 @@ class LowsideCurrentSense: public CurrentSense{
     float gain_b; //!< phase B gain
     float gain_c; //!< phase C gain
 
+    // per phase low pass fileters
+    LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current A low pass filter
+    LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current B low pass filter
+    LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current C low pass filter
+
   private:
 
     // hardware variables

src/current_sense/hardware_api.h

@@ -42,4 +42,8 @@ float _readADCVoltageLowSide(const int pinA);
  *  function syncing the Driver with the ADC  for the LowSide Sensing
  */
 void _driverSyncLowSide();
+
+
+void _startADC3PinConversionLowSide();
+
 #endif

src/current_sense/hardware_specific/atmega_mcu.cpp

@@ -0,0 +1,55 @@
+#include "../hardware_api.h"
+
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) 
+
+#define _ADC_VOLTAGE 5.0
+#define _ADC_RESOLUTION 1024.0
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+#ifndef cbi
+  #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+#endif
+#ifndef sbi
+  #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
+#endif
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageInline(const int pinA){
+  uint32_t raw_adc = analogRead(pinA);
+  return raw_adc * _ADC_CONV;
+}
+// function reading an ADC value and returning the read voltage
+void _configureADCInline(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+
+  // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default)  
+  // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz 
+  // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample 
+  cbi(ADCSRA, ADPS2);
+  sbi(ADCSRA, ADPS1);
+  sbi(ADCSRA, ADPS0);
+}
+
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageLowSide(const int pinA){
+  return _readADCVoltageInline(pinA);
+}
+// Configure low side for generic mcu
+// cannot do much but 
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+#endif

src/current_sense/hardware_specific/due_mcu.cpp

@@ -0,0 +1,38 @@
+#include "../hardware_api.h" 
+
+#if defined(__arm__) && defined(__SAM3X8E__) 
+
+#define _ADC_VOLTAGE 3.3
+#define _ADC_RESOLUTION 1024.0
+
+// adc counts to voltage conversion ratio
+// some optimizing for faster execution
+#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageInline(const int pinA){
+  uint32_t raw_adc = analogRead(pinA);
+  return raw_adc * _ADC_CONV;
+}
+// function reading an ADC value and returning the read voltage
+void _configureADCInline(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageLowSide(const int pinA){
+  return _readADCVoltageInline(pinA);
+}
+// Configure low side for generic mcu
+// cannot do much but 
+void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+}
+
+
+#endif