deepseek/wrobot
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

v1.3增加一个简单的越障模式

Browse Source
This commit is contained in:
myperworld committed 2025-11-22 22:52:53 +08:00
1 parent 9d56ba85f1
commit 7a1931cb72
1835 files changed
+475654 -1
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

v1.3/lib/Simple_FOC/src/current_sense/GenericCurrentSense.cpp
+162
View File
@@ -0,0 +1,162 @@
#include "GenericCurrentSense.h"
// GenericCurrentSense constructor
GenericCurrentSense::GenericCurrentSense(PhaseCurrent_s (*readCallback)(), void (*initCallback)()){
// if function provided add it to the
if(readCallback != nullptr) this->readCallback = readCallback;
if(initCallback != nullptr) this->initCallback = initCallback;
}
// Inline sensor init function
void GenericCurrentSense::init(){
// configure ADC variables
if(initCallback != nullptr) initCallback();
// calibrate zero offsets
calibrateOffsets();
}
// Function finding zero offsets of the ADC
void GenericCurrentSense::calibrateOffsets(){
const int calibration_rounds = 1000;
// find adc offset = zero current voltage
offset_ia = 0;
offset_ib = 0;
offset_ic = 0;
// read the adc voltage 1000 times ( arbitrary number )
for (int i = 0; i < calibration_rounds; i++) {
PhaseCurrent_s current = readCallback();
offset_ia += current.a;
offset_ib += current.b;
offset_ic += current.c;
_delay(1);
}
// calculate the mean offsets
offset_ia = offset_ia / calibration_rounds;
offset_ib = offset_ib / calibration_rounds;
offset_ic = offset_ic / calibration_rounds;
}
// read all three phase currents (if possible 2 or 3)
PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){
PhaseCurrent_s current = readCallback();
current.a = (current.a - offset_ia);// amps
current.b = (current.a - offset_ib);// amps
current.c = (current.a - offset_ic); // amps
return current;
}
// Function synchronizing current sense with motor driver.
// for in-line sensig no such thing is necessary
int GenericCurrentSense::driverSync(BLDCDriver *driver){
return 1;
}
// Function aligning the current sense with motor driver
// if all pins are connected well none of this is really necessary! - can be avoided
// returns flag
// 0 - fail
// 1 - success and nothing changed
// 2 - success but pins reconfigured
// 3 - success but gains inverted
// 4 - success but pins reconfigured and gains inverted
int GenericCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
int exit_flag = 1;
if(skip_align) return exit_flag;
// // set phase A active and phases B and C down
// driver->setPwm(voltage, 0, 0);
// _delay(200);
// PhaseCurrent_s c = getPhaseCurrents();
// // read the current 100 times ( arbitrary number )
// for (int i = 0; i < 100; i++) {
// PhaseCurrent_s c1 = getPhaseCurrents();
// c.a = c.a*0.6f + 0.4f*c1.a;
// c.b = c.b*0.6f + 0.4f*c1.b;
// c.c = c.c*0.6f + 0.4f*c1.c;
// _delay(3);
// }
// driver->setPwm(0, 0, 0);
// // align phase A
// float ab_ratio = fabs(c.a / c.b);
// float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
// if( ab_ratio > 1.5f ){ // should be ~2
// gain_a *= _sign(c.a);
// }else if( ab_ratio < 0.7f ){ // should be ~0.5
// // switch phase A and B
// int tmp_pinA = pinA;
// pinA = pinB;
// pinB = tmp_pinA;
// gain_a *= _sign(c.b);
// exit_flag = 2; // signal that pins have been switched
// }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
// // switch phase A and C
// int tmp_pinA = pinA;
// pinA = pinC;
// pinC= tmp_pinA;
// gain_a *= _sign(c.c);
// exit_flag = 2;// signal that pins have been switched
// }else{
// // error in current sense - phase either not measured or bad connection
// return 0;
// }
// // set phase B active and phases A and C down
// driver->setPwm(0, voltage, 0);
// _delay(200);
// c = getPhaseCurrents();
// // read the current 50 times
// for (int i = 0; i < 100; i++) {
// PhaseCurrent_s c1 = getPhaseCurrents();
// c.a = c.a*0.6f + 0.4f*c1.a;
// c.b = c.b*0.6f + 0.4f*c1.b;
// c.c = c.c*0.6f + 0.4f*c1.c;
// _delay(3);
// }
// driver->setPwm(0, 0, 0);
// float ba_ratio = fabs(c.b/c.a);
// float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
// if( ba_ratio > 1.5f ){ // should be ~2
// gain_b *= _sign(c.b);
// }else if( ba_ratio < 0.7f ){ // it should be ~0.5
// // switch phase A and B
// int tmp_pinB = pinB;
// pinB = pinA;
// pinA = tmp_pinB;
// gain_b *= _sign(c.a);
// exit_flag = 2; // signal that pins have been switched
// }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
// // switch phase A and C
// int tmp_pinB = pinB;
// pinB = pinC;
// pinC = tmp_pinB;
// gain_b *= _sign(c.c);
// exit_flag = 2; // signal that pins have been switched
// }else{
// // error in current sense - phase either not measured or bad connection
// return 0;
// }
// // if phase C measured
// if(_isset(pinC)){
// // set phase B active and phases A and C down
// driver->setPwm(0, 0, voltage);
// _delay(200);
// c = getPhaseCurrents();
// // read the adc voltage 500 times ( arbitrary number )
// for (int i = 0; i < 50; i++) {
// PhaseCurrent_s c1 = getPhaseCurrents();
// c.c = (c.c+c1.c)/50.0f;
// }
// driver->setPwm(0, 0, 0);
// gain_c *= _sign(c.c);
// }
// if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
// exit flag is either
// 0 - fail
// 1 - success and nothing changed
// 2 - success but pins reconfigured
// 3 - success but gains inverted
// 4 - success but pins reconfigured and gains inverted
return exit_flag;
}
v1.3/lib/Simple_FOC/src/current_sense/GenericCurrentSense.h
+41
View File
@@ -0,0 +1,41 @@
#ifndef GENERIC_CS_LIB_H
#define GENERIC_CS_LIB_H
#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"
class GenericCurrentSense: public CurrentSense{
public:
/**
GenericCurrentSense class constructor
*/
GenericCurrentSense(PhaseCurrent_s (*readCallback)() = nullptr, void (*initCallback)() = nullptr);
// CurrentSense interface implementing functions
void init() override;
PhaseCurrent_s getPhaseCurrents() override;
int driverSync(BLDCDriver *driver) override;
int driverAlign(BLDCDriver *driver, float voltage) override;
PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading
void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation
private:
/**
* Function finding zero offsets of the ADC
*/
void calibrateOffsets();
float offset_ia; //!< zero current A voltage value (center of the adc reading)
float offset_ib; //!< zero current B voltage value (center of the adc reading)
float offset_ic; //!< zero current C voltage value (center of the adc reading)
};
#endif
v1.3/lib/Simple_FOC/src/current_sense/InlineCurrentSense.cpp
+172
View File
@@ -0,0 +1,172 @@
#include "InlineCurrentSense.h"
// InlineCurrentSensor constructor
// - shunt_resistor - shunt resistor value
// - gain - current-sense op-amp gain
// - phA - A phase adc pin
// - phB - B phase adc pin
// - phC - C phase adc pin (optional)
InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){
pinA = _pinA;
pinB = _pinB;
pinC = _pinC;
shunt_resistor = _shunt_resistor;
amp_gain = _gain;
volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps
// gains for each phase
gain_a = volts_to_amps_ratio;
gain_b = volts_to_amps_ratio;
gain_c = volts_to_amps_ratio;
}
// Inline sensor init function
void InlineCurrentSense::init(){
// configure ADC variables
_configureADCInline(pinA,pinB,pinC);
// calibrate zero offsets
calibrateOffsets();
}
// Function finding zero offsets of the ADC
void InlineCurrentSense::calibrateOffsets(){
const int calibration_rounds = 1000;
// find adc offset = zero current voltage
offset_ia = 0;
offset_ib = 0;
offset_ic = 0;
// read the adc voltage 1000 times ( arbitrary number )
for (int i = 0; i < calibration_rounds; i++) {
offset_ia += _readADCVoltageInline(pinA);
offset_ib += _readADCVoltageInline(pinB);
if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC);
_delay(1);
}
// calculate the mean offsets
offset_ia = offset_ia / calibration_rounds;
offset_ib = offset_ib / calibration_rounds;
if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
}
// 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
return current;
}
// Function synchronizing current sense with motor driver.
// for in-line sensig no such thing is necessary
int InlineCurrentSense::driverSync(BLDCDriver *driver){
return 1;
}
// Function aligning the current sense with motor driver
// if all pins are connected well none of this is really necessary! - can be avoided
// returns flag
// 0 - fail
// 1 - success and nothing changed
// 2 - success but pins reconfigured
// 3 - success but gains inverted
// 4 - success but pins reconfigured and gains inverted
int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
int exit_flag = 1;
if(skip_align) return exit_flag;
// set phase A active and phases B and C down
driver->setPwm(voltage, 0, 0);
_delay(200);
PhaseCurrent_s c = getPhaseCurrents();
// read the current 100 times ( arbitrary number )
for (int i = 0; i < 100; i++) {
PhaseCurrent_s c1 = getPhaseCurrents();
c.a = c.a*0.6f + 0.4f*c1.a;
c.b = c.b*0.6f + 0.4f*c1.b;
c.c = c.c*0.6f + 0.4f*c1.c;
_delay(3);
}
driver->setPwm(0, 0, 0);
// align phase A
float ab_ratio = fabs(c.a / c.b);
float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
if( ab_ratio > 1.5f ){ // should be ~2
gain_a *= _sign(c.a);
}else if( ab_ratio < 0.7f ){ // should be ~0.5
// switch phase A and B
int tmp_pinA = pinA;
pinA = pinB;
pinB = tmp_pinA;
gain_a *= _sign(c.b);
exit_flag = 2; // signal that pins have been switched
}else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
// switch phase A and C
int tmp_pinA = pinA;
pinA = pinC;
pinC= tmp_pinA;
gain_a *= _sign(c.c);
exit_flag = 2;// signal that pins have been switched
}else{
// error in current sense - phase either not measured or bad connection
return 0;
}
// set phase B active and phases A and C down
driver->setPwm(0, voltage, 0);
_delay(200);
c = getPhaseCurrents();
// read the current 50 times
for (int i = 0; i < 100; i++) {
PhaseCurrent_s c1 = getPhaseCurrents();
c.a = c.a*0.6f + 0.4f*c1.a;
c.b = c.b*0.6f + 0.4f*c1.b;
c.c = c.c*0.6f + 0.4f*c1.c;
_delay(3);
}
driver->setPwm(0, 0, 0);
float ba_ratio = fabs(c.b/c.a);
float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
if( ba_ratio > 1.5f ){ // should be ~2
gain_b *= _sign(c.b);
}else if( ba_ratio < 0.7f ){ // it should be ~0.5
// switch phase A and B
int tmp_pinB = pinB;
pinB = pinA;
pinA = tmp_pinB;
gain_b *= _sign(c.a);
exit_flag = 2; // signal that pins have been switched
}else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
// switch phase A and C
int tmp_pinB = pinB;
pinB = pinC;
pinC = tmp_pinB;
gain_b *= _sign(c.c);
exit_flag = 2; // signal that pins have been switched
}else{
// error in current sense - phase either not measured or bad connection
return 0;
}
// if phase C measured
if(_isset(pinC)){
// set phase B active and phases A and C down
driver->setPwm(0, 0, voltage);
_delay(200);
c = getPhaseCurrents();
// read the adc voltage 500 times ( arbitrary number )
for (int i = 0; i < 50; i++) {
PhaseCurrent_s c1 = getPhaseCurrents();
c.c = (c.c+c1.c)/50.0f;
}
driver->setPwm(0, 0, 0);
gain_c *= _sign(c.c);
}
if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
// exit flag is either
// 0 - fail
// 1 - success and nothing changed
// 2 - success but pins reconfigured
// 3 - success but gains inverted
// 4 - success but pins reconfigured and gains inverted
return exit_flag;
}
v1.3/lib/Simple_FOC/src/current_sense/InlineCurrentSense.h
+65
View File
@@ -0,0 +1,65 @@
#ifndef INLINE_CS_LIB_H
#define INLINE_CS_LIB_H
#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"
class InlineCurrentSense: public CurrentSense{
public:
/**
InlineCurrentSense class constructor
@param shunt_resistor shunt resistor value
@param gain current-sense op-amp gain
@param phA A phase adc pin
@param phB B phase adc pin
@param phC C phase adc pin (optional)
*/
InlineCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET);
// CurrentSense interface implementing functions
void init() override;
PhaseCurrent_s getPhaseCurrents() override;
int driverSync(BLDCDriver *driver) override;
int driverAlign(BLDCDriver *driver, float voltage) override;
// ADC measuremnet gain for each phase
// support for different gains for different phases of more commonly - inverted phase currents
// this should be automated later
float gain_a; //!< phase A gain
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
int pinA; //!< pin A analog pin for current measurement
int pinB; //!< pin B analog pin for current measurement
int pinC; //!< pin C analog pin for current measurement
// gain variables
float shunt_resistor; //!< Shunt resistor value
float amp_gain; //!< amp gain value
float volts_to_amps_ratio; //!< Volts to amps ratio
/**
* Function finding zero offsets of the ADC
*/
void calibrateOffsets();
float offset_ia; //!< zero current A voltage value (center of the adc reading)
float offset_ib; //!< zero current B voltage value (center of the adc reading)
float offset_ic; //!< zero current C voltage value (center of the adc reading)
};
#endif
v1.3/lib/Simple_FOC/src/current_sense/LowsideCurrentSense.cpp
+179
View File
@@ -0,0 +1,179 @@
#include "LowsideCurrentSense.h"
// LowsideCurrentSensor constructor
// - shunt_resistor - shunt resistor value
// - gain - current-sense op-amp gain
// - phA - A phase adc pin
// - phB - B phase adc pin
// - phC - C phase adc pin (optional)
LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){
pinA = _pinA;
pinB = _pinB;
pinC = _pinC;
shunt_resistor = _shunt_resistor;
amp_gain = _gain;
volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps
// gains for each phase
gain_a = volts_to_amps_ratio;
gain_b = volts_to_amps_ratio;
gain_c = volts_to_amps_ratio;
}
// Lowside sensor init function
void LowsideCurrentSense::init(){
// configure ADC variables
_configureADCLowSide(pinA,pinB,pinC);
// sync the driver
_driverSyncLowSide();
// calibrate zero offsets
calibrateOffsets();
}
// Function finding zero offsets of the ADC
void LowsideCurrentSense::calibrateOffsets(){
const int calibration_rounds = 1000;
// find adc offset = zero current voltage
offset_ia = 0;
offset_ib = 0;
offset_ic = 0;
// read the adc voltage 1000 times ( arbitrary number )
for (int i = 0; i < calibration_rounds; i++) {
_startADC3PinConversionLowSide();
offset_ia += _readADCVoltageLowSide(pinA);
offset_ib += _readADCVoltageLowSide(pinB);
if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC);
_delay(1);
}
// calculate the mean offsets
offset_ia = offset_ia / calibration_rounds;
offset_ib = offset_ib / calibration_rounds;
if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
}
// 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
return current;
}
// Function synchronizing current sense with motor driver.
// for in-line sensig no such thing is necessary
int LowsideCurrentSense::driverSync(BLDCDriver *driver){
_driverSyncLowSide();
return 1;
}
// Function aligning the current sense with motor driver
// if all pins are connected well none of this is really necessary! - can be avoided
// returns flag
// 0 - fail
// 1 - success and nothing changed
// 2 - success but pins reconfigured
// 3 - success but gains inverted
// 4 - success but pins reconfigured and gains inverted
int LowsideCurrentSense::driverAlign(BLDCDriver *driver, float voltage){
int exit_flag = 1;
if(skip_align) return exit_flag;
// set phase A active and phases B and C down
driver->setPwm(voltage, 0, 0);
_delay(2000);
PhaseCurrent_s c = getPhaseCurrents();
// read the current 100 times ( arbitrary number )
for (int i = 0; i < 100; i++) {
PhaseCurrent_s c1 = getPhaseCurrents();
c.a = c.a*0.6f + 0.4f*c1.a;
c.b = c.b*0.6f + 0.4f*c1.b;
c.c = c.c*0.6f + 0.4f*c1.c;
_delay(3);
}
driver->setPwm(0, 0, 0);
// align phase A
float ab_ratio = fabs(c.a / c.b);
float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
if( ab_ratio > 1.5f ){ // should be ~2
gain_a *= _sign(c.a);
}else if( ab_ratio < 0.7f ){ // should be ~0.5
// switch phase A and B
int tmp_pinA = pinA;
pinA = pinB;
pinB = tmp_pinA;
gain_a *= _sign(c.b);
exit_flag = 2; // signal that pins have been switched
}else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5
// switch phase A and C
int tmp_pinA = pinA;
pinA = pinC;
pinC= tmp_pinA;
gain_a *= _sign(c.c);
exit_flag = 2;// signal that pins have been switched
}else{
// error in current sense - phase either not measured or bad connection
return 0;
}
// set phase B active and phases A and C down
driver->setPwm(0, voltage, 0);
_delay(200);
c = getPhaseCurrents();
// read the current 50 times
for (int i = 0; i < 100; i++) {
PhaseCurrent_s c1 = getPhaseCurrents();
c.a = c.a*0.6 + 0.4f*c1.a;
c.b = c.b*0.6 + 0.4f*c1.b;
c.c = c.c*0.6 + 0.4f*c1.c;
_delay(3);
}
driver->setPwm(0, 0, 0);
float ba_ratio = fabs(c.b/c.a);
float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
if( ba_ratio > 1.5f ){ // should be ~2
gain_b *= _sign(c.b);
}else if( ba_ratio < 0.7f ){ // it should be ~0.5
// switch phase A and B
int tmp_pinB = pinB;
pinB = pinA;
pinA = tmp_pinB;
gain_b *= _sign(c.a);
exit_flag = 2; // signal that pins have been switched
}else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
// switch phase A and C
int tmp_pinB = pinB;
pinB = pinC;
pinC = tmp_pinB;
gain_b *= _sign(c.c);
exit_flag = 2; // signal that pins have been switched
}else{
// error in current sense - phase either not measured or bad connection
return 0;
}
// if phase C measured
if(_isset(pinC)){
// set phase B active and phases A and C down
driver->setPwm(0, 0, voltage);
_delay(200);
c = getPhaseCurrents();
// read the adc voltage 500 times ( arbitrary number )
for (int i = 0; i < 50; i++) {
PhaseCurrent_s c1 = getPhaseCurrents();
c.c = (c.c+c1.c)/50.0f;
}
driver->setPwm(0, 0, 0);
gain_c *= _sign(c.c);
}
if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
// exit flag is either
// 0 - fail
// 1 - success and nothing changed
// 2 - success but pins reconfigured
// 3 - success but gains inverted
// 4 - success but pins reconfigured and gains inverted
return exit_flag;
}
v1.3/lib/Simple_FOC/src/current_sense/LowsideCurrentSense.h
+66
View File
@@ -0,0 +1,66 @@
#ifndef LOWSIDE_CS_LIB_H
#define LOWSIDE_CS_LIB_H
#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"
class LowsideCurrentSense: public CurrentSense{
public:
/**
LowsideCurrentSense class constructor
@param shunt_resistor shunt resistor value
@param gain current-sense op-amp gain
@param phA A phase adc pin
@param phB B phase adc pin
@param phC C phase adc pin (optional)
*/
LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET);
// CurrentSense interface implementing functions
void init() override;
PhaseCurrent_s getPhaseCurrents() override;
int driverSync(BLDCDriver *driver) override;
int driverAlign(BLDCDriver *driver, float voltage) override;
// ADC measuremnet gain for each phase
// support for different gains for different phases of more commonly - inverted phase currents
// this should be automated later
float gain_a; //!< phase A gain
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
int pinA; //!< pin A analog pin for current measurement
int pinB; //!< pin B analog pin for current measurement
int pinC; //!< pin C analog pin for current measurement
// gain variables
float shunt_resistor; //!< Shunt resistor value
float amp_gain; //!< amp gain value
float volts_to_amps_ratio; //!< Volts to amps ratio
/**
* Function finding zero offsets of the ADC
*/
void calibrateOffsets();
float offset_ia; //!< zero current A voltage value (center of the adc reading)
float offset_ib; //!< zero current B voltage value (center of the adc reading)
float offset_ic; //!< zero current C voltage value (center of the adc reading)
};
#endif
v1.3/lib/Simple_FOC/src/current_sense/hardware_api.h
+46
View File
@@ -0,0 +1,46 @@
#ifndef HARDWARE_UTILS_CURRENT_H
#define HARDWARE_UTILS_CURRENT_H
#include "../common/foc_utils.h"
#include "../common/time_utils.h"
/**
* function reading an ADC value and returning the read voltage
*
* @param pinA - the arduino pin to be read (it has to be ADC pin)
*/
float _readADCVoltageInline(const int pinA);
/**
* function reading an ADC value and returning the read voltage
*
* @param pinA - adc pin A
* @param pinB - adc pin B
* @param pinC - adc pin C
*/
void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET);
/**
* function reading an ADC value and returning the read voltage
*
* @param pinA - adc pin A
* @param pinB - adc pin B
* @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
*
* @param pinA - the arduino pin to be read (it has to be ADC pin)
*/
float _readADCVoltageLowSide(const int pinA);
/**
* function syncing the Driver with the ADC for the LowSide Sensing
*/
void _driverSyncLowSide();
#endif
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/atmega_mcu.cpp
+55
View File
@@ -0,0 +1,55 @@
#include "../hardware_api.h"
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__)
#define _ADC_VOLTAGE 5.0f
#define _ADC_RESOLUTION 1024.0f
// 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
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/due_mcu.cpp
+38
View File
@@ -0,0 +1,38 @@
#include "../hardware_api.h"
#if defined(__arm__) && defined(__SAM3X8E__)
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 1024.0f
// 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
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/esp32_adc_driver.cpp
+225
View File
@@ -0,0 +1,225 @@
#include "esp32_adc_driver.h"
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED)
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "rom/ets_sys.h"
#include "esp_attr.h"
#include "esp_intr.h"
#include "soc/rtc_io_reg.h"
#include "soc/rtc_cntl_reg.h"
#include "soc/sens_reg.h"
static uint8_t __analogAttenuation = 3;//11db
static uint8_t __analogWidth = 3;//12 bits
static uint8_t __analogCycles = 8;
static uint8_t __analogSamples = 0;//1 sample
static uint8_t __analogClockDiv = 1;
// Width of returned answer ()
static uint8_t __analogReturnedWidth = 12;
void __analogSetWidth(uint8_t bits){
if(bits < 9){
bits = 9;
} else if(bits > 12){
bits = 12;
}
__analogReturnedWidth = bits;
__analogWidth = bits - 9;
SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S);
SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S);
SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S);
SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S);
}
void __analogSetCycles(uint8_t cycles){
__analogCycles = cycles;
SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S);
SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S);
}
void __analogSetSamples(uint8_t samples){
if(!samples){
return;
}
__analogSamples = samples - 1;
SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S);
SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S);
}
void __analogSetClockDiv(uint8_t clockDiv){
if(!clockDiv){
return;
}
__analogClockDiv = clockDiv;
SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S);
SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S);
}
void __analogSetAttenuation(uint8_t attenuation)
{
__analogAttenuation = attenuation & 3;
uint32_t att_data = 0;
int i = 10;
while(i--){
att_data |= __analogAttenuation << (i * 2);
}
WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels
WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data);
}
void IRAM_ATTR __analogInit(){
static bool initialized = false;
if(initialized){
return;
}
__analogSetAttenuation(__analogAttenuation);
__analogSetCycles(__analogCycles);
__analogSetSamples(__analogSamples + 1);//in samples
__analogSetClockDiv(__analogClockDiv);
__analogSetWidth(__analogWidth + 9);//in bits
SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV);
SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV);
SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW
SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW
SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW
SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW
CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM
SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0
CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_CTRL_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM
SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S);
SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S);
SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S);
while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm==
initialized = true;
}
void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation)
{
int8_t channel = digitalPinToAnalogChannel(pin);
if(channel < 0 || attenuation > 3){
return ;
}
__analogInit();
if(channel > 7){
SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2));
} else {
SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2));
}
}
bool IRAM_ATTR __adcAttachPin(uint8_t pin){
int8_t channel = digitalPinToAnalogChannel(pin);
if(channel < 0){
return false;//not adc pin
}
int8_t pad = digitalPinToTouchChannel(pin);
if(pad >= 0){
uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG);
if(touch & (1 << pad)){
touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S))
| (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S))
| (1 << (pad + SENS_TOUCH_PAD_WORKEN_S)));
WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch);
}
} else if(pin == 25){
CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1
} else if(pin == 26){
CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2
}
pinMode(pin, ANALOG);
__analogInit();
return true;
}
bool IRAM_ATTR __adcStart(uint8_t pin){
int8_t channel = digitalPinToAnalogChannel(pin);
if(channel < 0){
return false;//not adc pin
}
if(channel > 9){
channel -= 10;
CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
} else {
CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S);
SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
}
return true;
}
bool IRAM_ATTR __adcBusy(uint8_t pin){
int8_t channel = digitalPinToAnalogChannel(pin);
if(channel < 0){
return false;//not adc pin
}
if(channel > 7){
return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0);
}
return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0);
}
uint16_t IRAM_ATTR __adcEnd(uint8_t pin)
{
uint16_t value = 0;
int8_t channel = digitalPinToAnalogChannel(pin);
if(channel < 0){
return 0;//not adc pin
}
if(channel > 7){
while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion
value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S);
} else {
while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion
value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S);
}
// Shift result if necessary
uint8_t from = __analogWidth + 9;
if (from == __analogReturnedWidth) {
return value;
}
if (from > __analogReturnedWidth) {
return value >> (from - __analogReturnedWidth);
}
return value << (__analogReturnedWidth - from);
}
void __analogReadResolution(uint8_t bits)
{
if(!bits || bits > 16){
return;
}
__analogSetWidth(bits); // hadware from 9 to 12
__analogReturnedWidth = bits; // software from 1 to 16
}
uint16_t IRAM_ATTR adcRead(uint8_t pin)
{
if(!__adcAttachPin(pin) || !__adcStart(pin)){
return 0;
}
return __adcEnd(pin);
}
#endif
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/esp32_adc_driver.h
+88
View File
@@ -0,0 +1,88 @@
#ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_
#define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_
#include "Arduino.h"
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED)
/*
* Get ADC value for pin
* */
uint16_t adcRead(uint8_t pin);
/*
* Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096).
* If between 9 and 12, it will equal the set hardware resolution, else value will be shifted.
* Range is 1 - 16
*
* Note: compatibility with Arduino SAM
*/
void __analogReadResolution(uint8_t bits);
/*
* Sets the sample bits and read resolution
* Default is 12bit (0 - 4095)
* Range is 9 - 12
* */
void __analogSetWidth(uint8_t bits);
/*
* Set number of cycles per sample
* Default is 8 and seems to do well
* Range is 1 - 255
* */
void __analogSetCycles(uint8_t cycles);
/*
* Set number of samples in the range.
* Default is 1
* Range is 1 - 255
* This setting splits the range into
* "samples" pieces, which could look
* like the sensitivity has been multiplied
* that many times
* */
void __analogSetSamples(uint8_t samples);
/*
* Set the divider for the ADC clock.
* Default is 1
* Range is 1 - 255
* */
void __analogSetClockDiv(uint8_t clockDiv);
/*
* Set the attenuation for all channels
* Default is 11db
* */
void __analogSetAttenuation(uint8_t attenuation);
/*
* Set the attenuation for particular pin
* Default is 11db
* */
void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation);
/*
* Attach pin to ADC (will also clear any other analog mode that could be on)
* */
bool __adcAttachPin(uint8_t pin);
/*
* Start ADC conversion on attached pin's bus
* */
bool __adcStart(uint8_t pin);
/*
* Check if conversion on the pin's ADC bus is currently running
* */
bool __adcBusy(uint8_t pin);
/*
* Get the result of the conversion (will wait if it have not finished)
* */
uint16_t __adcEnd(uint8_t pin);
#endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */
#endif /* ESP32 */
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/esp32_ledc_mcu.cpp
+30
View File
@@ -0,0 +1,30 @@
#include "../hardware_api.h"
#include "../../drivers/hardware_api.h"
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(SOC_MCPWM_SUPPORTED)
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 4095.0f
// adc counts to voltage conversion ratio
// some optimizing for faster execution
#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
/**
* Inline adc reading implementation
*/
// function reading an ADC value and returning the read voltage
float _readADCVoltageInline(const int pinA){
uint32_t raw_adc = analogRead(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);
}
#endif
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/esp32_mcu.cpp
+124
View File
@@ -0,0 +1,124 @@
#include "../hardware_api.h"
#include "../../drivers/hardware_api.h"
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED)
#include "esp32_adc_driver.h"
#include "driver/mcpwm.h"
#include "soc/mcpwm_reg.h"
#include "soc/mcpwm_struct.h"
#include <soc/sens_reg.h>
#include <soc/sens_struct.h>
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 4095.0f
// adc counts to voltage conversion ratio
// some optimizing for faster execution
#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
/**
* Inline adc reading implementation
*/
// function reading an ADC value and returning the read voltage
float _readADCVoltageInline(const int pinA){
uint32_t raw_adc = adcRead(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);
}
/**
* Low side adc reading implementation
*/
static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1};
int a1, a2, a3; // Current readings from internal current sensor amplifiers
int _pinA, _pinB, _pinC;
static void IRAM_ATTR isr_handler(void*);
byte currentState = 1;
// function reading an ADC value and returning the read voltage
float _readADCVoltageLowSide(const int pin){
uint32_t raw_adc;
if (pin == _pinA) raw_adc = a1;
else if (pin == _pinB) raw_adc = a2;
else if (pin == _pinC) raw_adc = a3;
return raw_adc * _ADC_CONV;
}
// function reading an ADC value and returning the read voltage
void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
_pinA = pinA;
_pinB = pinB;
_pinC = pinC;
pinMode(pinA, INPUT);
pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
}
void _driverSyncLowSide(){
// high side registers enable interrupt
// MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tez_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt
// MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tez_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt
// if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tez_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt
// low-side register enable interrupt
MCPWM[MCPWM_UNIT_0]->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt
MCPWM[MCPWM_UNIT_0]->int_ena.timer1_tep_int_ena = true;//A PWM timer 1 TEP event will trigger this interrupt
if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_ena.timer2_tep_int_ena = true;//A PWM timer 2 TEP event will trigger this interrupt
mcpwm_isr_register(MCPWM_UNIT_0, isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler
}
// Read currents when interrupt is triggered
static void IRAM_ATTR isr_handler(void*){
// // high side
// uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tez_int_st;
// uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tez_int_st;
// uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tez_int_st : 0;
// low side
uint32_t mcpwm_intr_status_0 = MCPWM[MCPWM_UNIT_0]->int_st.timer0_tep_int_st;
uint32_t mcpwm_intr_status_1 = MCPWM[MCPWM_UNIT_0]->int_st.timer1_tep_int_st;
uint32_t mcpwm_intr_status_2 = _isset(_pinC) ? MCPWM[MCPWM_UNIT_0]->int_st.timer2_tep_int_st : 0;
switch (currentState)
{
case 1 :
if (mcpwm_intr_status_0 > 0) a1 = adcRead(_pinA);
currentState = 2;
break;
case 2 :
if (mcpwm_intr_status_1 > 0) a2 = adcRead(_pinB);
currentState = _isset(_pinC) ? 3 : 1;
break;
case 3 :
if (mcpwm_intr_status_2 > 0) a3 = adcRead(_pinC);
currentState = 1;
break;
}
// high side
// MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tez_int_clr = mcpwm_intr_status_0;
// MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tez_int_clr = mcpwm_intr_status_1;
// if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tez_int_clr = mcpwm_intr_status_2;
// low side
MCPWM[MCPWM_UNIT_0]->int_clr.timer0_tep_int_clr = mcpwm_intr_status_0;
MCPWM[MCPWM_UNIT_0]->int_clr.timer1_tep_int_clr = mcpwm_intr_status_1;
if( _isset(_pinC) ) MCPWM[MCPWM_UNIT_0]->int_clr.timer2_tep_int_clr = mcpwm_intr_status_2;
}
#endif
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/generic_mcu.cpp
+31
View File
@@ -0,0 +1,31 @@
#include "../hardware_api.h"
// function reading an ADC value and returning the read voltage
__attribute__((weak)) float _readADCVoltageInline(const int pinA){
uint32_t raw_adc = analogRead(pinA);
return raw_adc * 5.0f/1024.0f;
}
// function reading an ADC value and returning the read voltage
__attribute__((weak)) 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
__attribute__((weak)) float _readADCVoltageLowSide(const int pinA){
return _readADCVoltageInline(pinA);
}
// Configure low side for generic mcu
// cannot do much but
__attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
pinMode(pinA, INPUT);
pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
}
// sync driver and the adc
__attribute__((weak)) void _driverSyncLowSide(){ }
__attribute__((weak)) void _startADC3PinConversionLowSide(){ }
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/samd21_mcu.cpp
+311
View File
@@ -0,0 +1,311 @@
#ifdef _SAMD21_
#include "samd21_mcu.h"
#include "../hardware_api.h"
static bool freeRunning = false;
static int _pinA, _pinB, _pinC;
static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode
static SAMDCurrentSenseADCDMA instance;
/**
* function reading an ADC value and returning the read voltage
*
* @param pinA - adc pin A
* @param pinB - adc pin B
* @param pinC - adc pin C
*/
void _configureADCLowSide(const int pinA,const int pinB,const int pinC)
{
_pinA = pinA;
_pinB = pinB;
_pinC = pinC;
freeRunning = true;
instance.init(pinA, pinB, pinC);
}
void _startADC3PinConversionLowSide()
{
instance.startADCScan();
}
/**
* function reading an ADC value and returning the read voltage
*
* @param pinA - the arduino pin to be read (it has to be ADC pin)
*/
float _readADCVoltageLowSide(const int pinA)
{
instance.readResults(a, b, c);
if(pinA == _pinA)
return instance.toVolts(a);
if(pinA == _pinB)
return instance.toVolts(b);
if(pinA == _pinC)
return instance.toVolts(c);
return NAN;
}
/**
* function syncing the Driver with the ADC for the LowSide Sensing
*/
void _driverSyncLowSide()
{
SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented"));
instance.startADCScan();
//TODO: hook with PWM interrupts
}
// Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless
static void adcStopWithDMA(void);
static void adcStartWithDMA(void);
/**
* @brief ADC sync wait
* @retval void
*/
static __inline__ void ADCsync() __attribute__((always_inline, unused));
static void ADCsync() {
while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free
}
// ADC DMA sequential free running (6) with Interrupts /////////////////
SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance()
{
return &instance;
}
SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA()
{
}
void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA)
{
this->pinA = pinA;
this->pinB = pinB;
this->pinC = pinC;
this->pinAREF = pinAREF;
this->channelDMA = channelDMA;
this->voltageAREF = voltageAREF;
this->maxCountsADC = 1 << adcBits;
countsToVolts = ( voltageAREF / maxCountsADC );
initPins();
initADC();
initDMA();
startADCScan(); //so we have something to read next time we call readResults()
}
void SAMDCurrentSenseADCDMA::startADCScan(){
adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize);
adcStartWithDMA();
}
bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){
if(ADC->CTRLA.bit.ENABLE)
return false;
uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber;
uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber;
a = adcBuffer[ainA];
b = adcBuffer[ainB];
if(_isset(pinC))
{
uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber;
c = adcBuffer[ainC];
}
return true;
}
float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) {
return counts * countsToVolts;
}
void SAMDCurrentSenseADCDMA::initPins(){
if (pinAREF>=0)
pinMode(pinAREF, INPUT);
pinMode(pinA, INPUT);
pinMode(pinB, INPUT);
uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber;
uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber;
firstAIN = min(ainA, ainB);
lastAIN = max(ainA, ainB);
if( _isset(pinC) )
{
uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber;
pinMode(pinC, INPUT);
firstAIN = min(firstAIN, ainC);
lastAIN = max(lastAIN, ainC);
}
oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout
BufferSize = lastAIN - oneBeforeFirstAIN + 1;
}
void SAMDCurrentSenseADCDMA::initADC(){
analogRead(pinA); // do some pin init pinPeripheral()
analogRead(pinB); // do some pin init pinPeripheral()
analogRead(pinC); // do some pin init pinPeripheral()
ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC
ADCsync();
//ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA
ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X
// ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; // default
if (pinAREF>=0)
ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA;
else
ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0;
ADCsync(); // ref 31.6.16
/*
Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan
This register gives the number of input sources included in the pin scan. The number of input sources included is
INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS +
INPUTOFFSET + INPUTSCAN.
The range of the scan mode must not exceed the number of input channels available on the device.
Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection
These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If
the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit
group in the SamplingControl register must be written.
Table 32-14. Positive Mux Input Selection
MUXPOS[4:0] Group configuration Description
0x00 PIN0 ADC AIN0 pin
0x01 PIN1 ADC AIN1 pin
0x02 PIN2 ADC AIN2 pin
0x03 PIN3 ADC AIN3 pin
0x04 PIN4 ADC AIN4 pin
0x05 PIN5 ADC AIN5 pin
0x06 PIN6 ADC AIN6 pin
0x07 PIN7 ADC AIN7 pin
0x08 PIN8 ADC AIN8 pin
0x09 PIN9 ADC AIN9 pin
0x0A PIN10 ADC AIN10 pin
0x0B PIN11 ADC AIN11 pin
0x0C PIN12 ADC AIN12 pin
0x0D PIN13 ADC AIN13 pin
0x0E PIN14 ADC AIN14 pin
0x0F PIN15 ADC AIN15 pin
0x10 PIN16 ADC AIN16 pin
0x11 PIN17 ADC AIN17 pin
0x12 PIN18 ADC AIN18 pin
0x13 PIN19 ADC AIN19 pin
0x14-0x17 Reserved
0x18 TEMP Temperature reference
0x19 BANDGAP Bandgap voltage
0x1A SCALEDCOREVCC 1/4 scaled core supply
0x1B SCALEDIOVCC 1/4 scaled I/O supply
0x1C DAC DAC output
0x1D-0x1F Reserved
*/
ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN;
ADCsync();
ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive)
ADCsync();
ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor
ADCsync();
ADC->AVGCTRL.reg = 0x00 ; //no averaging
ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16
// according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU
ADCsync();
ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT;
ADCsync();
}
volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16)));
void SAMDCurrentSenseADCDMA::initDMA() {
// probably on by default
PM->AHBMASK.reg |= PM_AHBMASK_DMAC ;
PM->APBBMASK.reg |= PM_APBBMASK_DMAC ;
NVIC_EnableIRQ( DMAC_IRQn ) ;
DMAC->BASEADDR.reg = (uint32_t)descriptor_section;
DMAC->WRBADDR.reg = (uint32_t)wrb;
DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf);
}
void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) {
DMAC->CHID.reg = DMAC_CHID_ID(channelDMA);
DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE;
DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST;
DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA));
DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0)
| DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY)
| DMAC_CHCTRLB_TRIGACT_BEAT;
DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts
descriptor.descaddr = 0;
descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg;
descriptor.btcnt = hwords;
descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address
descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID;
memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor));
// start channel
DMAC->CHID.reg = DMAC_CHID_ID(channelDMA);
DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
}
int iii = 0;
void adcStopWithDMA(void){
ADCsync();
ADC->CTRLA.bit.ENABLE = 0x00;
// ADCsync();
// if(iii++ % 1000 == 0)
// {
// SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a);
// SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: ");
// SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b);
// SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: ");
// SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c);
// SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!");
// }
}
void adcStartWithDMA(void){
ADCsync();
ADC->INPUTCTRL.bit.INPUTOFFSET = 0;
ADCsync();
ADC->SWTRIG.bit.FLUSH = 1;
ADCsync();
ADC->CTRLA.bit.ENABLE = 0x01;
ADCsync();
}
void DMAC_Handler() {
uint8_t active_channel;
active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number
DMAC->CHID.reg = DMAC_CHID_ID(active_channel);
adcStopWithDMA();
DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear
DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR;
DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP;
}
#endif
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/samd21_mcu.h
+69
View File
@@ -0,0 +1,69 @@
#ifdef _SAMD21_
#ifndef CURRENT_SENSE_SAMD21_H
#define CURRENT_SENSE_SAMD21_H
#define SIMPLEFOC_SAMD_DEBUG
#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL)
#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial
#endif
#include <stdint.h>
typedef struct {
uint16_t btctrl;
uint16_t btcnt;
uint32_t srcaddr;
uint32_t dstaddr;
uint32_t descaddr;
} dmacdescriptor ;
// AREF pin is 42
class SAMDCurrentSenseADCDMA
{
public:
static SAMDCurrentSenseADCDMA * getHardwareAPIInstance();
SAMDCurrentSenseADCDMA();
void init(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3);
void startADCScan();
bool readResults(uint16_t & a, uint16_t & b, uint16_t & c);
float toVolts(uint16_t counts);
private:
void adcToDMATransfer(void *rxdata, uint32_t hwords);
void initPins();
void initADC();
void initDMA();
uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout
uint32_t firstAIN;
uint32_t lastAIN;
uint32_t BufferSize = 0;
uint16_t adcBuffer[20];
uint32_t pinA;
uint32_t pinB;
uint32_t pinC;
uint32_t pinAREF;
uint32_t channelDMA; // DMA channel
bool freeRunning;
float voltageAREF;
float maxCountsADC;
float countsToVolts;
dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16)));
dmacdescriptor descriptor __attribute__ ((aligned (16)));
};
#endif
#endif
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/samd_mcu.cpp
+37
View File
@@ -0,0 +1,37 @@
#include "../hardware_api.h"
#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_)
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 1024.0f
// 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
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/stm32_mcu.cpp
+39
View File
@@ -0,0 +1,39 @@
#include "../hardware_api.h"
#if defined(_STM32_DEF_) and !defined(STM32G4xx)
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 1024.0f
// 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
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/stm32g4_hal.cpp
+333
View File
@@ -0,0 +1,333 @@
#include "../hardware_api.h"
#if defined(STM32G4xx)
#include "stm32g4xx_hal.h"
#include "stm32g4xx_ll_pwr.h"
#include "stm32g4xx_hal_adc.h"
#include "stm32g4_hal.h"
// From STM32 cube IDE
/**
******************************************************************************
* @attention
*
* <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under BSD 3-Clause license,
* the "License"; You may not use this file except in compliance with the
* License. You may obtain a copy of the License at:
* opensource.org/licenses/BSD-3-Clause
*
******************************************************************************
*/
/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
void MX_GPIO_Init(void)
{
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOF_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_ADC12_CLK_ENABLE();
}
/**
* Enable DMA controller clock
*/
void MX_DMA_Init(void)
{
/* DMA controller clock enable */
__HAL_RCC_DMAMUX1_CLK_ENABLE();
__HAL_RCC_DMA1_CLK_ENABLE();
__HAL_RCC_DMA2_CLK_ENABLE();
/* DMA interrupt init */
/* DMA1_Channel1_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn);
/* DMA1_Channel2_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
// Enable external clock for ADC
RCC_PeriphCLKInitTypeDef PeriphClkInit;
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC12;
PeriphClkInit.Adc12ClockSelection = RCC_ADC12CLKSOURCE_PLL;
HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);
}
/**
* @brief ADC1 Initialization Function
* @param None
* @retval None
*/
void MX_ADC1_Init(ADC_HandleTypeDef* hadc1)
{
/* USER CODE BEGIN ADC1_Init 0 */
/* USER CODE END ADC1_Init 0 */
ADC_MultiModeTypeDef multimode = {0};
ADC_ChannelConfTypeDef sConfig = {0};
/* USER CODE BEGIN ADC1_Init 1 */
/* USER CODE END ADC1_Init 1 */
/** Common config
*/
hadc1->Instance = ADC1;
hadc1->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16;
hadc1->Init.Resolution = ADC_RESOLUTION_12B;
hadc1->Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc1->Init.GainCompensation = 0;
hadc1->Init.ScanConvMode = ADC_SCAN_ENABLE;
hadc1->Init.EOCSelection = ADC_EOC_SINGLE_CONV;
hadc1->Init.LowPowerAutoWait = DISABLE;
hadc1->Init.ContinuousConvMode = DISABLE;
hadc1->Init.NbrOfConversion = 2;
hadc1->Init.DiscontinuousConvMode = DISABLE;
hadc1->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO;
hadc1->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
hadc1->Init.DMAContinuousRequests = ENABLE;
hadc1->Init.Overrun = ADC_OVR_DATA_PRESERVED;
if (HAL_ADC_Init(hadc1) != HAL_OK)
{
Error_Handler();
}
/** Configure the ADC multi-mode
*/
multimode.Mode = ADC_MODE_INDEPENDENT;
if (HAL_ADCEx_MultiModeConfigChannel(hadc1, &multimode) != HAL_OK)
{
Error_Handler();
}
/** Configure Regular Channel
*/
sConfig.Channel = ADC_CHANNEL_12; // ADC1_IN12 = PB1 = OP3_OUT
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
sConfig.OffsetNumber = ADC_OFFSET_NONE;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}
/** Configure Regular Channel
*/
sConfig.Channel = ADC_CHANNEL_3; // ADC1_IN3 = PA2 = OP1_OUT
sConfig.Rank = ADC_REGULAR_RANK_2;
if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN ADC1_Init 2 */
/* USER CODE END ADC1_Init 2 */
}
/**
* @brief ADC2 Initialization Function
* @param None
* @retval None
*/
void MX_ADC2_Init(ADC_HandleTypeDef* hadc2)
{
/* USER CODE BEGIN ADC2_Init 0 */
/* USER CODE END ADC2_Init 0 */
ADC_ChannelConfTypeDef sConfig = {0};
/* USER CODE BEGIN ADC2_Init 1 */
/* USER CODE END ADC2_Init 1 */
/** Common config
*/
hadc2->Instance = ADC2;
hadc2->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16;
hadc2->Init.Resolution = ADC_RESOLUTION_12B;
hadc2->Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc2->Init.GainCompensation = 0;
hadc2->Init.ScanConvMode = ADC_SCAN_ENABLE;
hadc2->Init.EOCSelection = ADC_EOC_SINGLE_CONV;
hadc2->Init.LowPowerAutoWait = DISABLE;
hadc2->Init.ContinuousConvMode = DISABLE;
hadc2->Init.NbrOfConversion = 1;
hadc2->Init.DiscontinuousConvMode = DISABLE;
hadc2->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO;
hadc2->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
hadc2->Init.DMAContinuousRequests = ENABLE;
hadc2->Init.Overrun = ADC_OVR_DATA_PRESERVED;
if (HAL_ADC_Init(hadc2) != HAL_OK)
{
Error_Handler();
}
/** Configure Regular Channel
*/
sConfig.Channel = ADC_CHANNEL_3; // ADC2_IN3 = PA6
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
sConfig.OffsetNumber = ADC_OFFSET_NONE;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN ADC2_Init 2 */
/* USER CODE END ADC2_Init 2 */
}
/**
* @brief OPAMP MSP Initialization
* This function configures the hardware resources used in this example
* @param hopamp-> OPAMP handle pointer
* @retval None
*/
void HAL_OPAMP_MspInit(OPAMP_HandleTypeDef* hopamp)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(hopamp->Instance==OPAMP1)
{
/* USER CODE BEGIN OPAMP1_MspInit 0 */
/* USER CODE END OPAMP1_MspInit 0 */
__HAL_RCC_GPIOA_CLK_ENABLE();
/**OPAMP1 GPIO Configuration
PA1 ------> OPAMP1_VINP
PA2 ------> OPAMP1_VOUT
PA3 ------> OPAMP1_VINM
*/
GPIO_InitStruct.Pin = GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* USER CODE BEGIN OPAMP1_MspInit 1 */
/* USER CODE END OPAMP1_MspInit 1 */
}
else if(hopamp->Instance==OPAMP2)
{
/* USER CODE BEGIN OPAMP2_MspInit 0 */
/* USER CODE END OPAMP2_MspInit 0 */
__HAL_RCC_GPIOA_CLK_ENABLE();
/**OPAMP2 GPIO Configuration
PA5 ------> OPAMP2_VINM
PA6 ------> OPAMP2_VOUT
PA7 ------> OPAMP2_VINP
*/
GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* USER CODE BEGIN OPAMP2_MspInit 1 */
/* USER CODE END OPAMP2_MspInit 1 */
}
else if(hopamp->Instance==OPAMP3)
{
/* USER CODE BEGIN OPAMP3_MspInit 0 */
/* USER CODE END OPAMP3_MspInit 0 */
__HAL_RCC_GPIOB_CLK_ENABLE();
/**OPAMP3 GPIO Configuration
PB0 ------> OPAMP3_VINP
PB1 ------> OPAMP3_VOUT
PB2 ------> OPAMP3_VINM
*/
GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* USER CODE BEGIN OPAMP3_MspInit 1 */
/* USER CODE END OPAMP3_MspInit 1 */
}
}
/**
* @brief OPAMP MSP De-Initialization
* This function freeze the hardware resources used in this example
* @param hopamp-> OPAMP handle pointer
* @retval None
*/
void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef* hopamp)
{
if(hopamp->Instance==OPAMP1)
{
/* USER CODE BEGIN OPAMP1_MspDeInit 0 */
/* USER CODE END OPAMP1_MspDeInit 0 */
/**OPAMP1 GPIO Configuration
PA1 ------> OPAMP1_VINP
PA2 ------> OPAMP1_VOUT
PA3 ------> OPAMP1_VINM
*/
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3);
/* USER CODE BEGIN OPAMP1_MspDeInit 1 */
/* USER CODE END OPAMP1_MspDeInit 1 */
}
else if(hopamp->Instance==OPAMP2)
{
/* USER CODE BEGIN OPAMP2_MspDeInit 0 */
/* USER CODE END OPAMP2_MspDeInit 0 */
/**OPAMP2 GPIO Configuration
PA5 ------> OPAMP2_VINM
PA6 ------> OPAMP2_VOUT
PA7 ------> OPAMP2_VINP
*/
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7);
/* USER CODE BEGIN OPAMP2_MspDeInit 1 */
/* USER CODE END OPAMP2_MspDeInit 1 */
}
else if(hopamp->Instance==OPAMP3)
{
/* USER CODE BEGIN OPAMP3_MspDeInit 0 */
/* USER CODE END OPAMP3_MspDeInit 0 */
/**OPAMP3 GPIO Configuration
PB0 ------> OPAMP3_VINP
PB1 ------> OPAMP3_VOUT
PB2 ------> OPAMP3_VINM
*/
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2);
/* USER CODE BEGIN OPAMP3_MspDeInit 1 */
/* USER CODE END OPAMP3_MspDeInit 1 */
}
}
#endif
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/stm32g4_hal.h
+18
View File
@@ -0,0 +1,18 @@
#ifndef stm32g4_hal
#define stm32g4_hal
#if defined(STM32G4xx)
#include <stm32g4xx_hal_adc.h>
#include <stm32g4xx_hal_opamp.h>
void MX_GPIO_Init(void);
void MX_DMA_Init(void);
void MX_ADC1_Init(ADC_HandleTypeDef* hadc1);
void MX_ADC2_Init(ADC_HandleTypeDef* hadc2);
void MX_OPAMP1_Init(OPAMP_HandleTypeDef* hopamp);
void MX_OPAMP2_Init(OPAMP_HandleTypeDef* hopamp);
void MX_OPAMP3_Init(OPAMP_HandleTypeDef* hopamp);
#endif
#endif
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/stm32g4_mcu.cpp
+130
View File
@@ -0,0 +1,130 @@
#include "../hardware_api.h"
#include "stm32g4_hal.h"
#if defined(STM32G4xx)
#define _ADC_VOLTAGE 3.3
#define _ADC_RESOLUTION 4096.0
#define ADC_BUF_LEN_1 2
#define ADC_BUF_LEN_2 1
static ADC_HandleTypeDef hadc1;
static ADC_HandleTypeDef hadc2;
static OPAMP_HandleTypeDef hopamp1;
static OPAMP_HandleTypeDef hopamp2;
static OPAMP_HandleTypeDef hopamp3;
static DMA_HandleTypeDef hdma_adc1;
static DMA_HandleTypeDef hdma_adc2;
uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion
uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion
#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) )
// function reading an ADC value and returning the read voltage
// As DMA is being used just return the DMA result
float _readADCVoltageInline(const int pin){
uint32_t raw_adc = 0;
if(pin == PA2) // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1
raw_adc = adcBuffer1[1];
else if(pin == PA6) // = ADC2_IN3 = phase V (OP2_OUT) on B-G431B-ESC1
raw_adc = adcBuffer2[0];
else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1
raw_adc = adcBuffer1[0];
return raw_adc * _ADC_CONV;
}
// do the same for low side sensing
float _readADCVoltageLowSide(const int pin){
return _readADCVoltageInline(pin);
}
void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){
// could this be replaced with LL_OPAMP calls??
hopamp->Instance = OPAMPx_Def;
hopamp->Init.PowerMode = OPAMP_POWERMODE_HIGHSPEED;
hopamp->Init.Mode = OPAMP_PGA_MODE;
hopamp->Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0;
hopamp->Init.InternalOutput = DISABLE;
hopamp->Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
hopamp->Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_IO0_BIAS;
hopamp->Init.PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15;
hopamp->Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
if (HAL_OPAMP_Init(hopamp) != HAL_OK)
{
Error_Handler();
}
}
void _configureOPAMPs(OPAMP_HandleTypeDef *OPAMPA, OPAMP_HandleTypeDef *OPAMPB, OPAMP_HandleTypeDef *OPAMPC){
// Configure the opamps
_configureOPAMP(OPAMPA, OPAMP1);
_configureOPAMP(OPAMPB, OPAMP2);
_configureOPAMP(OPAMPC, OPAMP3);
}
void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Channel_TypeDef* channel, uint32_t request) {
hdma_adc->Instance = channel;
hdma_adc->Init.Request = request;
hdma_adc->Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_adc->Init.PeriphInc = DMA_PINC_DISABLE;
hdma_adc->Init.MemInc = DMA_MINC_ENABLE;
hdma_adc->Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
hdma_adc->Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
hdma_adc->Init.Mode = DMA_CIRCULAR;
hdma_adc->Init.Priority = DMA_PRIORITY_LOW;
HAL_DMA_DeInit(hdma_adc);
if (HAL_DMA_Init(hdma_adc) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc);
}
void _configureADCInline(const int pinA,const int pinB,const int pinC){
HAL_Init();
MX_GPIO_Init();
MX_DMA_Init();
_configureOPAMPs(&hopamp1, &hopamp3, &hopamp2);
MX_ADC1_Init(&hadc1);
MX_ADC2_Init(&hadc2);
MX_DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1);
MX_DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2);
if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adcBuffer1, ADC_BUF_LEN_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adcBuffer2, ADC_BUF_LEN_2) != HAL_OK)
{
Error_Handler();
}
HAL_OPAMP_Start(&hopamp1);
HAL_OPAMP_Start(&hopamp2);
HAL_OPAMP_Start(&hopamp3);
// Check if the ADC DMA is collecting any data.
// If this fails, it likely means timer1 has not started. Verify that your application starts
// the motor pwm (usually BLDCDriver6PWM::init()) before initializing the ADC engine.
_delay(5);
if (adcBuffer1[0] == 0 || adcBuffer1[1] == 0 || adcBuffer2[0] == 0) {
Error_Handler();
}
}
// do the same for low side
void _configureADCLowSide(const int pinA,const int pinB,const int pinC){
_configureADCInline(pinA, pinB, pinC);
}
extern "C" {
void DMA1_Channel1_IRQHandler(void) {
HAL_DMA_IRQHandler(&hdma_adc1);
}
void DMA1_Channel2_IRQHandler(void) {
HAL_DMA_IRQHandler(&hdma_adc2);
}
}
#endif
v1.3/lib/Simple_FOC/src/current_sense/hardware_specific/teensy_mcu.cpp
+37
View File
@@ -0,0 +1,37 @@
#include "../hardware_api.h"
#if defined(__arm__) && defined(CORE_TEENSY)
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 1024.0f
// 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