matei/lemon-pepper-stepper
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

try to fix submodule

Browse Source
This commit is contained in:
matei jordache
2023-11-09 19:02:15 -05:00
parent c1d45aa443
commit deea94b076
366 changed files with 40228 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

163
firmware/lib/Arduino-FOC/src/current_sense/GenericCurrentSense.cpp Normal file
View File

@@ -0,0 +1,163 @@
#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
int GenericCurrentSense::init(){
// configure ADC variables
if(initCallback != nullptr) initCallback();
// calibrate zero offsets
calibrateOffsets();
// set the initialized flag
initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
// return success
return 1;
}
// 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.b - offset_ib); // amps
current.c = (current.c - offset_ic); // amps
return current;
}
// 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(float voltage){
_UNUSED(voltage) ; // remove unused parameter warning
int exit_flag = 1;
if(skip_align) return exit_flag;
if (!initialized) return 0;
// // 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;
}

40
firmware/lib/Arduino-FOC/src/current_sense/GenericCurrentSense.h Normal file
View File

@@ -0,0 +1,40 @@
#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
int init() override;
PhaseCurrent_s getPhaseCurrents() override;
int driverAlign(float align_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

253
firmware/lib/Arduino-FOC/src/current_sense/InlineCurrentSense.cpp Normal file
View File

@@ -0,0 +1,253 @@
#include "InlineCurrentSense.h"
#include "communication/SimpleFOCDebug.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;
};
InlineCurrentSense::InlineCurrentSense(float _mVpA, int _pinA, int _pinB, int _pinC){
pinA = _pinA;
pinB = _pinB;
pinC = _pinC;
volts_to_amps_ratio = 1000.0f / _mVpA; // mV 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
int InlineCurrentSense::init(){
// if no linked driver its fine in this case
// at least for init()
void* drv_params = driver ? driver->params : nullptr;
// configure ADC variables
params = _configureADCInline(drv_params,pinA,pinB,pinC);
// if init failed return fail
if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0;
// calibrate zero offsets
calibrateOffsets();
// set the initialized flag
initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
// return success
return 1;
}
// 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++) {
if(_isset(pinA)) offset_ia += _readADCVoltageInline(pinA, params);
if(_isset(pinB)) offset_ib += _readADCVoltageInline(pinB, params);
if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC, params);
_delay(1);
}
// calculate the mean offsets
if(_isset(pinA)) offset_ia = offset_ia / calibration_rounds;
if(_isset(pinB)) 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 = (!_isset(pinA)) ? 0 : (_readADCVoltageInline(pinA, params) - offset_ia)*gain_a;// amps
current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageInline(pinB, params) - offset_ib)*gain_b;// amps
current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps
return current;
}
// 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(float voltage){
int exit_flag = 1;
if(skip_align) return exit_flag;
if (driver==nullptr) {
SIMPLEFOC_DEBUG("CUR: No driver linked!");
return 0;
}
if (!initialized) return 0;
if(_isset(pinA)){
// 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 = c.b ? fabs(c.a / c.b) : 0;
float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2
gain_a *= _sign(c.a);
}else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2
gain_a *= _sign(c.a);
}else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5
// switch phase A and B
int tmp_pinA = pinA;
pinA = pinB;
pinB = tmp_pinA;
float tmp_offsetA = offset_ia;
offset_ia = offset_ib;
offset_ib = tmp_offsetA;
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;
float tmp_offsetA = offset_ia;
offset_ia = offset_ic;
offset_ic = tmp_offsetA;
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;
}
}
if(_isset(pinB)){
// set phase B active and phases A and C down
driver->setPwm(0, voltage, 0);
_delay(200);
PhaseCurrent_s 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 = c.a ? fabs(c.b / c.a) : 0;
float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2
gain_b *= _sign(c.b);
}else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2
gain_b *= _sign(c.b);
}else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5
// switch phase A and B
int tmp_pinB = pinB;
pinB = pinA;
pinA = tmp_pinB;
float tmp_offsetB = offset_ib;
offset_ib = offset_ia;
offset_ia = tmp_offsetB;
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;
float tmp_offsetB = offset_ib;
offset_ib = offset_ic;
offset_ic = tmp_offsetB;
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 C active and phases A and B down
driver->setPwm(0, 0, voltage);
_delay(200);
PhaseCurrent_s c = getPhaseCurrents();
// read the adc voltage 500 times ( arbitrary number )
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 ca_ratio = c.a ? fabs(c.c / c.a) : 0;
float cb_ratio = c.b ? fabs(c.c / c.b) : 0;
if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2
gain_c *= _sign(c.c);
}else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2
gain_c *= _sign(c.c);
}else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5
// switch phase A and C
int tmp_pinC = pinC;
pinC = pinA;
pinA = tmp_pinC;
float tmp_offsetC = offset_ic;
offset_ic = offset_ia;
offset_ia = tmp_offsetC;
gain_c *= _sign(c.a);
exit_flag = 2; // signal that pins have been switched
}else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5
// switch phase B and C
int tmp_pinC = pinC;
pinC = pinB;
pinB = tmp_pinC;
float tmp_offsetC = offset_ic;
offset_ic = offset_ib;
offset_ib = tmp_offsetC;
gain_c *= _sign(c.b);
exit_flag = 2; // signal that pins have been switched
}else{
// error in current sense - phase either not measured or bad connection
return 0;
}
}
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;
}

73
firmware/lib/Arduino-FOC/src/current_sense/InlineCurrentSense.h Normal file
View File

@@ -0,0 +1,73 @@
#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);
/**
InlineCurrentSense class constructor
@param mVpA mV per Amp ratio
@param phA A phase adc pin
@param phB B phase adc pin
@param phC C phase adc pin (optional)
*/
InlineCurrentSense(float mVpA, int pinA, int pinB, int pinC = NOT_SET);
// CurrentSense interface implementing functions
int init() override;
PhaseCurrent_s getPhaseCurrents() override;
int driverAlign(float align_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
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)
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();
};
#endif

254
firmware/lib/Arduino-FOC/src/current_sense/LowsideCurrentSense.cpp Normal file
View File

@@ -0,0 +1,254 @@
#include "LowsideCurrentSense.h"
#include "communication/SimpleFOCDebug.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;
}
LowsideCurrentSense::LowsideCurrentSense(float _mVpA, int _pinA, int _pinB, int _pinC){
pinA = _pinA;
pinB = _pinB;
pinC = _pinC;
volts_to_amps_ratio = 1000.0f / _mVpA; // mV 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
int LowsideCurrentSense::init(){
if (driver==nullptr) {
SIMPLEFOC_DEBUG("CUR: Driver not linked!");
return 0;
}
// configure ADC variables
params = _configureADCLowSide(driver->params,pinA,pinB,pinC);
// if init failed return fail
if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0;
// sync the driver
_driverSyncLowSide(driver->params, params);
// calibrate zero offsets
calibrateOffsets();
// set the initialized flag
initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
// return success
return 1;
}
// Function finding zero offsets of the ADC
void LowsideCurrentSense::calibrateOffsets(){
const int calibration_rounds = 2000;
// 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();
if(_isset(pinA)) offset_ia += (_readADCVoltageLowSide(pinA, params));
if(_isset(pinB)) offset_ib += (_readADCVoltageLowSide(pinB, params));
if(_isset(pinC)) offset_ic += (_readADCVoltageLowSide(pinC, params));
_delay(1);
}
// calculate the mean offsets
if(_isset(pinA)) offset_ia = offset_ia / calibration_rounds;
if(_isset(pinB)) 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 = (!_isset(pinA)) ? 0 : (_readADCVoltageLowSide(pinA, params) - offset_ia)*gain_a;// amps
current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageLowSide(pinB, params) - offset_ib)*gain_b;// amps
current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps
return current;
}
// 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(float voltage){
int exit_flag = 1;
if(skip_align) return exit_flag;
if (!initialized) return 0;
if(_isset(pinA)){
// 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 = c.b ? fabs(c.a / c.b) : 0;
float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2
gain_a *= _sign(c.a);
}else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2
gain_a *= _sign(c.a);
}else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5
// switch phase A and B
int tmp_pinA = pinA;
pinA = pinB;
pinB = tmp_pinA;
float tmp_offsetA = offset_ia;
offset_ia = offset_ib;
offset_ib = tmp_offsetA;
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;
float tmp_offsetA = offset_ia;
offset_ia = offset_ic;
offset_ic = tmp_offsetA;
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;
}
}
if(_isset(pinB)){
// set phase B active and phases A and C down
driver->setPwm(0, voltage, 0);
_delay(200);
PhaseCurrent_s 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 = c.a ? fabs(c.b / c.a) : 0;
float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2
gain_b *= _sign(c.b);
}else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2
gain_b *= _sign(c.b);
}else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5
// switch phase A and B
int tmp_pinB = pinB;
pinB = pinA;
pinA = tmp_pinB;
float tmp_offsetB = offset_ib;
offset_ib = offset_ia;
offset_ia = tmp_offsetB;
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;
float tmp_offsetB = offset_ib;
offset_ib = offset_ic;
offset_ic = tmp_offsetB;
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 C active and phases A and B down
driver->setPwm(0, 0, voltage);
_delay(200);
PhaseCurrent_s c = getPhaseCurrents();
// read the adc voltage 500 times ( arbitrary number )
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 ca_ratio = c.a ? fabs(c.c / c.a) : 0;
float cb_ratio = c.b ? fabs(c.c / c.b) : 0;
if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2
gain_c *= _sign(c.c);
}else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2
gain_c *= _sign(c.c);
}else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5
// switch phase A and C
int tmp_pinC = pinC;
pinC = pinA;
pinA = tmp_pinC;
float tmp_offsetC = offset_ic;
offset_ic = offset_ia;
offset_ia = tmp_offsetC;
gain_c *= _sign(c.a);
exit_flag = 2; // signal that pins have been switched
}else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5
// switch phase B and C
int tmp_pinC = pinC;
pinC = pinB;
pinB = tmp_pinC;
float tmp_offsetC = offset_ic;
offset_ic = offset_ib;
offset_ib = tmp_offsetC;
gain_c *= _sign(c.b);
exit_flag = 2; // signal that pins have been switched
}else{
// error in current sense - phase either not measured or bad connection
return 0;
}
}
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;
}

73
firmware/lib/Arduino-FOC/src/current_sense/LowsideCurrentSense.h Normal file
View File

@@ -0,0 +1,73 @@
#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 = _NC);
/**
LowsideCurrentSense class constructor
@param mVpA mV per Amp ratio
@param phA A phase adc pin
@param phB B phase adc pin
@param phC C phase adc pin (optional)
*/
LowsideCurrentSense(float mVpA, int pinA, int pinB, int pinC = _NC);
// CurrentSense interface implementing functions
int init() override;
PhaseCurrent_s getPhaseCurrents() override;
int driverAlign(float align_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
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)
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();
};
#endif

65
firmware/lib/Arduino-FOC/src/current_sense/hardware_api.h Normal file
View File

@@ -0,0 +1,65 @@
#ifndef HARDWARE_UTILS_CURRENT_H
#define HARDWARE_UTILS_CURRENT_H
#include "../common/foc_utils.h"
#include "../common/time_utils.h"
// flag returned if current sense init fails
#define SIMPLEFOC_CURRENT_SENSE_INIT_FAILED ((void*)-1)
// generic implementation of the hardware specific structure
// containing all the necessary current sense parameters
// will be returned as a void pointer from the _configureADCx functions
// will be provided to the _readADCVoltageX() as a void pointer
typedef struct GenericCurrentSenseParams {
int pins[3];
float adc_voltage_conv;
} GenericCurrentSenseParams;
/**
* function reading an ADC value and returning the read voltage
*
* @param pinA - the arduino pin to be read (it has to be ADC pin)
* @param cs_params -current sense parameter structure - hardware specific
*/
float _readADCVoltageInline(const int pinA, const void* cs_params);
/**
* function configuring an ADC for inline sensing.
*
* @param driver_params - driver parameter structure - hardware specific
* @param pinA - adc pin A
* @param pinB - adc pin B
* @param pinC - adc pin C
*/
void* _configureADCInline(const void *driver_params, const int pinA,const int pinB,const int pinC = NOT_SET);
/**
* function configuring an ADC for lowside sensing.
*
* @param driver_params - driver parameter structure - hardware specific
* @param pinA - adc pin A
* @param pinB - adc pin B
* @param pinC - adc pin C
*/
void* _configureADCLowSide(const void *driver_params, 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)
* @param cs_params -current sense parameter structure - hardware specific
*/
float _readADCVoltageLowSide(const int pinA, const void* cs_params);
/**
* function syncing the Driver with the ADC for the LowSide Sensing
* @param driver_params - driver parameter structure - hardware specific
* @param cs_params - current sense parameter structure - hardware specific
*/
void _driverSyncLowSide(void* driver_params, void* cs_params);
#endif

40
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/atmega_mcu.cpp Normal file
View File

@@ -0,0 +1,40 @@
#include "../hardware_api.h"
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__)
#define _ADC_VOLTAGE 5.0f
#define _ADC_RESOLUTION 1024.0f
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
// function reading an ADC value and returning the read voltage
void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){
_UNUSED(driver_params);
if( _isset(pinA) ) pinMode(pinA, INPUT);
if( _isset(pinB) ) pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
.pins = { pinA, pinB, pinC },
.adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION)
};
// 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);
return params;
}
#endif

27
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/due_mcu.cpp Normal file
View File

@@ -0,0 +1,27 @@
#include "../hardware_api.h"
#if defined(__arm__) && defined(__SAM3X8E__)
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 1024.0f
// function reading an ADC value and returning the read voltage
void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){
_UNUSED(driver_params);
if( _isset(pinA) ) pinMode(pinA, INPUT);
if( _isset(pinB) ) pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
.pins = { pinA, pinB, pinC },
.adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION)
};
return params;
}
#endif

259
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp Normal file
View File

@@ -0,0 +1,259 @@
#include "esp32_adc_driver.h"
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3)
#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)
{
int8_t channel = digitalPinToAnalogChannel(pin);
if(channel < 0){
return false;//not adc pin
}
__analogInit();
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);
}
uint16_t value = 0;
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);
}
#endif

88
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h Normal file
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 */

27
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/esp32/esp32_ledc_mcu.cpp Normal file
View File

@@ -0,0 +1,27 @@
#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"
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 4095.0f
// function reading an ADC value and returning the read voltage
void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){
_UNUSED(driver_params);
pinMode(pinA, INPUT);
pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
.pins = { pinA, pinB, pinC },
.adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION)
};
return params;
}
#endif

162
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp Normal file
View File

@@ -0,0 +1,162 @@
#include "../../hardware_api.h"
#include "../../../drivers/hardware_api.h"
#include "../../../drivers/hardware_specific/esp32/esp32_driver_mcpwm.h"
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC)
#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
typedef struct ESP32MCPWMCurrentSenseParams {
int pins[3];
float adc_voltage_conv;
mcpwm_unit_t mcpwm_unit;
int buffer_index;
} ESP32MCPWMCurrentSenseParams;
/**
* Inline adc reading implementation
*/
// function reading an ADC value and returning the read voltage
float _readADCVoltageInline(const int pinA, const void* cs_params){
uint32_t raw_adc = adcRead(pinA);
return raw_adc * ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv;
}
// function reading an ADC value and returning the read voltage
void* _configureADCInline(const void* driver_params, const int pinA, const int pinB, const int pinC){
_UNUSED(driver_params);
if( _isset(pinA) ) pinMode(pinA, INPUT);
if( _isset(pinB) ) pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams {
.pins = { pinA, pinB, pinC },
.adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION)
};
return params;
}
/**
* Low side adc reading implementation
*/
static void IRAM_ATTR mcpwm0_isr_handler(void*);
static void IRAM_ATTR mcpwm1_isr_handler(void*);
byte currentState = 1;
// two mcpwm units
// - max 2 motors per mcpwm unit (6 adc channels)
int adc_pins[2][6]={0};
int adc_pin_count[2]={0};
uint32_t adc_buffer[2][6]={0};
int adc_read_index[2]={0};
// function reading an ADC value and returning the read voltage
float _readADCVoltageLowSide(const int pin, const void* cs_params){
mcpwm_unit_t unit = ((ESP32MCPWMCurrentSenseParams*)cs_params)->mcpwm_unit;
int buffer_index = ((ESP32MCPWMCurrentSenseParams*)cs_params)->buffer_index;
float adc_voltage_conv = ((ESP32MCPWMCurrentSenseParams*)cs_params)->adc_voltage_conv;
for(int i=0; i < adc_pin_count[unit]; i++){
if( pin == ((ESP32MCPWMCurrentSenseParams*)cs_params)->pins[i]) // found in the buffer
return adc_buffer[unit][buffer_index + i] * adc_voltage_conv;
}
// not found
return 0;
}
// function configuring low-side current sensing
void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){
mcpwm_unit_t unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit;
int index_start = adc_pin_count[unit];
if( _isset(pinA) ) adc_pins[unit][adc_pin_count[unit]++] = pinA;
if( _isset(pinB) ) adc_pins[unit][adc_pin_count[unit]++] = pinB;
if( _isset(pinC) ) adc_pins[unit][adc_pin_count[unit]++] = pinC;
if( _isset(pinA) ) pinMode(pinA, INPUT);
if( _isset(pinB) ) pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
ESP32MCPWMCurrentSenseParams* params = new ESP32MCPWMCurrentSenseParams {
.pins = { pinA, pinB, pinC },
.adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION),
.mcpwm_unit = unit,
.buffer_index = index_start
};
return params;
}
void _driverSyncLowSide(void* driver_params, void* cs_params){
mcpwm_dev_t* mcpwm_dev = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_dev;
mcpwm_unit_t mcpwm_unit = ((ESP32MCPWMDriverParams*)driver_params)->mcpwm_unit;
// low-side register enable interrupt
mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEP event will trigger this interrupt
// high side registers enable interrupt
//mcpwm_dev->int_ena.timer0_tep_int_ena = true;//A PWM timer 0 TEZ event will trigger this interrupt
// register interrupts (mcpwm number, interrupt handler, handler argument = NULL, interrupt signal/flag, return handler = NULL)
if(mcpwm_unit == MCPWM_UNIT_0)
mcpwm_isr_register(mcpwm_unit, mcpwm0_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler
else
mcpwm_isr_register(mcpwm_unit, mcpwm1_isr_handler, NULL, ESP_INTR_FLAG_IRAM, NULL); //Set ISR Handler
}
// Read currents when interrupt is triggered
static void IRAM_ATTR mcpwm0_isr_handler(void*){
// // high side
// uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tez_int_st;
// low side
uint32_t mcpwm_intr_status = MCPWM0.int_st.timer0_tep_int_st;
if(mcpwm_intr_status){
adc_buffer[0][adc_read_index[0]] = adcRead(adc_pins[0][adc_read_index[0]]);
adc_read_index[0]++;
if(adc_read_index[0] == adc_pin_count[0]) adc_read_index[0] = 0;
}
// low side
MCPWM0.int_clr.timer0_tep_int_clr = mcpwm_intr_status;
// high side
// MCPWM0.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0;
}
// Read currents when interrupt is triggered
static void IRAM_ATTR mcpwm1_isr_handler(void*){
// // high side
// uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tez_int_st;
// low side
uint32_t mcpwm_intr_status = MCPWM1.int_st.timer0_tep_int_st;
if(mcpwm_intr_status){
adc_buffer[1][adc_read_index[1]] = adcRead(adc_pins[1][adc_read_index[1]]);
adc_read_index[1]++;
if(adc_read_index[1] == adc_pin_count[1]) adc_read_index[1] = 0;
}
// low side
MCPWM1.int_clr.timer0_tep_int_clr = mcpwm_intr_status;
// high side
// MCPWM1.int_clr.timer0_tez_int_clr = mcpwm_intr_status_0;
}
#endif

258
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp Normal file
View File

@@ -0,0 +1,258 @@
#include "esp32_adc_driver.h"
#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3))
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "rom/ets_sys.h"
#include "esp_attr.h"
//#include "esp_intr.h" // deprecated
#include "esp_intr_alloc.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_READER1_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_READER2_CTRL_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S);
}
void __analogSetCycles(uint8_t cycles){
__analogCycles = cycles;
// SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S);
// SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_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_READER1_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S);
SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_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_READER1_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S);
SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_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_READER1_CTRL_REG, SENS_SAR1_DATA_INV);
SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV);
SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW
SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW
SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW
SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW
CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM
SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0
CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM
SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S);
SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S);
SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S);
while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_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_CONF_REG);
if(touch & (1 << pad)){
touch &= ~((1 << (pad + SENS_TOUCH_OUTEN_S)));
WRITE_PERI_REG(SENS_SAR_TOUCH_CONF_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_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M);
SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M);
} else {
CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M);
SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S);
SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_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_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0);
}
return (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_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_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion
value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S);
} else {
while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion
value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_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)
{
int8_t channel = digitalPinToAnalogChannel(pin);
if(channel < 0){
return false;//not adc pin
}
__analogInit();
if(channel > 9){
channel -= 10;
CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M);
SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M);
} else {
CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M);
SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S);
SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M);
}
uint16_t value = 0;
if(channel > 7){
while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion
value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S);
} else {
while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion
value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_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);
}
#endif

41
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/generic_mcu.cpp Normal file
View File

@@ -0,0 +1,41 @@
#include "../hardware_api.h"
// function reading an ADC value and returning the read voltage
__attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){
uint32_t raw_adc = analogRead(pinA);
return raw_adc * ((GenericCurrentSenseParams*)cs_params)->adc_voltage_conv;
}
// function reading an ADC value and returning the read voltage
__attribute__((weak)) void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){
_UNUSED(driver_params);
if( _isset(pinA) ) pinMode(pinA, INPUT);
if( _isset(pinB) ) pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
.pins = { pinA, pinB, pinC },
.adc_voltage_conv = (5.0f)/(1024.0f)
};
return params;
}
// function reading an ADC value and returning the read voltage
__attribute__((weak)) float _readADCVoltageLowSide(const int pinA, const void* cs_params){
return _readADCVoltageInline(pinA, cs_params);
}
// Configure low side for generic mcu
// cannot do much but
__attribute__((weak)) void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){
return _configureADCInline(driver_params, pinA, pinB, pinC);
}
// sync driver and the adc
__attribute__((weak)) void _driverSyncLowSide(void* driver_params, void* cs_params){
_UNUSED(driver_params);
_UNUSED(cs_params);
}
__attribute__((weak)) void _startADC3PinConversionLowSide(){ }

265
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp Normal file
View File

@@ -0,0 +1,265 @@
#if defined(TARGET_RP2040)
#include "../../hardware_api.h"
#include "./rp2040_mcu.h"
#include "../../../drivers/hardware_specific/rp2040/rp2040_mcu.h"
#include "communication/SimpleFOCDebug.h"
#include "hardware/dma.h"
#include "hardware/irq.h"
#include "hardware/pwm.h"
#include "hardware/adc.h"
/* Singleton instance of the ADC engine */
RP2040ADCEngine engine;
alignas(32) const uint32_t trigger_value = ADC_CS_START_ONCE_BITS; // start once
/* Hardware API implementation */
float _readADCVoltageInline(const int pinA, const void* cs_params) {
// not super-happy with this. Here we have to return 1 phase current at a time, when actually we want to
// return readings from the same ADC conversion run. The ADC on RP2040 is anyway in round robin mode :-(
// like this we either have to block interrupts, or of course have the chance of reading across
// new ADC conversions, which probably won't improve the accuracy.
_UNUSED(cs_params);
if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) {
return engine.lastResults.raw[pinA-26]*engine.adc_conv;
}
// otherwise return NaN
return NAN;
};
void* _configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC) {
_UNUSED(driver_params);
if( _isset(pinA) )
engine.addPin(pinA);
if( _isset(pinB) )
engine.addPin(pinB);
if( _isset(pinC) )
engine.addPin(pinC);
engine.init(); // TODO this has to happen later if we want to support more than one motor...
engine.start();
return &engine;
};
// not supported at the moment
// void* _configureADCLowSide(const void *driver_params, const int pinA, const int pinB, const int pinC) {
// if( _isset(pinA) )
// engine.addPin(pinA);
// if( _isset(pinB) )
// engine.addPin(pinB);
// if( _isset(pinC) )
// engine.addPin(pinC);
// engine.setPWMTrigger(((RP2040DriverParams*)driver_params)->slice[0]);
// engine.init();
// engine.start();
// return &engine;
// };
// void _startADC3PinConversionLowSide() {
// // what is this for?
// };
// float _readADCVoltageLowSide(const int pinA, const void* cs_params) {
// // not super-happy with this. Here we have to return 1 phase current at a time, when actually we want to
// // return readings from the same ADC conversion run. The ADC on RP2040 is anyway in round robin mode :-(
// // like this we have block interrupts 3x instead of just once, and of course have the chance of reading across
// // new ADC conversions, which probably won't improve the accuracy.
// if (pinA>=26 && pinA<=29 && engine.channelsEnabled[pinA-26]) {
// return engine.lastResults[pinA-26]*engine.adc_conv;
// }
// // otherwise return NaN
// return NAN;
// };
// void _driverSyncLowSide(void* driver_params, void* cs_params) {
// // nothing to do
// };
volatile int rp2040_intcount = 0;
void _adcConversionFinishedHandler() {
// conversion of all channels finished. copy results.
volatile uint8_t* from = engine.samples;
if (engine.channelsEnabled[0])
engine.lastResults.raw[0] = (*from++);
if (engine.channelsEnabled[1])
engine.lastResults.raw[1] = (*from++);
if (engine.channelsEnabled[2])
engine.lastResults.raw[2] = (*from++);
if (engine.channelsEnabled[3])
engine.lastResults.raw[3] = (*from++);
//dma_channel_acknowledge_irq0(engine.readDMAChannel);
dma_hw->ints0 = 1u << engine.readDMAChannel;
//dma_start_channel_mask( (1u << engine.readDMAChannel) );
dma_channel_set_write_addr(engine.readDMAChannel, engine.samples, true);
// if (engine.triggerPWMSlice>=0)
// dma_channel_set_trans_count(engine.triggerDMAChannel, 1, true);
rp2040_intcount++;
};
/* ADC engine implementation */
RP2040ADCEngine::RP2040ADCEngine() {
channelsEnabled[0] = false;
channelsEnabled[1] = false;
channelsEnabled[2] = false;
channelsEnabled[3] = false;
initialized = false;
};
void RP2040ADCEngine::addPin(int pin) {
if (pin>=26 && pin<=29)
channelsEnabled[pin-26] = true;
else
SIMPLEFOC_DEBUG("RP2040-CUR: ERR: Not an ADC pin: ", pin);
};
// void RP2040ADCEngine::setPWMTrigger(uint slice){
// triggerPWMSlice = slice;
// };
bool RP2040ADCEngine::init() {
if (initialized)
return true;
adc_init();
int enableMask = 0x00;
int channelCount = 0;
for (int i = 3; i>=0; i--) {
if (channelsEnabled[i]){
adc_gpio_init(i+26);
enableMask |= (0x01<<i);
channelCount++;
}
}
adc_set_round_robin(enableMask);
adc_fifo_setup(
true, // Write each completed conversion to the sample FIFO
true, // Enable DMA data request (DREQ)
channelCount, // DREQ (and IRQ) asserted when all samples present
false, // We won't see the ERR bit because of 8 bit reads; disable.
true // Shift each sample to 8 bits when pushing to FIFO
);
if (samples_per_second<1 || samples_per_second>=500000) {
samples_per_second = 0;
adc_set_clkdiv(0);
}
else
adc_set_clkdiv(48000000/samples_per_second);
SIMPLEFOC_DEBUG("RP2040-CUR: ADC init");
readDMAChannel = dma_claim_unused_channel(true);
dma_channel_config cc1 = dma_channel_get_default_config(readDMAChannel);
channel_config_set_transfer_data_size(&cc1, DMA_SIZE_8);
channel_config_set_read_increment(&cc1, false);
channel_config_set_write_increment(&cc1, true);
channel_config_set_dreq(&cc1, DREQ_ADC);
channel_config_set_irq_quiet(&cc1, false);
dma_channel_configure(readDMAChannel,
&cc1,
samples, // dest
&adc_hw->fifo, // source
channelCount, // count
false // defer start
);
dma_channel_set_irq0_enabled(readDMAChannel, true);
irq_add_shared_handler(DMA_IRQ_0, _adcConversionFinishedHandler, PICO_SHARED_IRQ_HANDLER_DEFAULT_ORDER_PRIORITY);
SIMPLEFOC_DEBUG("RP2040-CUR: DMA init");
// if (triggerPWMSlice>=0) { // if we have a trigger
// triggerDMAChannel = dma_claim_unused_channel(true);
// dma_channel_config cc3 = dma_channel_get_default_config(triggerDMAChannel);
// channel_config_set_transfer_data_size(&cc3, DMA_SIZE_32);
// channel_config_set_read_increment(&cc3, false);
// channel_config_set_write_increment(&cc3, false);
// channel_config_set_irq_quiet(&cc3, true);
// channel_config_set_dreq(&cc3, DREQ_PWM_WRAP0+triggerPWMSlice); //pwm_get_dreq(triggerPWMSlice));
// pwm_set_irq_enabled(triggerPWMSlice, true);
// dma_channel_configure(triggerDMAChannel,
// &cc3,
// hw_set_alias_untyped(&adc_hw->cs), // dest
// &trigger_value, // source
// 1, // count
// true // defer start
// );
// SIMPLEFOC_DEBUG("RP2040-CUR: PWM trigger init slice ", triggerPWMSlice);
// }
initialized = true;
return initialized;
};
void RP2040ADCEngine::start() {
SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine starting");
irq_set_enabled(DMA_IRQ_0, true);
dma_start_channel_mask( (1u << readDMAChannel) );
for (int i=0;i<4;i++) {
if (channelsEnabled[i]) {
adc_select_input(i); // set input to first enabled channel
break;
}
}
// if (triggerPWMSlice>=0) {
// dma_start_channel_mask( (1u << triggerDMAChannel) );
// //hw_set_bits(&adc_hw->cs, trigger_value);
// }
// else
adc_run(true);
SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine started");
};
void RP2040ADCEngine::stop() {
adc_run(false);
irq_set_enabled(DMA_IRQ_0, false);
dma_channel_abort(readDMAChannel);
// if (triggerPWMSlice>=0)
// dma_channel_abort(triggerDMAChannel);
adc_fifo_drain();
SIMPLEFOC_DEBUG("RP2040-CUR: ADC engine stopped");
};
ADCResults RP2040ADCEngine::getLastResults() {
ADCResults r;
r.value = lastResults.value;
return r;
};
#endif

93
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/rp2040/rp2040_mcu.h Normal file
View File

@@ -0,0 +1,93 @@
#pragma once
/*
* RP2040 ADC features are very weak :-(
* - only 4 inputs
* - only 9 bit effective resolution
* - read only 1 input at a time
* - 2 microseconds conversion time!
* - no triggers from PWM / events, only DMA
*
* So to read 3 phases takes 6 microseconds. :-(
*
* The RP2040 ADC engine takes over the control of the MCU's ADC. Parallel ADC access is not permitted, as this would
* cause conflicts with the engine's DMA based access and cause crashes.
* To use the other ADC channels, use them via this engine. Use addPin() to add them to the conversion, and getLastResult()
* to retrieve their value at any time.
*
* For motor current sensing, the engine supports inline sensing only.
*
* Inline sensing is supported by offering a user-selectable fixed ADC sampling rate, which can be set between 500kHz and 1Hz.
* After starting the engine it will continuously sample and provide new values at the configured rate.
*
* The default sampling rate is 20kHz, which is suitable for 2 channels assuming you a 5kHz main loop speed (a new measurement is used per
* main loop iteration).
*
* Low-side sensing is currently not supported.
*
* The SimpleFOC PWM driver for RP2040 syncs all the slices, so the PWM trigger is applied to the first used slice. For current
* sensing to work correctly, all PWM slices have to be set to the same PWM frequency.
* In theory, two motors could be sensed using 2 shunts on each motor.
*
* Note that if using other ADC channels along with the motor current sensing, those channels will be subject to the same conversion schedule as the motor's ADC channels, i.e. convert at the same fixed rate in case
* of inline sensing.
*
* Solution to trigger ADC conversion from PWM via DMA:
* use the PWM wrap as a DREQ to a DMA channel, and have the DMA channel write to the ADC's CS register to trigger an ADC sample.
* Unfortunately, I could not get this to work, so no low side sensing for the moment.
*
* Solution for ADC conversion:
* ADC converts all channels in round-robin mode, and writes to FIFO. FIFO is emptied by a DMA which triggers after N conversions,
* where N is the number of ADC channels used. So this DMA copies all the values from one round-robin conversion.
*
*
*/
#define SIMPLEFOC_RP2040_ADC_RESOLUTION 256
#ifndef SIMPLEFOC_RP2040_ADC_VDDA
#define SIMPLEFOC_RP2040_ADC_VDDA 3.3f
#endif
union ADCResults {
uint32_t value;
uint8_t raw[4];
struct {
uint8_t ch0;
uint8_t ch1;
uint8_t ch2;
uint8_t ch3;
};
};
class RP2040ADCEngine {
public:
RP2040ADCEngine();
void addPin(int pin);
//void setPWMTrigger(uint slice);
bool init();
void start();
void stop();
ADCResults getLastResults(); // TODO find a better API and representation for this
int samples_per_second = 20000; // 20kHz default (assuming 2 shunts and 5kHz loop speed), set to 0 to convert in tight loop
float adc_conv = (SIMPLEFOC_RP2040_ADC_VDDA / SIMPLEFOC_RP2040_ADC_RESOLUTION); // conversion from raw ADC to float
//int triggerPWMSlice = -1;
bool initialized;
uint readDMAChannel;
//uint copyDMAChannel;
//uint triggerDMAChannel;
bool channelsEnabled[4];
volatile uint8_t samples[4];
volatile ADCResults lastResults;
//alignas(32) volatile uint8_t nextResults[4];
};

318
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/samd/samd21_mcu.cpp Normal file
View File

@@ -0,0 +1,318 @@
#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 configuring low-side current sensing
void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC)
{
_UNUSED(driver_params);
_pinA = pinA;
_pinB = pinB;
_pinC = pinC;
freeRunning = true;
instance.init(pinA, pinB, pinC);
GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
.pins = { pinA, pinB, pinC }
};
return params;
}
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, const void* cs_params)
{
_UNUSED(cs_params);
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(void* driver_params, void* cs_params)
{
_UNUSED(driver_params);
_UNUSED(cs_params);
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

69
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/samd/samd21_mcu.h Normal file
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

23
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/samd/samd_mcu.cpp Normal file
View File

@@ -0,0 +1,23 @@
#include "../../hardware_api.h"
#if defined(_SAMD21_)|| defined(_SAMD51_) || defined(_SAME51_)
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 1024.0f
// function reading an ADC value and returning the read voltage
void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){
_UNUSED(driver_params);
if( _isset(pinA) ) pinMode(pinA, INPUT);
if( _isset(pinB) ) pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
.pins = { pinA, pinB, pinC },
.adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION)
};
return params;
}
#endif

364
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp Normal file
View File

@@ -0,0 +1,364 @@
#include "../../../hardware_api.h"
#if defined(ARDUINO_B_G431B_ESC1)
#include "communication/SimpleFOCDebug.h"
#include "stm32g4xx_hal.h"
#include "stm32g4xx_ll_pwr.h"
#include "stm32g4xx_hal_adc.h"
#include "b_g431_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_T3_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)
{
SIMPLEFOC_DEBUG("HAL_ADC_Init failed!");
}
/** Configure the ADC multi-mode
*/
multimode.Mode = ADC_MODE_INDEPENDENT;
if (HAL_ADCEx_MultiModeConfigChannel(hadc1, &multimode) != HAL_OK)
{
SIMPLEFOC_DEBUG("HAL_ADCEx_MultiModeConfigChannel failed!");
}
/** 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)
{
SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!");
}
/** 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)
{
SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!");
}
//******************************************************************
// Temp, Poti ....
/** Configure Regular Channel (PB14, Temperature)
*/
sConfig.Channel = ADC_CHANNEL_5;
sConfig.Rank = ADC_REGULAR_RANK_4;
sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
sConfig.OffsetNumber = ADC_OFFSET_NONE;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK)
{
SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!");
}
/** Configure Regular Channel (PB14, Temperature)
*/
sConfig.Channel = ADC_CHANNEL_1;
sConfig.Rank = ADC_REGULAR_RANK_5;
sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
sConfig.OffsetNumber = ADC_OFFSET_NONE;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK)
{
SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!");
}
/* 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)
{
SIMPLEFOC_DEBUG("HAL_ADC_Init failed!");
}
/** 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)
{
SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!");
}
/* 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

18
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef B_G431_ESC1_HAL
#define B_G431_ESC1_HAL
#if defined(ARDUINO_B_G431B_ESC1)
#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

164
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp Normal file
View File

@@ -0,0 +1,164 @@
#include "../../../hardware_api.h"
#if defined(ARDUINO_B_G431B_ESC1)
#include "b_g431_hal.h"
#include "Arduino.h"
#include "../stm32_mcu.h"
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
#include "communication/SimpleFOCDebug.h"
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 4096.0f
#define ADC_BUF_LEN_1 5
#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;
volatile uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion
volatile uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion
// 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, const void* cs_params){
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];
else if (pin == A_TEMPERATURE)
raw_adc = adcBuffer1[3];
return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
}
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)
{
SIMPLEFOC_DEBUG("HAL_OPAMP_Init failed!");
}
}
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)
{
SIMPLEFOC_DEBUG("HAL_DMA_Init failed!");
}
__HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc);
}
void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){
_UNUSED(driver_params);
_UNUSED(pinA);
_UNUSED(pinB);
_UNUSED(pinC);
SIMPLEFOC_DEBUG("B-G431B does not implement inline current sense. Use low-side current sense instead.");
return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
}
void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){
_UNUSED(driver_params);
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)
{
SIMPLEFOC_DEBUG("DMA read init failed");
}
if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adcBuffer2, ADC_BUF_LEN_2) != HAL_OK)
{
SIMPLEFOC_DEBUG("DMA read init failed");
}
HAL_OPAMP_Start(&hopamp1);
HAL_OPAMP_Start(&hopamp2);
HAL_OPAMP_Start(&hopamp3);
Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams {
.pins = { pinA, pinB, pinC },
.adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION),
.timer_handle = (HardwareTimer *)(HardwareTimer_Handle[get_timer_index(TIM1)]->__this)
};
return params;
}
extern "C" {
void DMA1_Channel1_IRQHandler(void) {
HAL_DMA_IRQHandler(&hdma_adc1);
}
void DMA1_Channel2_IRQHandler(void) {
HAL_DMA_IRQHandler(&hdma_adc2);
}
}
void _driverSyncLowSide(void* _driver_params, void* _cs_params){
STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
// stop all the timers for the driver
_stopTimers(driver_params->timers, 6);
// if timer has repetition counter - it will downsample using it
// and it does not need the software downsample
if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){
// adjust the initial timer state such that the trigger for DMA transfer aligns with the pwm peaks instead of throughs.
// only necessary for the timers that have repetition counters
cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR;
}
// set the trigger output event
LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
// restart all the timers of the driver
_startTimers(driver_params->timers, 6);
}
#endif

33
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32_mcu.cpp Normal file
View File

@@ -0,0 +1,33 @@
#include "../../hardware_api.h"
#if defined(_STM32_DEF_) and !defined(ARDUINO_B_G431B_ESC1) and !defined(SIMPLEFOC_STM32_CUSTOMCURRENTSENSE)
#include "stm32_mcu.h"
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 1024.0f
// function reading an ADC value and returning the read voltage
__attribute__((weak)) void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){
_UNUSED(driver_params);
if( _isset(pinA) ) pinMode(pinA, INPUT);
if( _isset(pinB) ) pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams {
.pins = { pinA, pinB, pinC },
.adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION)
};
return params;
}
// function reading an ADC value and returning the read voltage
__attribute__((weak)) float _readADCVoltageInline(const int pinA, const void* cs_params){
uint32_t raw_adc = analogRead(pinA);
return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
}
#endif

21
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32_mcu.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef STM32_CURRENTSENSE_MCU_DEF
#define STM32_CURRENTSENSE_MCU_DEF
#include "../../hardware_api.h"
#include "../../../common/foc_utils.h"
#if defined(_STM32_DEF_)
// generic implementation of the hardware specific structure
// containing all the necessary current sense parameters
// will be returned as a void pointer from the _configureADCx functions
// will be provided to the _readADCVoltageX() as a void pointer
typedef struct Stm32CurrentSenseParams {
int pins[3] = {(int)NOT_SET};
float adc_voltage_conv;
ADC_HandleTypeDef* adc_handle = NP;
HardwareTimer* timer_handle = NP;
} Stm32CurrentSenseParams;
#endif
#endif

162
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp Normal file
View File

@@ -0,0 +1,162 @@
#include "stm32f1_hal.h"
#if defined(STM32F1xx)
#include "../../../../communication/SimpleFOCDebug.h"
#define _TRGO_NOT_AVAILABLE 12345
// timer to injected TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215
uint32_t _timerToInjectedTRGO(HardwareTimer* timer){
if(timer->getHandle()->Instance == TIM1)
return ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
#ifdef TIM2 // if defined timer 2
else if(timer->getHandle()->Instance == TIM2)
return ADC_EXTERNALTRIGINJECCONV_T2_TRGO;
#endif
#ifdef TIM4 // if defined timer 4
else if(timer->getHandle()->Instance == TIM4)
return ADC_EXTERNALTRIGINJECCONV_T4_TRGO;
#endif
#ifdef TIM5 // if defined timer 5
else if(timer->getHandle()->Instance == TIM5)
return ADC_EXTERNALTRIGINJECCONV_T5_TRGO;
#endif
else
return _TRGO_NOT_AVAILABLE;
}
// timer to regular TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F1xx_HAL_Driver/Inc/stm32f1xx_hal_adc_ex.h#L215
uint32_t _timerToRegularTRGO(HardwareTimer* timer){
if(timer->getHandle()->Instance == TIM3)
return ADC_EXTERNALTRIGCONV_T3_TRGO;
#ifdef TIM8 // if defined timer 8
else if(timer->getHandle()->Instance == TIM8)
return ADC_EXTERNALTRIGCONV_T8_TRGO;
#endif
else
return _TRGO_NOT_AVAILABLE;
}
ADC_HandleTypeDef hadc;
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
{
ADC_InjectionConfTypeDef sConfigInjected;
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE();
#ifdef ADC2 // if defined ADC2
else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE();
#endif
#ifdef ADC3 // if defined ADC3
else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE();
#endif
else{
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
#endif
return -1; // error not a valid ADC instance
}
hadc.Init.ScanConvMode = ADC_SCAN_ENABLE;
hadc.Init.ContinuousConvMode = ENABLE;
hadc.Init.DiscontinuousConvMode = DISABLE;
hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START;
hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc.Init.NbrOfConversion = 0;
HAL_ADC_Init(&hadc);
/**Configure for the selected ADC regular channel to be converted.
*/
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
*/
sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_1CYCLE_5;
sConfigInjected.AutoInjectedConv = DISABLE;
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
sConfigInjected.InjectedOffset = 0;
// automating TRGO flag finding - hardware specific
uint8_t tim_num = 0;
while(driver_params->timers[tim_num] != NP && tim_num < 6){
uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]);
if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
// if the code comes here, it has found the timer available
// timer does have trgo flag for injected channels
sConfigInjected.ExternalTrigInjecConv = trigger_flag;
// this will be the timer with which the ADC will sync
cs_params->timer_handle = driver_params->timers[tim_num-1];
// done
break;
}
if( cs_params->timer_handle == NP ){
// not possible to use these timers for low-side current sense
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
#endif
return -1;
}
// display which timer is being used
#ifdef SIMPLEFOC_STM32_DEBUG
// it would be better to use the getTimerNumber from driver
SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1);
#endif
// first channel
sConfigInjected.InjectedRank = ADC_REGULAR_RANK_1;
sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC));
HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
// second channel
sConfigInjected.InjectedRank = ADC_REGULAR_RANK_2;
sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC));
HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
// third channel - if exists
if(_isset(cs_params->pins[2])){
sConfigInjected.InjectedRank = ADC_REGULAR_RANK_3;
sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC));
HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected);
}
// enable interrupt
HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC1_2_IRQn);
cs_params->adc_handle = &hadc;
return 0;
}
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
{
uint8_t cnt = 0;
if(_isset(pinA)){
pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC);
cs_params->pins[cnt++] = pinA;
}
if(_isset(pinB)){
pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC);
cs_params->pins[cnt++] = pinB;
}
if(_isset(pinC)){
pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC);
cs_params->pins[cnt] = pinC;
}
}
extern "C" {
void ADC1_2_IRQHandler(void)
{
HAL_ADC_IRQHandler(&hadc);
}
}
#endif

17
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h Normal file
View File

@@ -0,0 +1,17 @@
#ifndef STM32F1_LOWSIDE_HAL
#define STM32F1_LOWSIDE_HAL
#include "Arduino.h"
#if defined(STM32F1xx)
#include "stm32f1xx_hal.h"
#include "../../../../common/foc_utils.h"
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
#include "../stm32_mcu.h"
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params);
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
#endif
#endif

104
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp Normal file
View File

@@ -0,0 +1,104 @@
#include "../../../hardware_api.h"
#if defined(STM32F1xx)
#include "../../../../common/foc_utils.h"
#include "../../../../drivers/hardware_api.h"
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
#include "../../../hardware_api.h"
#include "../stm32_mcu.h"
#include "stm32f1_hal.h"
#include "Arduino.h"
#define _ADC_VOLTAGE_F1 3.3f
#define _ADC_RESOLUTION_F1 4096.0f
// array of values of 4 injected channels per adc instance (3)
uint32_t adc_val[3][4]={0};
// does adc interrupt need a downsample - per adc (3)
bool needs_downsample[3] = {1};
// downsampling variable - per adc (3)
uint8_t tim_downsample[3] = {0};
int _adcToIndex(ADC_HandleTypeDef *AdcHandle){
if(AdcHandle->Instance == ADC1) return 0;
#ifdef ADC2 // if ADC2 exists
else if(AdcHandle->Instance == ADC2) return 1;
#endif
#ifdef ADC3 // if ADC3 exists
else if(AdcHandle->Instance == ADC3) return 2;
#endif
return 0;
}
void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){
Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams {
.pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET},
.adc_voltage_conv = (_ADC_VOLTAGE_F1) / (_ADC_RESOLUTION_F1)
};
_adc_gpio_init(cs_params, pinA,pinB,pinC);
if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
return cs_params;
}
void _driverSyncLowSide(void* _driver_params, void* _cs_params){
STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
// if compatible timer has not been found
if (cs_params->timer_handle == NULL) return;
// stop all the timers for the driver
_stopTimers(driver_params->timers, 6);
// if timer has repetition counter - it will downsample using it
// and it does not need the software downsample
if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){
// adjust the initial timer state such that the trigger
// - for DMA transfer aligns with the pwm peaks instead of throughs.
// - for interrupt based ADC transfer
// - only necessary for the timers that have repetition counters
cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR;
// remember that this timer has repetition counter - no need to downasmple
needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0;
}
// set the trigger output event
LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
// start the adc
HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle);
// restart all the timers of the driver
_startTimers(driver_params->timers, 6);
}
// function reading an ADC value and returning the read voltage
float _readADCVoltageLowSide(const int pin, const void* cs_params){
for(int i=0; i < 3; i++){
if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer
return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
}
return 0;
}
extern "C" {
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){
// calculate the instance
int adc_index = _adcToIndex(AdcHandle);
// if the timer han't repetition counter - downsample two times
if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) {
tim_downsample[adc_index] = 0;
return;
}
adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1);
adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2);
adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3);
}
}
#endif

170
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp Normal file
View File

@@ -0,0 +1,170 @@
#include "stm32f4_hal.h"
#if defined(STM32F4xx)
//#define SIMPLEFOC_STM32_DEBUG
#include "../../../../communication/SimpleFOCDebug.h"
#define _TRGO_NOT_AVAILABLE 12345
ADC_HandleTypeDef hadc;
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
{
ADC_InjectionConfTypeDef sConfigInjected;
// check if all pins belong to the same ADC
ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC);
ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr;
if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!");
#endif
return -1;
}
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE();
#ifdef ADC2 // if defined ADC2
else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE();
#endif
#ifdef ADC3 // if defined ADC3
else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE();
#endif
else{
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
#endif
return -1; // error not a valid ADC instance
}
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1);
#endif
hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
hadc.Init.Resolution = ADC_RESOLUTION_12B;
hadc.Init.ScanConvMode = ENABLE;
hadc.Init.ContinuousConvMode = ENABLE;
hadc.Init.DiscontinuousConvMode = DISABLE;
hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now
hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc.Init.NbrOfConversion = 1;
hadc.Init.DMAContinuousRequests = DISABLE;
hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
if ( HAL_ADC_Init(&hadc) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!");
#endif
return -1;
}
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
*/
sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES;
sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING;
sConfigInjected.AutoInjectedConv = DISABLE;
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
sConfigInjected.InjectedOffset = 0;
// automating TRGO flag finding - hardware specific
uint8_t tim_num = 0;
while(driver_params->timers[tim_num] != NP && tim_num < 6){
uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]);
if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
// if the code comes here, it has found the timer available
// timer does have trgo flag for injected channels
sConfigInjected.ExternalTrigInjecConv = trigger_flag;
// this will be the timer with which the ADC will sync
cs_params->timer_handle = driver_params->timers[tim_num-1];
// done
break;
}
if( cs_params->timer_handle == NP ){
// not possible to use these timers for low-side current sense
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
#endif
return -1;
}
// display which timer is being used
#ifdef SIMPLEFOC_STM32_DEBUG
// it would be better to use the getTimerNumber from driver
SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1);
#endif
// first channel
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1;
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0]));
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) );
#endif
return -1;
}
// second channel
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2;
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1]));
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ;
#endif
return -1;
}
// third channel - if exists
if(_isset(cs_params->pins[2])){
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3;
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2]));
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ;
#endif
return -1;
}
}
// enable interrupt
HAL_NVIC_SetPriority(ADC_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC_IRQn);
cs_params->adc_handle = &hadc;
return 0;
}
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
{
uint8_t cnt = 0;
if(_isset(pinA)){
pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC);
cs_params->pins[cnt++] = pinA;
}
if(_isset(pinB)){
pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC);
cs_params->pins[cnt++] = pinB;
}
if(_isset(pinC)){
pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC);
cs_params->pins[cnt] = pinC;
}
}
extern "C" {
void ADC_IRQHandler(void)
{
HAL_ADC_IRQHandler(&hadc);
}
}
#endif

18
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h Normal file
View File

@@ -0,0 +1,18 @@
#ifndef STM32F4_LOWSIDE_HAL
#define STM32F4_LOWSIDE_HAL
#include "Arduino.h"
#if defined(STM32F4xx)
#include "stm32f4xx_hal.h"
#include "../../../../common/foc_utils.h"
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
#include "../stm32_mcu.h"
#include "stm32f4_utils.h"
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params);
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
#endif
#endif

96
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp Normal file
View File

@@ -0,0 +1,96 @@
#include "../../../hardware_api.h"
#if defined(STM32F4xx)
#include "../../../../common/foc_utils.h"
#include "../../../../drivers/hardware_api.h"
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
#include "../../../hardware_api.h"
#include "../stm32_mcu.h"
#include "stm32f4_hal.h"
#include "stm32f4_utils.h"
#include "Arduino.h"
#define _ADC_VOLTAGE_F4 3.3f
#define _ADC_RESOLUTION_F4 4096.0f
// array of values of 4 injected channels per adc instance (3)
uint32_t adc_val[3][4]={0};
// does adc interrupt need a downsample - per adc (3)
bool needs_downsample[3] = {1};
// downsampling variable - per adc (3)
uint8_t tim_downsample[3] = {0};
void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){
Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams {
.pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET},
.adc_voltage_conv = (_ADC_VOLTAGE_F4) / (_ADC_RESOLUTION_F4)
};
_adc_gpio_init(cs_params, pinA,pinB,pinC);
if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
return cs_params;
}
void _driverSyncLowSide(void* _driver_params, void* _cs_params){
STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
// if compatible timer has not been found
if (cs_params->timer_handle == NULL) return;
// stop all the timers for the driver
_stopTimers(driver_params->timers, 6);
// if timer has repetition counter - it will downsample using it
// and it does not need the software downsample
if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){
// adjust the initial timer state such that the trigger
// - for DMA transfer aligns with the pwm peaks instead of throughs.
// - for interrupt based ADC transfer
// - only necessary for the timers that have repetition counters
cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR;
// remember that this timer has repetition counter - no need to downasmple
needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0;
}
// set the trigger output event
LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
// start the adc
HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle);
// restart all the timers of the driver
_startTimers(driver_params->timers, 6);
}
// function reading an ADC value and returning the read voltage
float _readADCVoltageLowSide(const int pin, const void* cs_params){
for(int i=0; i < 3; i++){
if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer
return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
}
return 0;
}
extern "C" {
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){
// calculate the instance
int adc_index = _adcToIndex(AdcHandle);
// if the timer han't repetition counter - downsample two times
if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) {
tim_downsample[adc_index] = 0;
return;
}
adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1);
adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2);
adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3);
}
}
#endif

193
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp Normal file
View File

@@ -0,0 +1,193 @@
#include "stm32f4_utils.h"
#if defined(STM32F4xx)
/* Exported Functions */
/**
* @brief Return ADC HAL channel linked to a PinName
* @param pin: PinName
* @retval Valid HAL channel
*/
uint32_t _getADCChannel(PinName pin)
{
uint32_t function = pinmap_function(pin, PinMap_ADC);
uint32_t channel = 0;
switch (STM_PIN_CHANNEL(function)) {
#ifdef ADC_CHANNEL_0
case 0:
channel = ADC_CHANNEL_0;
break;
#endif
case 1:
channel = ADC_CHANNEL_1;
break;
case 2:
channel = ADC_CHANNEL_2;
break;
case 3:
channel = ADC_CHANNEL_3;
break;
case 4:
channel = ADC_CHANNEL_4;
break;
case 5:
channel = ADC_CHANNEL_5;
break;
case 6:
channel = ADC_CHANNEL_6;
break;
case 7:
channel = ADC_CHANNEL_7;
break;
case 8:
channel = ADC_CHANNEL_8;
break;
case 9:
channel = ADC_CHANNEL_9;
break;
case 10:
channel = ADC_CHANNEL_10;
break;
case 11:
channel = ADC_CHANNEL_11;
break;
case 12:
channel = ADC_CHANNEL_12;
break;
case 13:
channel = ADC_CHANNEL_13;
break;
case 14:
channel = ADC_CHANNEL_14;
break;
case 15:
channel = ADC_CHANNEL_15;
break;
#ifdef ADC_CHANNEL_16
case 16:
channel = ADC_CHANNEL_16;
break;
#endif
case 17:
channel = ADC_CHANNEL_17;
break;
#ifdef ADC_CHANNEL_18
case 18:
channel = ADC_CHANNEL_18;
break;
#endif
#ifdef ADC_CHANNEL_19
case 19:
channel = ADC_CHANNEL_19;
break;
#endif
#ifdef ADC_CHANNEL_20
case 20:
channel = ADC_CHANNEL_20;
break;
case 21:
channel = ADC_CHANNEL_21;
break;
case 22:
channel = ADC_CHANNEL_22;
break;
case 23:
channel = ADC_CHANNEL_23;
break;
#ifdef ADC_CHANNEL_24
case 24:
channel = ADC_CHANNEL_24;
break;
case 25:
channel = ADC_CHANNEL_25;
break;
case 26:
channel = ADC_CHANNEL_26;
break;
#ifdef ADC_CHANNEL_27
case 27:
channel = ADC_CHANNEL_27;
break;
case 28:
channel = ADC_CHANNEL_28;
break;
case 29:
channel = ADC_CHANNEL_29;
break;
case 30:
channel = ADC_CHANNEL_30;
break;
case 31:
channel = ADC_CHANNEL_31;
break;
#endif
#endif
#endif
default:
_Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function)));
break;
}
return channel;
}
// timer to injected TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179
uint32_t _timerToInjectedTRGO(HardwareTimer* timer){
if(timer->getHandle()->Instance == TIM1)
return ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
#ifdef TIM2 // if defined timer 2
else if(timer->getHandle()->Instance == TIM2)
return ADC_EXTERNALTRIGINJECCONV_T2_TRGO;
#endif
#ifdef TIM4 // if defined timer 4
else if(timer->getHandle()->Instance == TIM4)
return ADC_EXTERNALTRIGINJECCONV_T4_TRGO;
#endif
#ifdef TIM5 // if defined timer 5
else if(timer->getHandle()->Instance == TIM5)
return ADC_EXTERNALTRIGINJECCONV_T5_TRGO;
#endif
else
return _TRGO_NOT_AVAILABLE;
}
// timer to regular TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331
uint32_t _timerToRegularTRGO(HardwareTimer* timer){
if(timer->getHandle()->Instance == TIM2)
return ADC_EXTERNALTRIGCONV_T2_TRGO;
#ifdef TIM3 // if defined timer 3
else if(timer->getHandle()->Instance == TIM3)
return ADC_EXTERNALTRIGCONV_T3_TRGO;
#endif
#ifdef TIM8 // if defined timer 8
else if(timer->getHandle()->Instance == TIM8)
return ADC_EXTERNALTRIGCONV_T8_TRGO;
#endif
else
return _TRGO_NOT_AVAILABLE;
}
int _adcToIndex(ADC_TypeDef *AdcHandle){
if(AdcHandle == ADC1) return 0;
#ifdef ADC2 // if ADC2 exists
else if(AdcHandle == ADC2) return 1;
#endif
#ifdef ADC3 // if ADC3 exists
else if(AdcHandle == ADC3) return 2;
#endif
#ifdef ADC4 // if ADC4 exists
else if(AdcHandle == ADC4) return 3;
#endif
#ifdef ADC5 // if ADC5 exists
else if(AdcHandle == ADC5) return 4;
#endif
return 0;
}
int _adcToIndex(ADC_HandleTypeDef *AdcHandle){
return _adcToIndex(AdcHandle->Instance);
}
#endif

34
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h Normal file
View File

@@ -0,0 +1,34 @@
#ifndef STM32F4_UTILS_HAL
#define STM32F4_UTILS_HAL
#include "Arduino.h"
#if defined(STM32F4xx)
#define _TRGO_NOT_AVAILABLE 12345
/* Exported Functions */
/**
* @brief Return ADC HAL channel linked to a PinName
* @param pin: PinName
* @retval Valid HAL channel
*/
uint32_t _getADCChannel(PinName pin);
// timer to injected TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h#L179
uint32_t _timerToInjectedTRGO(HardwareTimer* timer);
// timer to regular TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc.h#L331
uint32_t _timerToRegularTRGO(HardwareTimer* timer);
// function returning index of the ADC instance
int _adcToIndex(ADC_HandleTypeDef *AdcHandle);
int _adcToIndex(ADC_TypeDef *AdcHandle);
#endif
#endif

267
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp Normal file
View File

@@ -0,0 +1,267 @@
#include "stm32g4_hal.h"
#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) and !defined(SIMPLEFOC_STM32_CUSTOMCURRENTSENSE)
#include "../../../../communication/SimpleFOCDebug.h"
#define SIMPLEFOC_STM32_DEBUG
ADC_HandleTypeDef hadc;
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
{
ADC_InjectionConfTypeDef sConfigInjected;
// check if all pins belong to the same ADC
ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC);
ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr;
if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!");
#endif
return -1;
}
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
if(hadc.Instance == ADC1) {
#ifdef __HAL_RCC_ADC1_CLK_ENABLE
__HAL_RCC_ADC1_CLK_ENABLE();
#endif
#ifdef __HAL_RCC_ADC12_CLK_ENABLE
__HAL_RCC_ADC12_CLK_ENABLE();
#endif
}
#ifdef ADC2
else if (hadc.Instance == ADC2) {
#ifdef __HAL_RCC_ADC2_CLK_ENABLE
__HAL_RCC_ADC2_CLK_ENABLE();
#endif
#ifdef __HAL_RCC_ADC12_CLK_ENABLE
__HAL_RCC_ADC12_CLK_ENABLE();
#endif
}
#endif
#ifdef ADC3
else if (hadc.Instance == ADC3) {
#ifdef __HAL_RCC_ADC3_CLK_ENABLE
__HAL_RCC_ADC3_CLK_ENABLE();
#endif
#ifdef __HAL_RCC_ADC34_CLK_ENABLE
__HAL_RCC_ADC34_CLK_ENABLE();
#endif
#if defined(ADC345_COMMON)
__HAL_RCC_ADC345_CLK_ENABLE();
#endif
}
#endif
#ifdef ADC4
else if (hadc.Instance == ADC4) {
#ifdef __HAL_RCC_ADC4_CLK_ENABLE
__HAL_RCC_ADC4_CLK_ENABLE();
#endif
#ifdef __HAL_RCC_ADC34_CLK_ENABLE
__HAL_RCC_ADC34_CLK_ENABLE();
#endif
#if defined(ADC345_COMMON)
__HAL_RCC_ADC345_CLK_ENABLE();
#endif
}
#endif
#ifdef ADC5
else if (hadc.Instance == ADC5) {
#if defined(ADC345_COMMON)
__HAL_RCC_ADC345_CLK_ENABLE();
#endif
}
#endif
else{
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
#endif
return -1; // error not a valid ADC instance
}
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1);
#endif
hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
hadc.Init.Resolution = ADC_RESOLUTION_12B;
hadc.Init.ScanConvMode = ADC_SCAN_ENABLE;
hadc.Init.ContinuousConvMode = DISABLE;
hadc.Init.LowPowerAutoWait = DISABLE;
hadc.Init.GainCompensation = 0;
hadc.Init.DiscontinuousConvMode = DISABLE;
hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now
hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc.Init.NbrOfConversion = 2;
hadc.Init.DMAContinuousRequests = DISABLE;
hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED;
if ( HAL_ADC_Init(&hadc) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!");
#endif
return -1;
}
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
*/
sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLES_5;
sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING;
sConfigInjected.AutoInjectedConv = DISABLE;
sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED;
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE;
sConfigInjected.InjectedOffset = 0;
sConfigInjected.InjecOversamplingMode = DISABLE;
sConfigInjected.QueueInjectedContext = DISABLE;
// automating TRGO flag finding - hardware specific
uint8_t tim_num = 0;
while(driver_params->timers[tim_num] != NP && tim_num < 6){
uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]);
if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
// if the code comes here, it has found the timer available
// timer does have trgo flag for injected channels
sConfigInjected.ExternalTrigInjecConv = trigger_flag;
// this will be the timer with which the ADC will sync
cs_params->timer_handle = driver_params->timers[tim_num-1];
// done
break;
}
if( cs_params->timer_handle == NP ){
// not possible to use these timers for low-side current sense
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
#endif
return -1;
}
// first channel
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1;
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0]));
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) );
#endif
return -1;
}
// second channel
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2;
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1]));
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ;
#endif
return -1;
}
// third channel - if exists
if(_isset(cs_params->pins[2])){
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3;
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2]));
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ;
#endif
return -1;
}
}
if(hadc.Instance == ADC1) {
// enable interrupt
HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC1_2_IRQn);
}
#ifdef ADC2
else if (hadc.Instance == ADC2) {
// enable interrupt
HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC1_2_IRQn);
}
#endif
#ifdef ADC3
else if (hadc.Instance == ADC3) {
// enable interrupt
HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC3_IRQn);
}
#endif
#ifdef ADC4
else if (hadc.Instance == ADC4) {
// enable interrupt
HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC4_IRQn);
}
#endif
#ifdef ADC5
else if (hadc.Instance == ADC5) {
// enable interrupt
HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC5_IRQn);
}
#endif
cs_params->adc_handle = &hadc;
return 0;
}
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
{
uint8_t cnt = 0;
if(_isset(pinA)){
pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC);
cs_params->pins[cnt++] = pinA;
}
if(_isset(pinB)){
pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC);
cs_params->pins[cnt++] = pinB;
}
if(_isset(pinC)){
pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC);
cs_params->pins[cnt] = pinC;
}
}
extern "C" {
void ADC1_2_IRQHandler(void)
{
HAL_ADC_IRQHandler(&hadc);
}
#ifdef ADC3
void ADC3_IRQHandler(void)
{
HAL_ADC_IRQHandler(&hadc);
}
#endif
#ifdef ADC4
void ADC4_IRQHandler(void)
{
HAL_ADC_IRQHandler(&hadc);
}
#endif
#ifdef ADC5
void ADC5_IRQHandler(void)
{
HAL_ADC_IRQHandler(&hadc);
}
#endif
}
#endif

19
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h Normal file
View File

@@ -0,0 +1,19 @@
#ifndef STM32G4_LOWSIDE_HAL
#define STM32G4_LOWSIDE_HAL
#include "Arduino.h"
#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1)
#include "stm32g4xx_hal.h"
#include "../../../../common/foc_utils.h"
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
#include "../stm32_mcu.h"
#include "stm32g4_utils.h"
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params);
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
#endif
#endif

98
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp Normal file
View File

@@ -0,0 +1,98 @@
#include "../../../hardware_api.h"
#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) and !defined(SIMPLEFOC_STM32_CUSTOMCURRENTSENSE)
#include "../../../../common/foc_utils.h"
#include "../../../../drivers/hardware_api.h"
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
#include "../../../hardware_api.h"
#include "../stm32_mcu.h"
#include "stm32g4_hal.h"
#include "stm32g4_utils.h"
#include "Arduino.h"
#define _ADC_VOLTAGE_G4 3.3f
#define _ADC_RESOLUTION_G4 4096.0f
// array of values of 4 injected channels per adc instance (5)
uint32_t adc_val[5][4]={0};
// does adc interrupt need a downsample - per adc (5)
bool needs_downsample[5] = {1};
// downsampling variable - per adc (5)
uint8_t tim_downsample[5] = {0};
void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){
Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams {
.pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET},
.adc_voltage_conv = (_ADC_VOLTAGE_G4) / (_ADC_RESOLUTION_G4)
};
_adc_gpio_init(cs_params, pinA,pinB,pinC);
if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
return cs_params;
}
void _driverSyncLowSide(void* _driver_params, void* _cs_params){
STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
// if compatible timer has not been found
if (cs_params->timer_handle == NULL) return;
// stop all the timers for the driver
_stopTimers(driver_params->timers, 6);
// if timer has repetition counter - it will downsample using it
// and it does not need the software downsample
if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){
// adjust the initial timer state such that the trigger
// - for DMA transfer aligns with the pwm peaks instead of throughs.
// - for interrupt based ADC transfer
// - only necessary for the timers that have repetition counters
cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR;
// remember that this timer has repetition counter - no need to downasmple
needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0;
}
// set the trigger output event
LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
// start the adc
HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle);
// restart all the timers of the driver
_startTimers(driver_params->timers, 6);
}
// function reading an ADC value and returning the read voltage
float _readADCVoltageLowSide(const int pin, const void* cs_params){
for(int i=0; i < 3; i++){
if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer
return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
}
return 0;
}
extern "C" {
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){
// calculate the instance
int adc_index = _adcToIndex(AdcHandle);
// if the timer han't repetition counter - downsample two times
if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) {
tim_downsample[adc_index] = 0;
return;
}
adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1);
adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2);
adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3);
}
}
#endif

237
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp Normal file
View File

@@ -0,0 +1,237 @@
#include "stm32g4_utils.h"
#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) and !defined(SIMPLEFOC_STM32_CUSTOMCURRENTSENSE)
/* Exported Functions */
/**
* @brief Return ADC HAL channel linked to a PinName
* @param pin: PinName
* @retval Valid HAL channel
*/
uint32_t _getADCChannel(PinName pin)
{
uint32_t function = pinmap_function(pin, PinMap_ADC);
uint32_t channel = 0;
switch (STM_PIN_CHANNEL(function)) {
#ifdef ADC_CHANNEL_0
case 0:
channel = ADC_CHANNEL_0;
break;
#endif
case 1:
channel = ADC_CHANNEL_1;
break;
case 2:
channel = ADC_CHANNEL_2;
break;
case 3:
channel = ADC_CHANNEL_3;
break;
case 4:
channel = ADC_CHANNEL_4;
break;
case 5:
channel = ADC_CHANNEL_5;
break;
case 6:
channel = ADC_CHANNEL_6;
break;
case 7:
channel = ADC_CHANNEL_7;
break;
case 8:
channel = ADC_CHANNEL_8;
break;
case 9:
channel = ADC_CHANNEL_9;
break;
case 10:
channel = ADC_CHANNEL_10;
break;
case 11:
channel = ADC_CHANNEL_11;
break;
case 12:
channel = ADC_CHANNEL_12;
break;
case 13:
channel = ADC_CHANNEL_13;
break;
case 14:
channel = ADC_CHANNEL_14;
break;
case 15:
channel = ADC_CHANNEL_15;
break;
#ifdef ADC_CHANNEL_16
case 16:
channel = ADC_CHANNEL_16;
break;
#endif
case 17:
channel = ADC_CHANNEL_17;
break;
#ifdef ADC_CHANNEL_18
case 18:
channel = ADC_CHANNEL_18;
break;
#endif
#ifdef ADC_CHANNEL_19
case 19:
channel = ADC_CHANNEL_19;
break;
#endif
#ifdef ADC_CHANNEL_20
case 20:
channel = ADC_CHANNEL_20;
break;
case 21:
channel = ADC_CHANNEL_21;
break;
case 22:
channel = ADC_CHANNEL_22;
break;
case 23:
channel = ADC_CHANNEL_23;
break;
#ifdef ADC_CHANNEL_24
case 24:
channel = ADC_CHANNEL_24;
break;
case 25:
channel = ADC_CHANNEL_25;
break;
case 26:
channel = ADC_CHANNEL_26;
break;
#ifdef ADC_CHANNEL_27
case 27:
channel = ADC_CHANNEL_27;
break;
case 28:
channel = ADC_CHANNEL_28;
break;
case 29:
channel = ADC_CHANNEL_29;
break;
case 30:
channel = ADC_CHANNEL_30;
break;
case 31:
channel = ADC_CHANNEL_31;
break;
#endif
#endif
#endif
default:
_Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function)));
break;
}
return channel;
}
// timer to injected TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217
uint32_t _timerToInjectedTRGO(HardwareTimer* timer){
if(timer->getHandle()->Instance == TIM1)
return ADC_EXTERNALTRIGINJEC_T1_TRGO;
#ifdef TIM2 // if defined timer 2
else if(timer->getHandle()->Instance == TIM2)
return ADC_EXTERNALTRIGINJEC_T2_TRGO;
#endif
#ifdef TIM3 // if defined timer 3
else if(timer->getHandle()->Instance == TIM3)
return ADC_EXTERNALTRIGINJEC_T3_TRGO;
#endif
#ifdef TIM4 // if defined timer 4
else if(timer->getHandle()->Instance == TIM4)
return ADC_EXTERNALTRIGINJEC_T4_TRGO;
#endif
#ifdef TIM6 // if defined timer 6
else if(timer->getHandle()->Instance == TIM6)
return ADC_EXTERNALTRIGINJEC_T6_TRGO;
#endif
#ifdef TIM7 // if defined timer 7
else if(timer->getHandle()->Instance == TIM7)
return ADC_EXTERNALTRIGINJEC_T7_TRGO;
#endif
#ifdef TIM8 // if defined timer 8
else if(timer->getHandle()->Instance == TIM8)
return ADC_EXTERNALTRIGINJEC_T8_TRGO;
#endif
#ifdef TIM15 // if defined timer 15
else if(timer->getHandle()->Instance == TIM15)
return ADC_EXTERNALTRIGINJEC_T15_TRGO;
#endif
#ifdef TIM20 // if defined timer 15
else if(timer->getHandle()->Instance == TIM20)
return ADC_EXTERNALTRIGINJEC_T20_TRGO;
#endif
else
return _TRGO_NOT_AVAILABLE;
}
// timer to regular TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519
uint32_t _timerToRegularTRGO(HardwareTimer* timer){
if(timer->getHandle()->Instance == TIM1)
return ADC_EXTERNALTRIG_T1_TRGO;
#ifdef TIM2 // if defined timer 2
else if(timer->getHandle()->Instance == TIM2)
return ADC_EXTERNALTRIG_T2_TRGO;
#endif
#ifdef TIM3 // if defined timer 3
else if(timer->getHandle()->Instance == TIM3)
return ADC_EXTERNALTRIG_T3_TRGO;
#endif
#ifdef TIM4 // if defined timer 4
else if(timer->getHandle()->Instance == TIM4)
return ADC_EXTERNALTRIG_T4_TRGO;
#endif
#ifdef TIM6 // if defined timer 6
else if(timer->getHandle()->Instance == TIM6)
return ADC_EXTERNALTRIG_T6_TRGO;
#endif
#ifdef TIM7 // if defined timer 7
else if(timer->getHandle()->Instance == TIM7)
return ADC_EXTERNALTRIG_T7_TRGO;
#endif
#ifdef TIM8 // if defined timer 8
else if(timer->getHandle()->Instance == TIM8)
return ADC_EXTERNALTRIG_T7_TRGO;
#endif
#ifdef TIM15 // if defined timer 15
else if(timer->getHandle()->Instance == TIM15)
return ADC_EXTERNALTRIG_T15_TRGO;
#endif
#ifdef TIM20 // if defined timer 15
else if(timer->getHandle()->Instance == TIM20)
return ADC_EXTERNALTRIG_T20_TRGO;
#endif
else
return _TRGO_NOT_AVAILABLE;
}
int _adcToIndex(ADC_TypeDef *AdcHandle){
if(AdcHandle == ADC1) return 0;
#ifdef ADC2 // if ADC2 exists
else if(AdcHandle == ADC2) return 1;
#endif
#ifdef ADC3 // if ADC3 exists
else if(AdcHandle == ADC3) return 2;
#endif
#ifdef ADC4 // if ADC4 exists
else if(AdcHandle == ADC4) return 3;
#endif
#ifdef ADC5 // if ADC5 exists
else if(AdcHandle == ADC5) return 4;
#endif
return 0;
}
int _adcToIndex(ADC_HandleTypeDef *AdcHandle){
return _adcToIndex(AdcHandle->Instance);
}
#endif

34
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h Normal file
View File

@@ -0,0 +1,34 @@
#ifndef STM32G4_UTILS_HAL
#define STM32G4_UTILS_HAL
#include "Arduino.h"
#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) and !defined(SIMPLEFOC_STM32_CUSTOMCURRENTSENSE)
#define _TRGO_NOT_AVAILABLE 12345
/* Exported Functions */
/**
* @brief Return ADC HAL channel linked to a PinName
* @param pin: PinName
* @retval Valid HAL channel
*/
uint32_t _getADCChannel(PinName pin);
// timer to injected TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217
uint32_t _timerToInjectedTRGO(HardwareTimer* timer);
// timer to regular TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519
uint32_t _timerToRegularTRGO(HardwareTimer* timer);
// function returning index of the ADC instance
int _adcToIndex(ADC_HandleTypeDef *AdcHandle);
int _adcToIndex(ADC_TypeDef *AdcHandle);
#endif
#endif

266
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp Normal file
View File

@@ -0,0 +1,266 @@
#include "stm32l4_hal.h"
#if defined(STM32L4xx)
#include "../../../../communication/SimpleFOCDebug.h"
#define SIMPLEFOC_STM32_DEBUG
ADC_HandleTypeDef hadc;
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
{
ADC_InjectionConfTypeDef sConfigInjected;
// check if all pins belong to the same ADC
ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC);
ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr;
if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!");
#endif
return -1;
}
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
if(hadc.Instance == ADC1) {
#ifdef __HAL_RCC_ADC1_CLK_ENABLE
__HAL_RCC_ADC1_CLK_ENABLE();
#endif
#ifdef __HAL_RCC_ADC12_CLK_ENABLE
__HAL_RCC_ADC12_CLK_ENABLE();
#endif
}
#ifdef ADC2
else if (hadc.Instance == ADC2) {
#ifdef __HAL_RCC_ADC2_CLK_ENABLE
__HAL_RCC_ADC2_CLK_ENABLE();
#endif
#ifdef __HAL_RCC_ADC12_CLK_ENABLE
__HAL_RCC_ADC12_CLK_ENABLE();
#endif
}
#endif
#ifdef ADC3
else if (hadc.Instance == ADC3) {
#ifdef __HAL_RCC_ADC3_CLK_ENABLE
__HAL_RCC_ADC3_CLK_ENABLE();
#endif
#ifdef __HAL_RCC_ADC34_CLK_ENABLE
__HAL_RCC_ADC34_CLK_ENABLE();
#endif
#if defined(ADC345_COMMON)
__HAL_RCC_ADC345_CLK_ENABLE();
#endif
}
#endif
#ifdef ADC4
else if (hadc.Instance == ADC4) {
#ifdef __HAL_RCC_ADC4_CLK_ENABLE
__HAL_RCC_ADC4_CLK_ENABLE();
#endif
#ifdef __HAL_RCC_ADC34_CLK_ENABLE
__HAL_RCC_ADC34_CLK_ENABLE();
#endif
#if defined(ADC345_COMMON)
__HAL_RCC_ADC345_CLK_ENABLE();
#endif
}
#endif
#ifdef ADC5
else if (hadc.Instance == ADC5) {
#if defined(ADC345_COMMON)
__HAL_RCC_ADC345_CLK_ENABLE();
#endif
}
#endif
else{
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
#endif
return -1; // error not a valid ADC instance
}
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1);
#endif
hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
hadc.Init.Resolution = ADC_RESOLUTION_12B;
hadc.Init.ScanConvMode = ADC_SCAN_ENABLE;
hadc.Init.ContinuousConvMode = DISABLE;
hadc.Init.LowPowerAutoWait = DISABLE;
hadc.Init.DiscontinuousConvMode = DISABLE;
hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now
hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc.Init.NbrOfConversion = 2;
hadc.Init.DMAContinuousRequests = DISABLE;
hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED;
if ( HAL_ADC_Init(&hadc) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!");
#endif
return -1;
}
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
*/
sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLES_5;
sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING;
sConfigInjected.AutoInjectedConv = DISABLE;
sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED;
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE;
sConfigInjected.InjectedOffset = 0;
sConfigInjected.InjecOversamplingMode = DISABLE;
sConfigInjected.QueueInjectedContext = DISABLE;
// automating TRGO flag finding - hardware specific
uint8_t tim_num = 0;
while(driver_params->timers[tim_num] != NP && tim_num < 6){
uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]);
if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
// if the code comes here, it has found the timer available
// timer does have trgo flag for injected channels
sConfigInjected.ExternalTrigInjecConv = trigger_flag;
// this will be the timer with which the ADC will sync
cs_params->timer_handle = driver_params->timers[tim_num-1];
// done
break;
}
if( cs_params->timer_handle == NP ){
// not possible to use these timers for low-side current sense
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
#endif
return -1;
}
// first channel
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1;
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0]));
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) );
#endif
return -1;
}
// second channel
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2;
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1]));
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ;
#endif
return -1;
}
// third channel - if exists
if(_isset(cs_params->pins[2])){
sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3;
sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2]));
if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
#ifdef SIMPLEFOC_STM32_DEBUG
SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ;
#endif
return -1;
}
}
if(hadc.Instance == ADC1) {
// enable interrupt
HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC1_2_IRQn);
}
#ifdef ADC2
else if (hadc.Instance == ADC2) {
// enable interrupt
HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC1_2_IRQn);
}
#endif
#ifdef ADC3
else if (hadc.Instance == ADC3) {
// enable interrupt
HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC3_IRQn);
}
#endif
#ifdef ADC4
else if (hadc.Instance == ADC4) {
// enable interrupt
HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC4_IRQn);
}
#endif
#ifdef ADC5
else if (hadc.Instance == ADC5) {
// enable interrupt
HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(ADC5_IRQn);
}
#endif
cs_params->adc_handle = &hadc;
return 0;
}
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
{
uint8_t cnt = 0;
if(_isset(pinA)){
pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC);
cs_params->pins[cnt++] = pinA;
}
if(_isset(pinB)){
pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC);
cs_params->pins[cnt++] = pinB;
}
if(_isset(pinC)){
pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC);
cs_params->pins[cnt] = pinC;
}
}
extern "C" {
void ADC1_2_IRQHandler(void)
{
HAL_ADC_IRQHandler(&hadc);
}
#ifdef ADC3
void ADC3_IRQHandler(void)
{
HAL_ADC_IRQHandler(&hadc);
}
#endif
#ifdef ADC4
void ADC4_IRQHandler(void)
{
HAL_ADC_IRQHandler(&hadc);
}
#endif
#ifdef ADC5
void ADC5_IRQHandler(void)
{
HAL_ADC_IRQHandler(&hadc);
}
#endif
}
#endif

19
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h Normal file
View File

@@ -0,0 +1,19 @@
#ifndef STM32L4_LOWSIDE_HAL
#define STM32L4_LOWSIDE_HAL
#include "Arduino.h"
#if defined(STM32L4xx)
#include "stm32l4xx_hal.h"
#include "../../../../common/foc_utils.h"
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
#include "../stm32_mcu.h"
#include "stm32l4_utils.h"
int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params);
void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
#endif
#endif

98
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp Normal file
View File

@@ -0,0 +1,98 @@
#include "../../../hardware_api.h"
#if defined(STM32L4xx)
#include "../../../../common/foc_utils.h"
#include "../../../../drivers/hardware_api.h"
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
#include "../../../hardware_api.h"
#include "../stm32_mcu.h"
#include "stm32l4_hal.h"
#include "stm32l4_utils.h"
#include "Arduino.h"
#define _ADC_VOLTAGE_L4 3.3f
#define _ADC_RESOLUTION_L4 4096.0f
// array of values of 4 injected channels per adc instance (5)
uint32_t adc_val[5][4]={0};
// does adc interrupt need a downsample - per adc (5)
bool needs_downsample[5] = {1};
// downsampling variable - per adc (5)
uint8_t tim_downsample[5] = {0};
void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){
Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams {
.pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET},
.adc_voltage_conv = (_ADC_VOLTAGE_L4) / (_ADC_RESOLUTION_L4)
};
_adc_gpio_init(cs_params, pinA,pinB,pinC);
if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
return cs_params;
}
void _driverSyncLowSide(void* _driver_params, void* _cs_params){
STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
// if compatible timer has not been found
if (cs_params->timer_handle == NULL) return;
// stop all the timers for the driver
_stopTimers(driver_params->timers, 6);
// if timer has repetition counter - it will downsample using it
// and it does not need the software downsample
if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){
// adjust the initial timer state such that the trigger
// - for DMA transfer aligns with the pwm peaks instead of throughs.
// - for interrupt based ADC transfer
// - only necessary for the timers that have repetition counters
cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR;
// remember that this timer has repetition counter - no need to downasmple
needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0;
}
// set the trigger output event
LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
// start the adc
HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle);
// restart all the timers of the driver
_startTimers(driver_params->timers, 6);
}
// function reading an ADC value and returning the read voltage
float _readADCVoltageLowSide(const int pin, const void* cs_params){
for(int i=0; i < 3; i++){
if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer
return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
}
return 0;
}
extern "C" {
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){
// calculate the instance
int adc_index = _adcToIndex(AdcHandle);
// if the timer han't repetition counter - downsample two times
if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) {
tim_downsample[adc_index] = 0;
return;
}
adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1);
adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2);
adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3);
}
}
#endif

221
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp Normal file
View File

@@ -0,0 +1,221 @@
#include "stm32l4_utils.h"
#if defined(STM32L4xx)
/* Exported Functions */
/**
* @brief Return ADC HAL channel linked to a PinName
* @param pin: PinName
* @retval Valid HAL channel
*/
uint32_t _getADCChannel(PinName pin)
{
uint32_t function = pinmap_function(pin, PinMap_ADC);
uint32_t channel = 0;
switch (STM_PIN_CHANNEL(function)) {
#ifdef ADC_CHANNEL_0
case 0:
channel = ADC_CHANNEL_0;
break;
#endif
case 1:
channel = ADC_CHANNEL_1;
break;
case 2:
channel = ADC_CHANNEL_2;
break;
case 3:
channel = ADC_CHANNEL_3;
break;
case 4:
channel = ADC_CHANNEL_4;
break;
case 5:
channel = ADC_CHANNEL_5;
break;
case 6:
channel = ADC_CHANNEL_6;
break;
case 7:
channel = ADC_CHANNEL_7;
break;
case 8:
channel = ADC_CHANNEL_8;
break;
case 9:
channel = ADC_CHANNEL_9;
break;
case 10:
channel = ADC_CHANNEL_10;
break;
case 11:
channel = ADC_CHANNEL_11;
break;
case 12:
channel = ADC_CHANNEL_12;
break;
case 13:
channel = ADC_CHANNEL_13;
break;
case 14:
channel = ADC_CHANNEL_14;
break;
case 15:
channel = ADC_CHANNEL_15;
break;
#ifdef ADC_CHANNEL_16
case 16:
channel = ADC_CHANNEL_16;
break;
#endif
case 17:
channel = ADC_CHANNEL_17;
break;
#ifdef ADC_CHANNEL_18
case 18:
channel = ADC_CHANNEL_18;
break;
#endif
#ifdef ADC_CHANNEL_19
case 19:
channel = ADC_CHANNEL_19;
break;
#endif
#ifdef ADC_CHANNEL_20
case 20:
channel = ADC_CHANNEL_20;
break;
case 21:
channel = ADC_CHANNEL_21;
break;
case 22:
channel = ADC_CHANNEL_22;
break;
case 23:
channel = ADC_CHANNEL_23;
break;
#ifdef ADC_CHANNEL_24
case 24:
channel = ADC_CHANNEL_24;
break;
case 25:
channel = ADC_CHANNEL_25;
break;
case 26:
channel = ADC_CHANNEL_26;
break;
#ifdef ADC_CHANNEL_27
case 27:
channel = ADC_CHANNEL_27;
break;
case 28:
channel = ADC_CHANNEL_28;
break;
case 29:
channel = ADC_CHANNEL_29;
break;
case 30:
channel = ADC_CHANNEL_30;
break;
case 31:
channel = ADC_CHANNEL_31;
break;
#endif
#endif
#endif
default:
_Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function)));
break;
}
return channel;
}
// timer to injected TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal_adc_ex.h#L210
uint32_t _timerToInjectedTRGO(HardwareTimer* timer){
if(timer->getHandle()->Instance == TIM1)
return ADC_EXTERNALTRIGINJEC_T1_TRGO;
#ifdef TIM2 // if defined timer 2
else if(timer->getHandle()->Instance == TIM2)
return ADC_EXTERNALTRIGINJEC_T2_TRGO;
#endif
#ifdef TIM3 // if defined timer 3
else if(timer->getHandle()->Instance == TIM3)
return ADC_EXTERNALTRIGINJEC_T3_TRGO;
#endif
#ifdef TIM4 // if defined timer 4
else if(timer->getHandle()->Instance == TIM4)
return ADC_EXTERNALTRIGINJEC_T4_TRGO;
#endif
#ifdef TIM6 // if defined timer 6
else if(timer->getHandle()->Instance == TIM6)
return ADC_EXTERNALTRIGINJEC_T6_TRGO;
#endif
#ifdef TIM8 // if defined timer 8
else if(timer->getHandle()->Instance == TIM8)
return ADC_EXTERNALTRIGINJEC_T8_TRGO;
#endif
#ifdef TIM15 // if defined timer 15
else if(timer->getHandle()->Instance == TIM15)
return ADC_EXTERNALTRIGINJEC_T15_TRGO;
#endif
else
return _TRGO_NOT_AVAILABLE;
}
// timer to regular TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519
uint32_t _timerToRegularTRGO(HardwareTimer* timer){
if(timer->getHandle()->Instance == TIM1)
return ADC_EXTERNALTRIG_T1_TRGO;
#ifdef TIM2 // if defined timer 2
else if(timer->getHandle()->Instance == TIM2)
return ADC_EXTERNALTRIG_T2_TRGO;
#endif
#ifdef TIM3 // if defined timer 3
else if(timer->getHandle()->Instance == TIM3)
return ADC_EXTERNALTRIG_T3_TRGO;
#endif
#ifdef TIM4 // if defined timer 4
else if(timer->getHandle()->Instance == TIM4)
return ADC_EXTERNALTRIG_T4_TRGO;
#endif
#ifdef TIM6 // if defined timer 6
else if(timer->getHandle()->Instance == TIM6)
return ADC_EXTERNALTRIG_T6_TRGO;
#endif
#ifdef TIM8 // if defined timer 8
else if(timer->getHandle()->Instance == TIM8)
return ADC_EXTERNALTRIG_T8_TRGO;
#endif
#ifdef TIM15 // if defined timer 15
else if(timer->getHandle()->Instance == TIM15)
return ADC_EXTERNALTRIG_T15_TRGO;
#endif
else
return _TRGO_NOT_AVAILABLE;
}
int _adcToIndex(ADC_TypeDef *AdcHandle){
if(AdcHandle == ADC1) return 0;
#ifdef ADC2 // if ADC2 exists
else if(AdcHandle == ADC2) return 1;
#endif
#ifdef ADC3 // if ADC3 exists
else if(AdcHandle == ADC3) return 2;
#endif
#ifdef ADC4 // if ADC4 exists
else if(AdcHandle == ADC4) return 3;
#endif
#ifdef ADC5 // if ADC5 exists
else if(AdcHandle == ADC5) return 4;
#endif
return 0;
}
int _adcToIndex(ADC_HandleTypeDef *AdcHandle){
return _adcToIndex(AdcHandle->Instance);
}
#endif

34
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h Normal file
View File

@@ -0,0 +1,34 @@
#ifndef STM32L4_UTILS_HAL
#define STM32L4_UTILS_HAL
#include "Arduino.h"
#if defined(STM32L4xx)
#define _TRGO_NOT_AVAILABLE 12345
/* Exported Functions */
/**
* @brief Return ADC HAL channel linked to a PinName
* @param pin: PinName
* @retval Valid HAL channel
*/
uint32_t _getADCChannel(PinName pin);
// timer to injected TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217
uint32_t _timerToInjectedTRGO(HardwareTimer* timer);
// timer to regular TRGO
// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519
uint32_t _timerToRegularTRGO(HardwareTimer* timer);
// function returning index of the ADC instance
int _adcToIndex(ADC_HandleTypeDef *AdcHandle);
int _adcToIndex(ADC_TypeDef *AdcHandle);
#endif
#endif

24
firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/teensy_mcu.cpp Normal file
View File

@@ -0,0 +1,24 @@
#include "../hardware_api.h"
#if defined(__arm__) && defined(CORE_TEENSY)
#define _ADC_VOLTAGE 3.3f
#define _ADC_RESOLUTION 1024.0f
// function reading an ADC value and returning the read voltage
void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){
_UNUSED(driver_params);
if( _isset(pinA) ) pinMode(pinA, INPUT);
if( _isset(pinB) ) pinMode(pinB, INPUT);
if( _isset(pinC) ) pinMode(pinC, INPUT);
GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
.pins = { pinA, pinB, pinC },
.adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION)
};
return params;
}
#endif