diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml
index 0ffbed91..f870e780 100644
--- a/.github/workflows/ccpp.yml
+++ b/.github/workflows/ccpp.yml
@@ -26,7 +26,8 @@ jobs:
- 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:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo
+ - STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg
- STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive
- STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475
- STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1
@@ -92,6 +93,10 @@ jobs:
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, double_full_control_example.ino, stm32_current_control_low_side.ino
+ - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # nucleo f7 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, double_full_control_example.ino, stm32_current_control_low_side.ino
+
# Do not cancel all jobs / architectures if one job fails
fail-fast: false
diff --git a/.github/workflows/teensy.yml b/.github/workflows/teensy.yml
index 9fc88b9a..f1f89185 100644
--- a/.github/workflows/teensy.yml
+++ b/.github/workflows/teensy.yml
@@ -29,6 +29,11 @@ jobs:
run: pio ci --lib="." --board=teensy41 --board=teensy40
env:
PLATFORMIO_CI_SRC: examples/hardware_specific_examples/Teensy/Teensy4/open_loop_velocity_6pwm/open_loop_velocity_6pwm.ino
+
+ - name: PIO Run Teensy 4
+ run: pio ci --lib="." --board=teensy41 --board=teensy40
+ env:
+ PLATFORMIO_CI_SRC: examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino
- name: PIO Run Teensy 3
run: pio ci --lib="." --board=teensy31 --board=teensy30 --board=teensy35 --board=teensy36
diff --git a/README.md b/README.md
index 29e715fe..3323a794 100644
--- a/README.md
+++ b/README.md
@@ -19,11 +19,9 @@ Additionally, most of the efforts at this moment are still channeled towards the
Therefore this is an attempt to:
- 🎯 Demystify FOC algorithm and make a robust but simple Arduino library: [Arduino *SimpleFOClibrary*](https://docs.simplefoc.com/arduino_simplefoc_library_showcase)
- Support as many motor + sensor + driver + mcu combinations out there
-- 🎯 Develop a modular FOC supporting BLDC driver boards:
- - ***NEW*** 📢: *Minimalistic* BLDC driver (<3Amps) : [SimpleFOCMini ](https://github.com/simplefoc/SimpleFOCMini).
- - *Low-power* gimbal driver (<5Amps) : [*Arduino Simple**FOC**Shield*](https://docs.simplefoc.com/arduino_simplefoc_shield_showcase).
- - *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)
+- 🎯 Develop modular and easy to use FOC supporting BLDC driver boards
+ - For official driver boards see [SimpleFOCBoards](https://docs.simplefoc.com/boards)
+ - Many many more boards developed by the community members, see [SimpleFOCCommunity](https://community.simplefoc.com/)
> NEW RELEASE 📢 : SimpleFOClibrary v2.3.3
> - Teensy4
@@ -31,7 +29,8 @@ Therefore this is an attempt to:
> - support for center aligned 6pwm and 3pwm (optional) [#392](https://github.com/simplefoc/Arduino-FOC/pull/392)
> - stm32
> - support for center aligned pwm (even across multiple timers and motors/drivers) [#374](https://github.com/simplefoc/Arduino-FOC/pull/374), [#388](https://github.com/simplefoc/Arduino-FOC/pull/388)
-> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378),
+> - support for DMA based low-side current sensing: [#383](https://github.com/simplefoc/Arduino-FOC/pull/383),[#378](https://github.com/simplefoc/Arduino-FOC/pull/378)
+> - support for f7 architecture [#388](https://github.com/simplefoc/Arduino-FOC/pull/388),[#394](https://github.com/simplefoc/Arduino-FOC/pull/394)
> - KV rating calculation fix [#347](https://github.com/simplefoc/Arduino-FOC/pull/347)
> - Much more performant Space Vector PWM calculation [#340](https://github.com/simplefoc/Arduino-FOC/pull/340)
> - And much more:
diff --git a/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino
new file mode 100644
index 00000000..841134d8
--- /dev/null
+++ b/examples/hardware_specific_examples/DRV8302_driver/teensy4_current_control_low_side/teensy4_current_control_low_side.ino
@@ -0,0 +1,168 @@
+/**
+ * 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 EN_GATE 11
+#define M_PWM 22
+#define GAIN 20
+#define M_OC 23
+#define OC_ADJ 19
+
+#define INH_A 2
+#define INL_A 3
+#define INH_B 8
+#define INL_B 7
+#define INH_C 6
+#define INL_C 9
+
+#define IOUTA 14
+#define IOUTB 15
+#define IOUTC 16
+
+// 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);
+
+// encoder instance
+Encoder encoder = Encoder(10, 11, 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 6pwm mode
+ pinMode(M_PWM, OUTPUT);
+ digitalWrite(M_PWM,LOW); // high for 3pwm
+ // 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);
+
+
+ // driver config
+ // power supply voltage [V]
+ driver.voltage_power_supply = 19;
+ driver.pwm_frequency = 20000; // suggested not higher than 22khz
+ 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/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp
new file mode 100644
index 00000000..d4cffec6
--- /dev/null
+++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.cpp
@@ -0,0 +1,185 @@
+#include "stm32f7_hal.h"
+
+#if defined(STM32F7xx)
+
+//#define SIMPLEFOC_STM32_DEBUG
+
+#include "../../../../communication/SimpleFOCDebug.h"
+#define _TRGO_NOT_AVAILABLE 12345
+
+ADC_HandleTypeDef hadc;
+
+int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params)
+{
+ ADC_InjectionConfTypeDef sConfigInjected;
+
+ // check if all pins belong to the same ADC
+ ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
+ ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC);
+ ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr;
+ if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){
+#ifdef SIMPLEFOC_STM32_DEBUG
+ SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!");
+#endif
+ return -1;
+ }
+
+
+ /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
+ */
+ hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC);
+
+ if(hadc.Instance == ADC1) __HAL_RCC_ADC1_CLK_ENABLE();
+#ifdef ADC2 // if defined ADC2
+ else if(hadc.Instance == ADC2) __HAL_RCC_ADC2_CLK_ENABLE();
+#endif
+#ifdef ADC3 // if defined ADC3
+ else if(hadc.Instance == ADC3) __HAL_RCC_ADC3_CLK_ENABLE();
+#endif
+ else{
+#ifdef SIMPLEFOC_STM32_DEBUG
+ SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!");
+#endif
+ return -1; // error not a valid ADC instance
+ }
+
+#ifdef SIMPLEFOC_STM32_DEBUG
+ SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1);
+#endif
+
+ hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2;
+ hadc.Init.Resolution = ADC_RESOLUTION_12B;
+ hadc.Init.ScanConvMode = ENABLE;
+ hadc.Init.ContinuousConvMode = DISABLE;
+ hadc.Init.DiscontinuousConvMode = DISABLE;
+ hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+ hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now
+ hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT;
+ hadc.Init.NbrOfConversion = 1;
+ hadc.Init.DMAContinuousRequests = DISABLE;
+ hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+ if ( HAL_ADC_Init(&hadc) != HAL_OK){
+#ifdef SIMPLEFOC_STM32_DEBUG
+ SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!");
+#endif
+ return -1;
+ }
+
+ /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
+ */
+ sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2;
+ sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES;
+ sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISINGFALLING;
+ sConfigInjected.AutoInjectedConv = DISABLE;
+ sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
+ sConfigInjected.InjectedOffset = 0;
+
+ // automating TRGO flag finding - hardware specific
+ uint8_t tim_num = 0;
+ for (size_t i=0; i<6; i++) {
+ HardwareTimer *timer_to_check = driver_params->timers[tim_num++];
+ TIM_TypeDef *instance_to_check = timer_to_check->getHandle()->Instance;
+
+ // bool TRGO_already_configured = instance_to_check->CR2 & LL_TIM_TRGO_UPDATE;
+ // if(TRGO_already_configured) continue;
+
+ uint32_t trigger_flag = _timerToInjectedTRGO(timer_to_check);
+ if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels
+
+ // if the code comes here, it has found the timer available
+ // timer does have trgo flag for injected channels
+ sConfigInjected.ExternalTrigInjecConv = trigger_flag;
+
+ // this will be the timer with which the ADC will sync
+ cs_params->timer_handle = timer_to_check;
+ if (!IS_TIM_REPETITION_COUNTER_INSTANCE(instance_to_check)) {
+ // workaround for errata 2.2.1 in ES0290 Rev 7
+ // https://www.st.com/resource/en/errata_sheet/es0290-stm32f74xxx-and-stm32f75xxx-device-limitations-stmicroelectronics.pdf
+ __HAL_RCC_DAC_CLK_ENABLE();
+ }
+ // done
+ break;
+ }
+ if( cs_params->timer_handle == NP ){
+ // not possible to use these timers for low-side current sense
+ #ifdef SIMPLEFOC_STM32_DEBUG
+ SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!");
+ #endif
+ return -1;
+ }
+ // display which timer is being used
+ #ifdef SIMPLEFOC_STM32_DEBUG
+ // it would be better to use the getTimerNumber from driver
+ SIMPLEFOC_DEBUG("STM32-CS: injected trigger for timer index: ", get_timer_index(cs_params->timer_handle->getHandle()->Instance) + 1);
+ #endif
+
+
+ // first channel
+ sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1;
+ sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0]));
+ if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
+#ifdef SIMPLEFOC_STM32_DEBUG
+ SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) );
+#endif
+ return -1;
+ }
+
+ // second channel
+ sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2;
+ sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1]));
+ if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
+#ifdef SIMPLEFOC_STM32_DEBUG
+ SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ;
+#endif
+ return -1;
+ }
+
+ // third channel - if exists
+ if(_isset(cs_params->pins[2])){
+ sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3;
+ sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2]));
+ if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){
+#ifdef SIMPLEFOC_STM32_DEBUG
+ SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ;
+#endif
+ return -1;
+ }
+ }
+
+ #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
+ // enable interrupt
+ HAL_NVIC_SetPriority(ADC_IRQn, 0, 0);
+ HAL_NVIC_EnableIRQ(ADC_IRQn);
+ #endif
+
+ cs_params->adc_handle = &hadc;
+ return 0;
+}
+
+void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC)
+{
+ uint8_t cnt = 0;
+ if(_isset(pinA)){
+ pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC);
+ cs_params->pins[cnt++] = pinA;
+ }
+ if(_isset(pinB)){
+ pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC);
+ cs_params->pins[cnt++] = pinB;
+ }
+ if(_isset(pinC)){
+ pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC);
+ cs_params->pins[cnt] = pinC;
+ }
+}
+
+#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
+extern "C" {
+ void ADC_IRQHandler(void)
+ {
+ HAL_ADC_IRQHandler(&hadc);
+ }
+}
+#endif
+
+#endif
\ No newline at end of file
diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h
new file mode 100644
index 00000000..0a3614b5
--- /dev/null
+++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_hal.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "Arduino.h"
+
+#if defined(STM32F7xx)
+#include "stm32f7xx_hal.h"
+#include "../../../../common/foc_utils.h"
+#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
+#include "../stm32_mcu.h"
+#include "stm32f7_utils.h"
+
+int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params);
+void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC);
+
+#endif
diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp
new file mode 100644
index 00000000..3040ea46
--- /dev/null
+++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_mcu.cpp
@@ -0,0 +1,111 @@
+#include "../../../hardware_api.h"
+
+#if defined(STM32F7xx)
+#include "../../../../common/foc_utils.h"
+#include "../../../../drivers/hardware_api.h"
+#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
+#include "../../../hardware_api.h"
+#include "../stm32_mcu.h"
+#include "stm32f7_hal.h"
+#include "stm32f7_utils.h"
+#include "Arduino.h"
+
+
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 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] = {1};
+
+void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){
+
+ Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams {
+ .pins={(int)NOT_SET,(int)NOT_SET,(int)NOT_SET},
+ .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION)
+ };
+ _adc_gpio_init(cs_params, pinA,pinB,pinC);
+ if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
+ return cs_params;
+}
+
+
+void _driverSyncLowSide(void* _driver_params, void* _cs_params){
+ STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
+ Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
+
+ // if compatible timer has not been found
+ if (cs_params->timer_handle == NULL) return;
+
+ // stop all the timers for the driver
+ _stopTimers(driver_params->timers, 6);
+
+ // if timer has repetition counter - it will downsample using it
+ // and it does not need the software downsample
+ if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){
+ // adjust the initial timer state such that the trigger
+ // - for DMA transfer aligns with the pwm peaks instead of throughs.
+ // - for interrupt based ADC transfer
+ // - only necessary for the timers that have repetition counters
+
+ cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
+ cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR;
+ // remember that this timer has repetition counter - no need to downasmple
+ needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0;
+ }
+ // set the trigger output event
+ LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
+
+ // start the adc
+ #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
+ HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle);
+ #else
+ HAL_ADCEx_InjectedStart(cs_params->adc_handle);
+ #endif
+
+ // restart all the timers of the driver
+ _startTimers(driver_params->timers, 6);
+}
+
+
+// function reading an ADC value and returning the read voltage
+float _readADCVoltageLowSide(const int pin, const void* cs_params){
+ for(int i=0; i < 3; i++){
+ if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]){ // found in the buffer
+ #ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
+ return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
+ #else
+ // an optimized way to go from i to the channel i=0 -> channel 1, i=1 -> channel 2, i=2 -> channel 3
+ uint32_t channel = (i == 0) ? ADC_INJECTED_RANK_1 : (i == 1) ? ADC_INJECTED_RANK_2 : ADC_INJECTED_RANK_3;
+ return HAL_ADCEx_InjectedGetValue(((Stm32CurrentSenseParams*)cs_params)->adc_handle, channel) * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
+ #endif
+ }
+ }
+ return 0;
+}
+
+#ifdef SIMPLEFOC_STM32_ADC_INTERRUPT
+extern "C" {
+ void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){
+
+ // calculate the instance
+ int adc_index = _adcToIndex(AdcHandle);
+
+ // if the timer han't repetition counter - downsample two times
+ if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) {
+ tim_downsample[adc_index] = 0;
+ return;
+ }
+
+ adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1);
+ adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2);
+ adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3);
+ }
+}
+#endif
+
+#endif
\ No newline at end of file
diff --git a/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp
new file mode 100644
index 00000000..d5f8c6b2
--- /dev/null
+++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.cpp
@@ -0,0 +1,255 @@
+#include "stm32f7_utils.h"
+
+#if defined(STM32F7xx)
+
+/* Exported Functions */
+
+
+PinName analog_to_pin(uint32_t pin) {
+ PinName pin_name = analogInputToPinName(pin);
+ if (pin_name == NC) {
+ return (PinName) pin;
+ }
+ return pin_name;
+}
+
+
+/**
+ * @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;
+}
+/*
+TIM1
+TIM2
+TIM3
+TIM4
+TIM5
+TIM6
+TIM7
+TIM12
+TIM13
+TIM14
+
+ADC_EXTERNALTRIGINJECCONV_T1_TRGO
+ADC_EXTERNALTRIGINJECCONV_T2_TRGO
+ADC_EXTERNALTRIGINJECCONV_T4_TRGO
+
+ADC_EXTERNALTRIGINJECCONV_T1_TRGO2
+ADC_EXTERNALTRIGINJECCONV_T8_TRGO2
+ADC_EXTERNALTRIGINJECCONV_T5_TRGO
+ADC_EXTERNALTRIGINJECCONV_T6_TRGO
+*/
+// 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
+#ifdef TIM6 // if defined timer 6
+ else if(timer->getHandle()->Instance == TIM6)
+ return ADC_EXTERNALTRIGINJECCONV_T6_TRGO;
+#endif
+#ifdef TIM8 // if defined timer 8
+ else if(timer->getHandle()->Instance == TIM8)
+ return ADC_EXTERNALTRIGINJECCONV_T8_TRGO;
+#endif
+ else
+ return _TRGO_NOT_AVAILABLE;
+}
+/*
+
+ADC_EXTERNALTRIGCONV_T5_TRGO
+ADC_EXTERNALTRIGCONV_T8_TRGO
+ADC_EXTERNALTRIGCONV_T8_TRGO2
+ADC_EXTERNALTRIGCONV_T1_TRGO
+ADC_EXTERNALTRIGCONV_T1_TRGO2
+ADC_EXTERNALTRIGCONV_T2_TRGO
+ADC_EXTERNALTRIGCONV_T4_TRGO
+ADC_EXTERNALTRIGCONV_T6_TRGO
+*/
+
+// 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 == TIM1)
+ return ADC_EXTERNALTRIGCONV_T1_TRGO;
+#ifdef TIM2 // if defined timer 2
+ else if(timer->getHandle()->Instance == TIM2)
+ return ADC_EXTERNALTRIGCONV_T2_TRGO;
+#endif
+#ifdef TIM4 // if defined timer 4
+ else if(timer->getHandle()->Instance == TIM4)
+ return ADC_EXTERNALTRIGCONV_T4_TRGO;
+#endif
+#ifdef TIM5 // if defined timer 5
+ else if(timer->getHandle()->Instance == TIM5)
+ return ADC_EXTERNALTRIGCONV_T5_TRGO;
+#endif
+#ifdef TIM6 // if defined timer 6
+ else if(timer->getHandle()->Instance == TIM6)
+ return ADC_EXTERNALTRIGCONV_T6_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/stm32f7/stm32f7_utils.h b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h
new file mode 100644
index 00000000..017ff464
--- /dev/null
+++ b/src/current_sense/hardware_specific/stm32/stm32f7/stm32f7_utils.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include "Arduino.h"
+
+#if defined(STM32F7xx)
+
+#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
\ No newline at end of file