diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 5ee6e772..e44a42b6 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -16,6 +16,7 @@ jobs: - adafruit:samd:adafruit_metro_m4 # samd51 - esp32:esp32:esp32doit-devkit-v1 # esp32 - esp32:esp32:esp32s2 # esp32s2 + - esp32:esp32:esp32s3 # esp32s3 - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive @@ -26,7 +27,7 @@ jobs: - arduino-boards-fqbn: arduino:avr:uno # arudino uno - compiling almost all examples sketch-names: '**.ino' required-libraries: PciManager - sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example + sketches-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control,esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino @@ -51,6 +52,10 @@ jobs: platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json sketch-names: bldc_driver_3pwm_standalone.ino, stepper_driver_2pwm_standalone.ino, stepper_driver_4pwm_standalone + - arduino-boards-fqbn: esp32:esp32:esp32s3 # esp32s3 + platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json + sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino + - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # esp32 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json sketch-names: esp32_position_control.ino, esp32_i2c_dual_bus_example.ino, esp32_current_control_low_side.ino, esp32_spi_alt_example.ino @@ -68,11 +73,11 @@ jobs: - arduino-boards-fqbn: STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: odrive_example_encoder.ino, odrive_example_spi.ino + sketch-names: odrive_example_encoder.ino, odrive_example_spi.ino, stm32_current_control_low_side.ino - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # nucleo one full example platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, sdouble_full_control_example.ino + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, sdouble_full_control_example.ino, stm32_current_control_low_side.ino # Do not cancel all jobs / architectures if one job fails diff --git a/README.md b/README.md index e49ef384..3a1b3571 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,17 @@ -# Arduino Simple Field Oriented Control (FOC) library - +# SimpleFOClibrary - **Simple** Field Oriented Control (FOC) **library**
+### A Cross-Platform FOC implementation for BLDC and Stepper motors
based on the Arduino IDE and PlatformIO ![Library Compile](https://github.com/simplefoc/Arduino-FOC/workflows/Library%20Compile/badge.svg) -[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) +![GitHub release (latest by date)](https://img.shields.io/github/v/release/simplefoc/arduino-foc) +![GitHub Release Date](https://img.shields.io/github/release-date/simplefoc/arduino-foc?color=blue) +![GitHub commits since tagged version](https://img.shields.io/github/commits-since/simplefoc/arduino-foc/latest/dev) +![GitHub commit activity (branch)](https://img.shields.io/github/commit-activity/m/simplefoc/arduino-foc/dev) + [![arduino-library-badge](https://www.ardu-badge.com/badge/Simple%20FOC.svg?)](https://www.ardu-badge.com/badge/Simple%20FOC.svg) +[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![status](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d/status.svg)](https://joss.theoj.org/papers/4382445f249e064e9f0a7f6c1bb06b1d) + We live in very exciting times 😃! BLDC motors are entering the hobby community more and more and many great projects have already emerged leveraging their far superior dynamics and power capabilities. BLDC motors have numerous advantages over regular DC motors but they have one big disadvantage, the complexity of control. Even though it has become relatively easy to design and manufacture PCBs and create our own hardware solutions for driving BLDC motors the proper low-cost solutions are yet to come. One of the reasons for this is the apparent complexity of writing the BLDC driving algorithms, Field oriented control (FOC) being an example of one of the most efficient ones. The solutions that can be found on-line are almost exclusively very specific for certain hardware configuration and the microcontroller architecture used. Additionally, most of the efforts at this moment are still channeled towards the high-power applications of the BLDC motors and proper low-cost and low-power FOC supporting boards are very hard to find today and even may not exist.
@@ -18,54 +24,31 @@ Therefore this is an attempt to: - *Medium-power* BLDC driver (<30Amps): [Arduino SimpleFOCPowerShield ](https://github.com/simplefoc/Arduino-SimpleFOC-PowerShield). - See also [@byDagor](https://github.com/byDagor)'s *fully-integrated* ESP32 based board: [Dagor Brushless Controller](https://github.com/byDagor/Dagor-Brushless-Controller) -

NEWS 📢: SimpleFOClibrary has been published in the Journal of Open Source Software

-

- SimpleFOC: A Field Oriented Control (FOC) Library for Controlling Brushless Direct Current (BLDC) and Stepper Motors.
- A. Skuric, HS. Bank, R. Unger, O. Williams, D. González-Reyes
-Journal of Open Source Software, 7(74), 4232, https://doi.org/10.21105/joss.04232 -

- -
-

NEW RELEASE 📢: SimpleFOClibrary v2.2.2 see release

- -
- -## Arduino *SimpleFOClibrary* v2.2 +> NEW RELEASE 📢 : SimpleFOClibrary v2.2.3 +> - stm32 low-side current sensing +> - g4 supported +> - thoroughly tested f1/f4/g4 - [#187](https://github.com/simplefoc/Arduino-FOC/issues/187) +> - bg431b: added support for VBAT and TEMPERATURE readings [#222](https://github.com/simplefoc/Arduino-FOC/pull/222) +> - bugfixing +> - leonardo +> - mega2560 [#190](https://github.com/simplefoc/Arduino-FOC/issues/190) +> - inline current sense without driver [#188](https://github.com/simplefoc/Arduino-FOC/issues/188) +> - bg431b support current sense fix [#210](https://github.com/simplefoc/Arduino-FOC/pull/210) +> - StepperDriver4PWM wrong init [#182](https://github.com/simplefoc/Arduino-FOC/issues/182) +> - open loop back-emf vlotage issue [#219](https://github.com/simplefoc/Arduino-FOC/issues/219) +> - SAMD51 compile issue [#217](https://github.com/simplefoc/Arduino-FOC/issues/217) +> - ESP32-S3 compile issue [#198](https://github.com/simplefoc/Arduino-FOC/issues/198) +> - ESP32 compile issue [#208](https://github.com/simplefoc/Arduino-FOC/issues/208), [#207](https://github.com/simplefoc/Arduino-FOC/issues/207) +> - magnetic sensor direction finding more robust [#173](https://github.com/simplefoc/Arduino-FOC/issues/173), [#164](https://github.com/simplefoc/Arduino-FOC/pull/164) +> - `StepDirListener` improved timing [#169](https://github.com/simplefoc/Arduino-FOC/issues/169), [#209](https://github.com/simplefoc/Arduino-FOC/pull/209) +> - API changes +> - `setPhaseVoltage` is now public function +> - `getVelocity` can now be called as many times as necessary (it recalculates the velocity if the time between calls is longer then `minDeltaT` - default 0.1ms) +> - BG431 board can be used only with `LowsideCurrentSense` class! +> - `initFOC` fails if current sense not initialised +> - driver and curent sense have to be well initialised for `initFOC` to start +> - `cs.init()` and `driver.init()` return `1` if well initialised and `0` if failed +## Arduino *SimpleFOClibrary* v2.2.3

diff --git a/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino new file mode 100644 index 00000000..3e3f04ab --- /dev/null +++ b/examples/hardware_specific_examples/DRV8302_driver/stm32_current_control_low_side/stm32_current_control_low_side.ino @@ -0,0 +1,167 @@ +/** + * Comprehensive BLDC motor control example using encoder and the DRV8302 board + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * check the https://docs.simplefoc.com for full list of motor commands + * + */ +#include + +// DRV8302 pins connections +// don't forget to connect the common ground pin +#define INH_A PA8 +#define INH_B PA9 +#define INH_C PA10 + +#define EN_GATE PB7 +#define M_PWM PB4 +#define M_OC PB3 +#define OC_ADJ PB6 +#define OC_GAIN PB5 + +#define IOUTA PA0 +#define IOUTB PA1 +#define IOUTC PA2 + +// Motor instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver3PWM driver = BLDCDriver3PWM(INH_A, INH_B, INH_C, EN_GATE); + +// DRV8302 board has 0.005Ohm shunt resistors and the gain of 12.22 V/V +LowsideCurrentSense cs = LowsideCurrentSense(0.005f, 12.22f, IOUTA, IOUTB, IOUTC); + +// encoder instance +Encoder encoder = Encoder(PB14, PB15, 2048); + +// Interrupt routine intialisation +// channel A and B callbacks +void doA(){encoder.handleA();} +void doB(){encoder.handleB();} + + +// commander interface +Commander command = Commander(Serial); +void onMotor(char* cmd){ command.motor(&motor, cmd); } + +void setup() { + + // initialize encoder sensor hardware + encoder.init(); + encoder.enableInterrupts(doA, doB); + // link the motor to the sensor + motor.linkSensor(&encoder); + + // DRV8302 specific code + // M_OC - enable overcurrent protection + pinMode(M_OC,OUTPUT); + digitalWrite(M_OC,LOW); + // M_PWM - enable 3pwm mode + pinMode(M_PWM,OUTPUT); + digitalWrite(M_PWM,HIGH); + // OD_ADJ - set the maximum overcurrent limit possible + // Better option would be to use voltage divisor to set exact value + pinMode(OC_ADJ,OUTPUT); + digitalWrite(OC_ADJ,HIGH); + pinMode(OC_GAIN,OUTPUT); + digitalWrite(OC_GAIN,LOW); + + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 19; + driver.pwm_frequency = 15000; // suggested under 18khz + driver.init(); + // link the motor and the driver + motor.linkDriver(&driver); + // link current sense and the driver + cs.linkDriver(&driver); + + // align voltage + motor.voltage_sensor_align = 0.5; + + // control loop type and torque mode + motor.torque_controller = TorqueControlType::voltage; + motor.controller = MotionControlType::torque; + motor.motion_downsample = 0.0; + + // velocity loop PID + motor.PID_velocity.P = 0.2; + motor.PID_velocity.I = 5.0; + // Low pass filtering time constant + motor.LPF_velocity.Tf = 0.02; + // angle loop PID + motor.P_angle.P = 20.0; + // Low pass filtering time constant + motor.LPF_angle.Tf = 0.0; + // current q loop PID + motor.PID_current_q.P = 3.0; + motor.PID_current_q.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_q.Tf = 0.02; + // current d loop PID + motor.PID_current_d.P = 3.0; + motor.PID_current_d.I = 100.0; + // Low pass filtering time constant + motor.LPF_current_d.Tf = 0.02; + + // Limits + motor.velocity_limit = 100.0; // 100 rad/s velocity limit + motor.voltage_limit = 12.0; // 12 Volt limit + motor.current_limit = 2.0; // 2 Amp current limit + + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q + motor.monitor_downsample = 0; + + // initialise motor + motor.init(); + + cs.init(); + // driver 8302 has inverted gains on all channels + cs.gain_a *=-1; + cs.gain_b *=-1; + cs.gain_c *=-1; + motor.linkCurrentSense(&cs); + + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 0; + + // define the motor id + command.add('M', onMotor, "motor"); + + Serial.println(F("Full control example: ")); + Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); + Serial.println(F("Initial motion control loop is voltage loop.")); + Serial.println(F("Initial target voltage 2V.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // monitoring the state variables + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/library.properties b/library.properties index d4b639d5..79e6cc4b 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.2.2 +version=2.2.3 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index c417e684..6f4eb12d 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -121,7 +121,15 @@ int BLDCMotor::initFOC( float zero_electric_offset, Direction _sensor_direction // and checks the direction of measuremnt. _delay(500); if(exit_flag){ - if(current_sense) exit_flag *= alignCurrentSense(); + if(current_sense){ + if (!current_sense->initialized) { + motor_status = FOCMotorStatus::motor_calib_failed; + SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized"); + exit_flag = 0; + }else{ + exit_flag *= alignCurrentSense(); + } + } else SIMPLEFOC_DEBUG("MOT: No current sense."); } @@ -175,7 +183,7 @@ int BLDCMotor::alignSensor() { for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0f; setPhaseVoltage(voltage_sensor_align, 0, angle); - sensor->update(); + sensor->update(); _delay(2); } // take and angle in the middle @@ -185,7 +193,7 @@ int BLDCMotor::alignSensor() { for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); - sensor->update(); + sensor->update(); _delay(2); } sensor->update(); @@ -337,7 +345,7 @@ void BLDCMotor::move(float new_target) { // when switching to a 2-component representation. if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode - // get angular velocity + // get angular velocity TODO the velocity reading probably also shouldn't happen in open loop modes? shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated // if disabled do nothing @@ -628,9 +636,11 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit); - + if(_isset(phase_resistance)){ + Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf))/phase_resistance; + } // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -666,8 +676,11 @@ float BLDCMotor::angleOpenloop(float target_angle){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit); + if(_isset(phase_resistance)){ + Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf))/phase_resistance; + } // set the maximal allowed voltage (voltage_limit) with the necessary angle // sensor precision: this calculation is OK due to the normalisation setPhaseVoltage(Uq, 0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs)); diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index 4c87be67..a585ebe2 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -78,7 +78,7 @@ class BLDCMotor: public FOCMotor * @param Ud Current voltage in d axis to set to the motor * @param angle_el current electrical angle of the motor */ - void setPhaseVoltage(float Uq, float Ud, float angle_el); + void setPhaseVoltage(float Uq, float Ud, float angle_el) override; private: // FOC methods diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index f8b112ef..9dccd1ea 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -141,6 +141,7 @@ int StepperMotor::alignSensor() { for (int i = 0; i <=500; i++ ) { float angle = _3PI_2 + _2PI * i / 500.0f; setPhaseVoltage(voltage_sensor_align, 0, angle); + sensor->update(); _delay(2); } // take and angle in the middle @@ -150,6 +151,7 @@ int StepperMotor::alignSensor() { for (int i = 500; i >=0; i-- ) { float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); + sensor->update(); _delay(2); } sensor->update(); @@ -377,8 +379,11 @@ float StepperMotor::velocityOpenloop(float target_velocity){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit); + if(_isset(phase_resistance)){ + Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf))/phase_resistance; + } // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -412,8 +417,11 @@ float StepperMotor::angleOpenloop(float target_angle){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) - Uq = _constrain(current_limit*phase_resistance + voltage_bemf,-voltage_limit, voltage_limit); + if(_isset(phase_resistance)){ + Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit); + // recalculate the current + current.q = (Uq - fabs(voltage_bemf))/phase_resistance; + } // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle((shaft_angle), pole_pairs)); diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 9ee39b79..8d9fcc1b 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -78,10 +78,7 @@ class StepperMotor: public FOCMotor float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform - private: - - // FOC methods - /** + /** * Method using FOC to set Uq to the motor at the optimal angle * Heart of the FOC algorithm * @@ -89,8 +86,10 @@ class StepperMotor: public FOCMotor * @param Ud Current voltage in d axis to set to the motor * @param angle_el current electrical angle of the motor */ - void setPhaseVoltage(float Uq, float Ud , float angle_el); + void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + private: + /** Sensor alignment to electrical 0 angle of the motor */ int alignSensor(); /** Motor and sensor alignment to the sensors absolute 0 angle */ diff --git a/src/common/base_classes/CurrentSense.h b/src/common/base_classes/CurrentSense.h index ad1bd9ca..ad9f926d 100644 --- a/src/common/base_classes/CurrentSense.h +++ b/src/common/base_classes/CurrentSense.h @@ -28,7 +28,8 @@ class CurrentSense{ // variables bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC() - BLDCDriver* driver; //!< driver link + BLDCDriver* driver = nullptr; //!< driver link + bool initialized = false; // true if current sense was successfully initialized void* params = 0; //!< pointer to hardware specific parameters of current sensing /** diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 670c69b2..0b86b89c 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -127,6 +127,16 @@ class FOCMotor */ virtual void move(float target = NOT_SET)=0; + /** + * Method using FOC to set Uq to the motor at the optimal angle + * Heart of the FOC algorithm + * + * @param Uq Current voltage in q axis to set to the motor + * @param Ud Current voltage in d axis to set to the motor + * @param angle_el current electrical angle of the motor + */ + virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=0; + // State calculation methods /** Shaft angle calculation in radians [rad] */ float shaftAngle(); @@ -137,6 +147,7 @@ class FOCMotor float shaftVelocity(); + /** * Electrical angle calculation */ diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index f3b19053..f806cddf 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -18,7 +18,7 @@ void Sensor::update() { float Sensor::getVelocity() { // calculate sample time float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6; - if (Ts 0 ) ) +#ifndef _round #define _round(x) ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f)) +#endif #define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) #define _sqrt(a) (_sqrtApprox(a)) #define _isset(a) ( (a) != (NOT_SET) ) diff --git a/src/communication/StepDirListener.cpp b/src/communication/StepDirListener.cpp index c8f19b47..ee4f69e9 100644 --- a/src/communication/StepDirListener.cpp +++ b/src/communication/StepDirListener.cpp @@ -13,7 +13,7 @@ void StepDirListener::init(){ } void StepDirListener::enableInterrupt(void (*doA)()){ - attachInterrupt(digitalPinToInterrupt(pin_step), doA, CHANGE); + attachInterrupt(digitalPinToInterrupt(pin_step), doA, polarity); } void StepDirListener::attach(float* variable){ @@ -22,15 +22,15 @@ void StepDirListener::attach(float* variable){ void StepDirListener::handle(){ // read step status - bool step = digitalRead(pin_step); + //bool step = digitalRead(pin_step); // update counter only on rising edge - if(step && step != step_active){ - if(digitalRead(pin_dir)) + //if(step && step != step_active){ + if(digitalRead(pin_dir)) count++; - else + else count--; - } - step_active = step; + //} + //step_active = step; // if attached variable update it if(attached_variable) *attached_variable = getValue(); } diff --git a/src/communication/StepDirListener.h b/src/communication/StepDirListener.h index 1c50c700..7d7f2494 100644 --- a/src/communication/StepDirListener.h +++ b/src/communication/StepDirListener.h @@ -4,6 +4,12 @@ #include "Arduino.h" #include "../common/foc_utils.h" + +#if defined(_STM32_DEF_) || defined(ESP_H) || defined(ARDUINO_ARCH_AVR) || defined(ARDUINO_SAM_DUE) +#define PinStatus int +#endif + + /** * Step/Dir listenner class for easier interraction with this communication interface. */ @@ -47,11 +53,13 @@ class StepDirListener int pin_step; //!< step pin int pin_dir; //!< direction pin long count; //!< current counter value - should be set to 0 for homing + PinStatus polarity = RISING; //!< polarity of the step pin private: float* attached_variable = nullptr; //!< pointer to the attached variable float counter_to_value; //!< step counter to value - bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable + //bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable + }; #endif \ No newline at end of file diff --git a/src/current_sense/GenericCurrentSense.cpp b/src/current_sense/GenericCurrentSense.cpp index 269e0ba5..d0592ef2 100644 --- a/src/current_sense/GenericCurrentSense.cpp +++ b/src/current_sense/GenericCurrentSense.cpp @@ -13,6 +13,8 @@ int GenericCurrentSense::init(){ if(initCallback != nullptr) initCallback(); // calibrate zero offsets calibrateOffsets(); + // set the initialized flag + initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success return 1; } diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 86f81a47..eff00785 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -21,12 +21,17 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ // 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(driver->params,pinA,pinB,pinC); + 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; } diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 8b9fe3ed..1453687b 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -29,6 +29,8 @@ int LowsideCurrentSense::init(){ _driverSyncLowSide(driver->params, params); // calibrate zero offsets calibrateOffsets(); + // set the initialized flag + initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED); // return success return 1; } diff --git a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp index 93aba630..807c387d 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp @@ -1,6 +1,6 @@ #include "esp32_adc_driver.h" -#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) +#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" diff --git a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp index 941fb385..6bde97dc 100644 --- a/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32/esp32_mcu.cpp @@ -68,8 +68,6 @@ 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){ - uint32_t raw_adc; - 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; diff --git a/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp new file mode 100644 index 00000000..2e956770 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp @@ -0,0 +1,257 @@ +#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_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 \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp index 806c68e7..dc505d6f 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp @@ -1,10 +1,13 @@ #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 /** ****************************************************************************** @@ -92,7 +95,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) hadc1->Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc1->Init.LowPowerAutoWait = DISABLE; hadc1->Init.ContinuousConvMode = DISABLE; - hadc1->Init.NbrOfConversion = 2; + hadc1->Init.NbrOfConversion = 5; hadc1->Init.DiscontinuousConvMode = DISABLE; hadc1->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; hadc1->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; @@ -101,7 +104,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) if (HAL_ADC_Init(hadc1) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_Init failed!"); } /** Configure the ADC multi-mode @@ -109,7 +112,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) multimode.Mode = ADC_MODE_INDEPENDENT; if (HAL_ADCEx_MultiModeConfigChannel(hadc1, &multimode) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADCEx_MultiModeConfigChannel failed!"); } /** Configure Regular Channel */ @@ -121,7 +124,7 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } /** Configure Regular Channel */ @@ -129,7 +132,48 @@ void MX_ADC1_Init(ADC_HandleTypeDef* hadc1) sConfig.Rank = ADC_REGULAR_RANK_2; if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); + } + + //****************************************************************** + // Temp, Poti .... + /* Configure Regular Channel (PB12, Potentiometer) + */ + sConfig.Channel = ADC_CHANNEL_11; + sConfig.Rank = ADC_REGULAR_RANK_3; + 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_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 */ @@ -173,7 +217,7 @@ void MX_ADC2_Init(ADC_HandleTypeDef* hadc2) if (HAL_ADC_Init(hadc2) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_Init failed!"); } /** Configure Regular Channel */ @@ -185,7 +229,7 @@ void MX_ADC2_Init(ADC_HandleTypeDef* hadc2) sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!"); } /* USER CODE BEGIN ADC2_Init 2 */ diff --git a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp index 4adfff57..31cf3b38 100644 --- a/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp @@ -6,10 +6,11 @@ #include "Arduino.h" #include "../stm32_mcu.h" #include "../../../../drivers/hardware_specific/stm32_mcu.h" +#include "communication/SimpleFOCDebug.h" #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 4096.0f -#define ADC_BUF_LEN_1 2 +#define ADC_BUF_LEN_1 5 #define ADC_BUF_LEN_2 1 static ADC_HandleTypeDef hadc1; @@ -21,8 +22,8 @@ static OPAMP_HandleTypeDef hopamp3; static DMA_HandleTypeDef hdma_adc1; static DMA_HandleTypeDef hdma_adc2; -uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion -uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion +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 @@ -36,6 +37,14 @@ float _readADCVoltageInline(const int pin, const void* cs_params){ else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1 raw_adc = adcBuffer1[0]; #endif + + else if (pin == A_POTENTIOMETER) + raw_adc = adcBuffer1[2]; + else if (pin == A_TEMPERATURE) + raw_adc = adcBuffer1[3]; + else if (pin == A_VBUS) + raw_adc = adcBuffer1[4]; + return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; } @@ -52,7 +61,7 @@ void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){ hopamp->Init.UserTrimming = OPAMP_TRIMMING_FACTORY; if (HAL_OPAMP_Init(hopamp) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("HAL_OPAMP_Init failed!"); } } void _configureOPAMPs(OPAMP_HandleTypeDef *OPAMPA, OPAMP_HandleTypeDef *OPAMPB, OPAMP_HandleTypeDef *OPAMPC){ @@ -75,13 +84,24 @@ void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Chan HAL_DMA_DeInit(hdma_adc); if (HAL_DMA_Init(hdma_adc) != HAL_OK) { - Error_Handler(); + 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(); @@ -95,28 +115,21 @@ void* _configureADCInline(const void* driver_params, const int pinA,const int pi if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adcBuffer1, ADC_BUF_LEN_1) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("DMA read init failed"); } if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adcBuffer2, ADC_BUF_LEN_2) != HAL_OK) { - Error_Handler(); + SIMPLEFOC_DEBUG("DMA read init failed"); } HAL_OPAMP_Start(&hopamp1); HAL_OPAMP_Start(&hopamp2); HAL_OPAMP_Start(&hopamp3); - - // Check if the ADC DMA is collecting any data. - // If this fails, it likely means timer1 has not started. Verify that your application starts - // the motor pwm (usually BLDCDriver6PWM::init()) before initializing the ADC engine. - _delay(5); - if (adcBuffer1[0] == 0 || adcBuffer1[1] == 0 || adcBuffer2[0] == 0) { - return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; - } Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams { .pins = { pinA, pinB, pinC }, - .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION) + .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION), + .timer_handle = (HardwareTimer *)(HardwareTimer_Handle[get_timer_index(TIM1)]->__this) }; return params; diff --git a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp index 5015c058..720ff285 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp @@ -33,7 +33,7 @@ int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { - .pins={0}, + .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); diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp index ac890817..68a9d094 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp @@ -2,53 +2,29 @@ #if defined(STM32F4xx) +//#define SIMPLEFOC_STM32_DEBUG + #include "../../../../communication/SimpleFOCDebug.h" #define _TRGO_NOT_AVAILABLE 12345 - -// 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; -} - 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); @@ -67,6 +43,10 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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; @@ -78,9 +58,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive hadc.Init.NbrOfConversion = 1; hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; - HAL_ADC_Init(&hadc); - /**Configure for the selected ADC regular channel to be converted. - */ + 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 */ @@ -119,20 +102,37 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1); #endif + // first channel - sConfigInjected.InjectedRank = 1; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + 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 = 2; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + 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 = 3; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + 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 diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h index 35b5aa67..42d75aa9 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h @@ -8,6 +8,7 @@ #include "../../../../common/foc_utils.h" #include "../../../../drivers/hardware_specific/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); diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp index f764f65c..0ab1c8f8 100644 --- a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp @@ -7,6 +7,7 @@ #include "../../../hardware_api.h" #include "../stm32_mcu.h" #include "stm32f4_hal.h" +#include "stm32f4_utils.h" #include "Arduino.h" @@ -21,21 +22,10 @@ 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={0}, + .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); diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp new file mode 100644 index 00000000..20793d8c --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp @@ -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 \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h new file mode 100644 index 00000000..b4549bad --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h @@ -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 \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp index 10587bd1..87b3ebdc 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp @@ -3,100 +3,31 @@ #if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) #include "../../../../communication/SimpleFOCDebug.h" -#define _TRGO_NOT_AVAILABLE 12345 - -// 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 == TIM2) - 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 == TIM2) - 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; -} +//#define SIMPLEFOC_STM32_DEBUG 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) + + // 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(); @@ -127,6 +58,26 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive __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 @@ -134,21 +85,31 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive #endif return -1; // error not a valid ADC instance } - - hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + +#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.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 = 1; + hadc.Init.NbrOfConversion = 2; hadc.Init.DMAContinuousRequests = DISABLE; hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; - HAL_ADC_Init(&hadc); - /**Configure for the selected ADC regular channel to be converted. - */ + 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 */ @@ -156,8 +117,12 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive 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; @@ -176,36 +141,80 @@ int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* drive } if( cs_params->timer_handle == NP ){ // not possible to use these timers for low-side current sense - #ifdef SIMPLEFOC_STM32_DEBUG +#ifdef SIMPLEFOC_STM32_DEBUG SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); - #endif +#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 = 1; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[0]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + 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 = 2; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[1]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + 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 = 3; - sConfigInjected.InjectedChannel = STM_PIN_CHANNEL(pinmap_function(analogInputToPinName(cs_params->pins[2]), PinMap_ADC)); - HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected); + 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(ADC1_2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + + + 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; @@ -229,10 +238,30 @@ void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const in } extern "C" { - void ADC_IRQHandler(void) + 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 \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h index c4159c72..8d3b9b59 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h @@ -9,6 +9,7 @@ #include "../../../../common/foc_utils.h" #include "../../../../drivers/hardware_specific/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); diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp index 254216f3..a18905e6 100644 --- a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp @@ -8,6 +8,7 @@ #include "../../../hardware_api.h" #include "../stm32_mcu.h" #include "stm32g4_hal.h" +#include "stm32g4_utils.h" #include "Arduino.h" @@ -15,28 +16,18 @@ #define _ADC_RESOLUTION_G4 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; -} +// 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={0}, + .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); @@ -67,11 +58,11 @@ void _driverSyncLowSide(void* _driver_params, void* _cs_params){ // 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); } @@ -100,7 +91,7 @@ extern "C" { 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); + adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); } } diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp new file mode 100644 index 00000000..89a9bc34 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp @@ -0,0 +1,237 @@ +#include "stm32g4_utils.h" + +#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) + +/* 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 \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h new file mode 100644 index 00000000..fa857bd0 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h @@ -0,0 +1,34 @@ + +#ifndef STM32G4_UTILS_HAL +#define STM32G4_UTILS_HAL + +#include "Arduino.h" + +#if defined(STM32G4xx) && !defined(ARDUINO_B_G431B_ESC1) + +#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 \ No newline at end of file diff --git a/src/drivers/hardware_api.h b/src/drivers/hardware_api.h index cb2385b7..a1ca885a 100644 --- a/src/drivers/hardware_api.h +++ b/src/drivers/hardware_api.h @@ -19,6 +19,17 @@ typedef struct GenericDriverParams { float dead_zone; } GenericDriverParams; +/** + * Configuring PWM frequency, resolution and alignment + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param pwm_frequency - frequency in hertz - if applicable + * @param pinA pinA pwm pin + * + * @return -1 if failed, or pointer to internal driver parameters struct if successful + */ +void* _configure1PWM(long pwm_frequency, const int pinA); /** * Configuring PWM frequency, resolution and alignment @@ -80,6 +91,17 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const */ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l); +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 2PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param params the driver parameters + */ +void _writeDutyCycle1PWM(float dc_a, void* params); + /** * Function setting the duty cycle to the pwm pin (ex. analogWrite()) * - Stepper driver - 2PWM setting diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp index 8ee1a727..11b7b945 100644 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -25,6 +25,21 @@ void _pinHighFrequency(const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure1PWM(long pwm_frequency,const int pinA) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -56,6 +71,14 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index 49318c52..39d9bf38 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -17,6 +17,20 @@ void _pinHighFrequency(const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +// supports Arudino/ATmega328 +void* _configure1PWM(long pwm_frequency,const int pinA) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -50,6 +64,18 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } + + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/atmega32u4_mcu.cpp b/src/drivers/hardware_specific/atmega32u4_mcu.cpp index 9001fd90..1e2adb81 100644 --- a/src/drivers/hardware_specific/atmega32u4_mcu.cpp +++ b/src/drivers/hardware_specific/atmega32u4_mcu.cpp @@ -28,6 +28,20 @@ void _pinHighFrequency(const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure1PWM(long pwm_frequency,const int pinA) { + // High PWM frequency + // - always max 32kHz + _pinHighFrequency(pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -59,6 +73,17 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp index 55348e2b..2d34493e 100644 --- a/src/drivers/hardware_specific/esp32_ledc_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_ledc_mcu.cpp @@ -53,6 +53,33 @@ void _setHighFrequency(const long freq, const int pin, const int channel){ + + + +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + // check if enough channels available + if ( channel_index + 1 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED; + + int ch1 = channel_index++; + _setHighFrequency(pwm_frequency, pinA, ch1); + + ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams { + .channels = { ch1 }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + + + + + + + void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max @@ -123,6 +150,12 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in +void _writeDutyCycle1PWM(float dc_a, void* params){ + ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); +} + + + void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params){ ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_a, 0, _PWM_RES)); ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_b, 0, _PWM_RES)); diff --git a/src/drivers/hardware_specific/esp8266_mcu.cpp b/src/drivers/hardware_specific/esp8266_mcu.cpp index 5bdfd5d2..0f8c9397 100644 --- a/src/drivers/hardware_specific/esp8266_mcu.cpp +++ b/src/drivers/hardware_specific/esp8266_mcu.cpp @@ -12,6 +12,22 @@ void _setHighFrequency(const long freq, const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -60,6 +76,13 @@ void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const in return params; } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index cb3bfd34..fe6e65a4 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -5,6 +5,18 @@ __attribute__((weak)) void analogWrite(uint8_t pin, int value){ }; #endif +// function setting the high pwm frequency to the supplied pin +// - Stepper motor - 1PWM setting +// - hardware speciffic +// in generic case dont do anything +__attribute__((weak)) void* _configure1PWM(long pwm_frequency, const int pinA) { + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -59,6 +71,15 @@ __attribute__((weak)) void* _configure6PWM(long pwm_frequency, float dead_zone, } +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 1PWM setting +// - hardware speciffic +__attribute__((weak)) void _writeDutyCycle1PWM(float dc_a, void* params){ + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 2c127d71..f7b58f80 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -6,17 +6,31 @@ #define SIMPLEFOC_DEBUG_RP2040 +#include "../hardware_api.h" -#ifdef SIMPLEFOC_DEBUG_RP2040 -#ifndef SIMPLEFOC_RP2040_DEBUG_SERIAL -#define SIMPLEFOC_RP2040_DEBUG_SERIAL Serial -#endif +// these defines determine the polarity of the PWM output. Normally, the polarity is active-high, +// i.e. a high-level PWM output is expected to switch on the MOSFET. But should your driver design +// require inverted polarity, you can change the defines below, or set them via your build environment +// or board definition files. +// used for 2-PWM, 3-PWM, and 4-PWM modes +#ifndef SIMPLEFOC_PWM_ACTIVE_HIGH +#define SIMPLEFOC_PWM_ACTIVE_HIGH true +#endif +// used fof 6-PWM mode, high-side +#ifndef SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH +#define SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH true +#endif +// used fof 6-PWM mode, low-side +#ifndef SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH +#define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH true #endif -#include "Arduino.h" +#define _PWM_FREQUENCY 24000 +#define _PWM_FREQUENCY_MAX 66000 +#define _PWM_FREQUENCY_MIN 5000 typedef struct RP2040DriverParams { @@ -46,18 +60,18 @@ void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* para pwm_set_phase_correct(slice, true); uint16_t wrapvalue = ((125L * 1000L * 1000L) / pwm_frequency) / 2L - 1L; if (wrapvalue < 999) wrapvalue = 999; // 66kHz, resolution 1000 - if (wrapvalue > 3299) wrapvalue = 3299; // 20kHz, resolution 3300 + if (wrapvalue > 12499) wrapvalue = 12499; // 20kHz, resolution 12500 #ifdef SIMPLEFOC_DEBUG_RP2040 - SIMPLEFOC_RP2040_DEBUG_SERIAL.print("Configuring pin "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pin); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" slice "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(slice); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" channel "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(chan); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" frequency "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(pwm_frequency); - SIMPLEFOC_RP2040_DEBUG_SERIAL.print(" top value "); - SIMPLEFOC_RP2040_DEBUG_SERIAL.println(wrapvalue); + SimpleFOCDebug::print("Configuring pin "); + SimpleFOCDebug::print(pin); + SimpleFOCDebug::print(" slice "); + SimpleFOCDebug::print((int)slice); + SimpleFOCDebug::print(" channel "); + SimpleFOCDebug::print((int)chan); + SimpleFOCDebug::print(" frequency "); + SimpleFOCDebug::print((int)pwm_frequency); + SimpleFOCDebug::print(" top value "); + SimpleFOCDebug::println(wrapvalue); #endif pwm_set_wrap(slice, wrapvalue); wrapvalues[slice] = wrapvalue; @@ -77,15 +91,30 @@ void syncSlices() { pwm_set_counter(i, 0); } // enable all slices - pwm_set_mask_enabled(0x7F); + pwm_set_mask_enabled(0xFF); +} + + + +void* _configure1PWM(long pwm_frequency, const int pinA) { + RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); + params->pwm_frequency = pwm_frequency; + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + syncSlices(); + return params; } + void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; - setupPWM(pinA, pwm_frequency, false, params, 0); - setupPWM(pinB, pwm_frequency, false, params, 1); + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); syncSlices(); return params; } @@ -94,10 +123,12 @@ void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; - setupPWM(pinA, pwm_frequency, false, params, 0); - setupPWM(pinB, pwm_frequency, false, params, 1); - setupPWM(pinC, pwm_frequency, false, params, 2); + setupPWM(pinA, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); + setupPWM(pinC, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 2); syncSlices(); return params; } @@ -107,11 +138,13 @@ void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const i void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; - setupPWM(pin1A, pwm_frequency, false, params, 0); - setupPWM(pin1B, pwm_frequency, false, params, 1); - setupPWM(pin2A, pwm_frequency, false, params, 2); - setupPWM(pin2B, pwm_frequency, false, params, 3); + setupPWM(pin1A, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0); + setupPWM(pin1B, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1); + setupPWM(pin2A, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 2); + setupPWM(pin2B, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 3); syncSlices(); return params; } @@ -120,14 +153,16 @@ void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { // non-PIO solution... RP2040DriverParams* params = new RP2040DriverParams(); + if( !pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; + else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); params->pwm_frequency = pwm_frequency; params->dead_zone = dead_zone; - setupPWM(pinA_h, pwm_frequency, false, params, 0); - setupPWM(pinB_h, pwm_frequency, false, params, 2); - setupPWM(pinC_h, pwm_frequency, false, params, 4); - setupPWM(pinA_l, pwm_frequency, true, params, 1); - setupPWM(pinB_l, pwm_frequency, true, params, 3); - setupPWM(pinC_l, pwm_frequency, true, params, 5); + setupPWM(pinA_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 0); + setupPWM(pinB_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 2); + setupPWM(pinC_h, pwm_frequency, !SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, params, 4); + setupPWM(pinA_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 1); + setupPWM(pinB_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 3); + setupPWM(pinC_l, pwm_frequency, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH, params, 5); syncSlices(); return params; } @@ -143,6 +178,12 @@ void writeDutyCycle(float val, uint slice, uint chan) { +void _writeDutyCycle1PWM(float dc_a, void* params) { + writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); +} + + + void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]); diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index a9191702..f9cd8467 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -14,6 +14,36 @@ #endif +#ifndef TCC3_CH0 +#define TCC3_CH0 NOT_ON_TIMER +#define TCC3_CH1 NOT_ON_TIMER +#endif + +#ifndef TCC4_CH0 +#define TCC4_CH0 NOT_ON_TIMER +#define TCC4_CH1 NOT_ON_TIMER +#endif + + +#ifndef TC4_CH0 +#define TC4_CH0 NOT_ON_TIMER +#define TC4_CH1 NOT_ON_TIMER +#endif + +#ifndef TC5_CH0 +#define TC5_CH0 NOT_ON_TIMER +#define TC5_CH1 NOT_ON_TIMER +#endif + +#ifndef TC6_CH0 +#define TC6_CH0 NOT_ON_TIMER +#define TC6_CH1 NOT_ON_TIMER +#endif + +#ifndef TC7_CH0 +#define TC7_CH0 NOT_ON_TIMER +#define TC7_CH1 NOT_ON_TIMER +#endif @@ -109,7 +139,12 @@ struct wo_association WO_associations[] = { wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; +#ifndef TCC3_CC_NUM +uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM }; +#else uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM, TCC3_CC_NUM, TCC4_CC_NUM }; +#endif + struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { for (int i=0;iresume(); + #ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-DRV: Starting timer ", getTimerNumber(get_timer_index(timers_to_start[i]->getHandle()->Instance))); + #endif } } diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index 93331252..37865d2d 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -12,6 +12,21 @@ void _setHighFrequency(const long freq, const int pin){ } +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 2PWM setting +// - hardware speciffic +void* _configure1PWM(long pwm_frequency, const int pinA) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + _setHighFrequency(pwm_frequency, pinA); + GenericDriverParams* params = new GenericDriverParams { + .pins = { pinA }, + .pwm_frequency = pwm_frequency + }; + return params; +} + + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -60,6 +75,17 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i return params; } + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 2PWM setting +// - hardware speciffic +void _writeDutyCycle1PWM(float dc_a, void* params) { + // transform duty cycle from [0,1] to [0,255] + analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); +} + + // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic @@ -68,6 +94,8 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); } + + // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 3d2ba42e..6c2367ff 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -116,7 +116,10 @@ float HallSensor::getVelocity(){ if (pulse_diff == 0 || ((long)(_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old return 0; } else { - return direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f); + float vel = direction * (_2PI / (float)cpr) / (pulse_diff / 1000000.0f); + // quick fix https://github.com/simplefoc/Arduino-FOC/issues/192 + if(vel < -velocity_max || vel > velocity_max) vel = 0.0f; //if velocity is out of range then make it zero + return vel; } } diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 9e80ed2b..d6b4493a 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -76,6 +76,9 @@ class HallSensor: public Sensor{ // this is sometimes useful to identify interrupt issues (e.g. weak or no pullup resulting in 1000s of interrupts) volatile long total_interrupts; + // variable used to filter outliers - rad/s + float velocity_max = 1000.0f; + private: Direction decodeDirection(int oldState, int newState);