FEAT fix of esp32 driver + added support for per phase lpf for current sensing

Author: askuric

keywords.txt

@@ -49,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.0 //!< default voltage for sensor and motor zero alignemt
 // low pass filter velocity
-#define DEF_VEL_FILTER_Tf 0.005 //!< 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

@@ -52,9 +52,9 @@ void LowsideCurrentSense::calibrateOffsets(){
 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

@@ -28,9 +28,6 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET)
  * @param pinC - adc pin C
  */
 void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET);
-
-void _startADC3PinConversionLowSide();
-
 /**
  *  function reading an ADC value and returning the read voltage
  * 
@@ -42,4 +39,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/esp32_adc_driver.cpp

@@ -1,21 +1,6 @@
-// Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#if defined(ESP_H)
-
 #include "esp32_adc_driver.h"
-#include "Arduino.h"
+
+#ifdef ESP_H
 
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"

src/current_sense/hardware_specific/esp32_adc_driver.h

@@ -1,28 +1,11 @@
-/*
- Arduino.h - Main include file for the Arduino SDK
- Copyright (c) 2005-2013 Arduino Team.  All right reserved.
-
- This library is free software; you can redistribute it and/or
- modify it under the terms of the GNU Lesser General Public
- License as published by the Free Software Foundation; either
- version 2.1 of the License, or (at your option) any later version.
-
- This library is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with this library; if not, write to the Free Software
- Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
- */
 
-#if !defined(MAIN_ESP32_HAL_ADC_DRIVER_H_) && defined(ESP_H) 
-#define MAIN_ESP32_HAL_ADC_DRIVER_H_
 
-#include "esp32-hal.h"
+#ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ 
+#define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_
 
+#include "Arduino.h"
 
+#ifdef ESP_H
 /*
  * Get ADC value for pin
  * */
@@ -101,4 +84,5 @@ bool __adcBusy(uint8_t pin);
  * */
 uint16_t __adcEnd(uint8_t pin);
 
-#endif /* MAIN_ESP32_HAL_ADC_H_ */
+#endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */
+#endif /* ESP32 */

src/current_sense/hardware_specific/esp32_mcu.cpp

@@ -1,6 +1,6 @@
 #include "../hardware_api.h"
 
-#if defined(ESP_H)
+#ifdef ESP_H
 
 #include "driver/mcpwm.h"
 #include "soc/mcpwm_reg.h"
@@ -11,8 +11,8 @@
 
 #include "esp32_adc_driver.h"
 
-#define _ADC_VOLTAGE 3.3
-#define _ADC_RESOLUTION 4095.0
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 4095.0f
 
 static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1};
 int a1, a2, a3;         //Current readings from internal current sensor amplifiers
@@ -98,8 +98,9 @@ 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){
+void _configureADCInline(const int pinA,const int pinB, const int pinC){
   pinMode(pinA, INPUT);
   pinMode(pinB, INPUT);
   if( _isset(pinC) ) pinMode(pinC, INPUT);