try to fix submodule

firmware/lib/Arduino-FOC/src/drivers/BLDCDriver3PWM.cpp

@@ -0,0 +1,92 @@
+#include "BLDCDriver3PWM.h"
+
+BLDCDriver3PWM::BLDCDriver3PWM(int phA, int phB, int phC, int en1, int en2, int en3){
+  // Pin initialization
+  pwmA = phA;
+  pwmB = phB;
+  pwmC = phC;
+
+  // enable_pin pin
+  enableA_pin = en1;
+  enableB_pin = en2;
+  enableC_pin = en3;
+
+  // default power-supply value
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+  pwm_frequency = NOT_SET;
+
+}
+
+// enable motor driver
+void  BLDCDriver3PWM::enable(){
+    // enable_pin the driver - if enable_pin pin available
+    if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, enable_active_high);
+    if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, enable_active_high);
+    if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, enable_active_high);
+    // set zero to PWM
+    setPwm(0,0,0);
+}
+
+// disable motor driver
+void BLDCDriver3PWM::disable()
+{
+  // set zero to PWM
+  setPwm(0, 0, 0);
+  // disable the driver - if enable_pin pin available
+  if ( _isset(enableA_pin) ) digitalWrite(enableA_pin, !enable_active_high);
+  if ( _isset(enableB_pin) ) digitalWrite(enableB_pin, !enable_active_high);
+  if ( _isset(enableC_pin) ) digitalWrite(enableC_pin, !enable_active_high);
+
+}
+
+// init hardware pins
+int BLDCDriver3PWM::init() {
+  // PWM pins
+  pinMode(pwmA, OUTPUT);
+  pinMode(pwmB, OUTPUT);
+  pinMode(pwmC, OUTPUT);
+  if( _isset(enableA_pin)) pinMode(enableA_pin, OUTPUT);
+  if( _isset(enableB_pin)) pinMode(enableB_pin, OUTPUT);
+  if( _isset(enableC_pin)) pinMode(enableC_pin, OUTPUT);
+
+
+  // sanity check for the voltage limit configuration
+  if(!_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;
+
+  // Set the pwm frequency to the pins
+  // hardware specific function - depending on driver and mcu
+  params = _configure3PWM(pwm_frequency, pwmA, pwmB, pwmC);
+  initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED);
+  return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
+}
+
+
+
+// Set voltage to the pwm pin
+void BLDCDriver3PWM::setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) {
+  // disable if needed
+  if( _isset(enableA_pin) &&  _isset(enableB_pin)  && _isset(enableC_pin) ){
+    digitalWrite(enableA_pin, sa == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high);
+    digitalWrite(enableB_pin, sb == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high);
+    digitalWrite(enableC_pin, sc == PhaseState::PHASE_ON ? enable_active_high:!enable_active_high);
+  }
+}
+
+// Set voltage to the pwm pin
+void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) {
+
+  // limit the voltage in driver
+  Ua = _constrain(Ua, 0.0f, voltage_limit);
+  Ub = _constrain(Ub, 0.0f, voltage_limit);
+  Uc = _constrain(Uc, 0.0f, voltage_limit);
+  // calculate duty cycle
+  // limited in [0,1]
+  dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
+  dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
+  dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
+
+  // hardware specific writing
+  // hardware specific function - depending on driver and mcu
+  _writeDutyCycle3PWM(dc_a, dc_b, dc_c, params);
+}

firmware/lib/Arduino-FOC/src/drivers/BLDCDriver3PWM.h

@@ -0,0 +1,64 @@
+#ifndef BLDCDriver3PWM_h
+#define BLDCDriver3PWM_h
+
+#include "../common/base_classes/BLDCDriver.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "hardware_api.h"
+
+/**
+ 3 pwm bldc driver class
+*/
+class BLDCDriver3PWM: public BLDCDriver
+{
+  public:
+    /**
+      BLDCDriver class constructor
+      @param phA A phase pwm pin
+      @param phB B phase pwm pin
+      @param phC C phase pwm pin
+      @param en1 enable pin (optional input)
+      @param en2 enable pin (optional input)
+      @param en3 enable pin (optional input)
+    */
+    BLDCDriver3PWM(int phA,int phB,int phC, int en1 = NOT_SET, int en2 = NOT_SET, int en3 = NOT_SET);
+    
+    /**  Motor hardware init function */
+  	int init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    // hardware variables
+  	int pwmA; //!< phase A pwm pin number
+  	int pwmB; //!< phase B pwm pin number
+  	int pwmC; //!< phase C pwm pin number
+    int enableA_pin; //!< enable pin number
+    int enableB_pin; //!< enable pin number
+    int enableC_pin; //!< enable pin number
+    bool enable_active_high = true;
+
+    /** 
+     * Set phase voltages to the harware 
+     * 
+     * @param Ua - phase A voltage
+     * @param Ub - phase B voltage
+     * @param Uc - phase C voltage
+    */
+    void setPwm(float Ua, float Ub, float Uc) override;
+
+    /** 
+     * Set phase voltages to the harware 
+     * 
+     * @param sc - phase A state : active / disabled ( high impedance )
+     * @param sb - phase B state : active / disabled ( high impedance )
+     * @param sa - phase C state : active / disabled ( high impedance )
+    */
+    virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) override;
+  private:
+};
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/BLDCDriver6PWM.cpp

@@ -0,0 +1,103 @@
+#include "BLDCDriver6PWM.h"
+
+BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int en){
+  // Pin initialization
+  pwmA_h = phA_h;
+  pwmB_h = phB_h;
+  pwmC_h = phC_h;
+  pwmA_l = phA_l;
+  pwmB_l = phB_l;
+  pwmC_l = phC_l;
+
+  // enable_pin pin
+  enable_pin = en;
+
+  // default power-supply value
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+  pwm_frequency = NOT_SET;
+
+  // dead zone initial - 2%
+  dead_zone = 0.02f;
+
+}
+
+// enable motor driver
+void  BLDCDriver6PWM::enable(){
+    // enable_pin the driver - if enable_pin pin available
+    if ( _isset(enable_pin) ) digitalWrite(enable_pin, enable_active_high);
+    // set phase state enabled
+    setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_ON);
+    // set zero to PWM
+    setPwm(0, 0, 0);
+}
+
+// disable motor driver
+void BLDCDriver6PWM::disable()
+{
+  // set phase state to disabled
+  setPhaseState(PhaseState::PHASE_OFF, PhaseState::PHASE_OFF, PhaseState::PHASE_OFF);
+  // set zero to PWM
+  setPwm(0, 0, 0);
+  // disable the driver - if enable_pin pin available
+  if ( _isset(enable_pin) ) digitalWrite(enable_pin, !enable_active_high);
+
+}
+
+// init hardware pins
+int BLDCDriver6PWM::init() {
+
+  // PWM pins
+  pinMode(pwmA_h, OUTPUT);
+  pinMode(pwmB_h, OUTPUT);
+  pinMode(pwmC_h, OUTPUT);
+  pinMode(pwmA_l, OUTPUT);
+  pinMode(pwmB_l, OUTPUT);
+  pinMode(pwmC_l, OUTPUT);
+  if(_isset(enable_pin)) pinMode(enable_pin, OUTPUT);
+
+
+  // sanity check for the voltage limit configuration
+  if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;
+
+  // set phase state to disabled
+  phase_state[0] = PhaseState::PHASE_OFF;
+  phase_state[1] = PhaseState::PHASE_OFF;
+  phase_state[2] = PhaseState::PHASE_OFF;
+
+  // set zero to PWM
+  dc_a = dc_b = dc_c = 0;
+
+  // configure 6pwm
+  // hardware specific function - depending on driver and mcu
+  params = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l);
+  initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED);
+  return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
+}
+
+
+// Set voltage to the pwm pin
+void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) {
+  // limit the voltage in driver
+  Ua = _constrain(Ua, 0, voltage_limit);
+  Ub = _constrain(Ub, 0, voltage_limit);
+  Uc = _constrain(Uc, 0, voltage_limit);
+  // calculate duty cycle
+  // limited in [0,1]
+  dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
+  dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
+  dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
+  // hardware specific writing
+  // hardware specific function - depending on driver and mcu
+  _writeDutyCycle6PWM(dc_a, dc_b, dc_c, phase_state, params);
+}
+
+
+// Set the phase state
+// actually changing the state is only done on the next call to setPwm, and depends
+// on the hardware capabilities of the driver and MCU.
+void BLDCDriver6PWM::setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) {
+  phase_state[0] = sa;
+  phase_state[1] = sb;
+  phase_state[2] = sc;
+}

firmware/lib/Arduino-FOC/src/drivers/BLDCDriver6PWM.h

@@ -0,0 +1,70 @@
+#ifndef BLDCDriver6PWM_h
+#define BLDCDriver6PWM_h
+
+#include "../common/base_classes/BLDCDriver.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "hardware_api.h"
+
+/**
+ 6 pwm bldc driver class
+*/
+class BLDCDriver6PWM: public BLDCDriver
+{
+  public:
+    /**
+      BLDCDriver class constructor
+      @param phA_h A phase pwm pin
+      @param phA_l A phase pwm pin
+      @param phB_h B phase pwm pin
+      @param phB_l A phase pwm pin
+      @param phC_h C phase pwm pin
+      @param phC_l A phase pwm pin
+      @param en enable pin (optional input)
+    */
+    BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h,int phC_l, int en = NOT_SET);
+    
+    /**  Motor hardware init function */
+  	int init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    // hardware variables
+  	int pwmA_h,pwmA_l; //!< phase A pwm pin number
+  	int pwmB_h,pwmB_l; //!< phase B pwm pin number
+  	int pwmC_h,pwmC_l; //!< phase C pwm pin number
+    int enable_pin; //!< enable pin number
+    bool enable_active_high = true;
+
+    float dead_zone; //!< a percentage of dead-time(zone) (both high and low side in low) for each pwm cycle [0,1]
+
+    PhaseState phase_state[3]; //!< phase state (active / disabled)
+
+
+    /** 
+     * Set phase voltages to the harware 
+     * 
+     * @param Ua - phase A voltage
+     * @param Ub - phase B voltage
+     * @param Uc - phase C voltage
+    */
+    void setPwm(float Ua, float Ub, float Uc) override;
+
+    /** 
+     * Set phase voltages to the harware 
+     * 
+     * @param sc - phase A state : active / disabled ( high impedance )
+     * @param sb - phase B state : active / disabled ( high impedance )
+     * @param sa - phase C state : active / disabled ( high impedance )
+    */
+    virtual void setPhaseState(PhaseState sa, PhaseState sb, PhaseState sc) override;
+
+  private:
+        
+};
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.cpp

@@ -0,0 +1,107 @@
+#include "StepperDriver2PWM.h"
+
+StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int* _in1, int _pwm2, int* _in2, int en1, int en2){
+  // Pin initialization
+  pwm1 = _pwm1; // phase 1 pwm pin number
+  dir1a = _in1[0]; // phase 1 INA pin number
+  dir1b = _in1[1]; // phase 1 INB pin number
+  pwm2 = _pwm2; // phase 2 pwm pin number
+  dir2a = _in2[0]; // phase 2 INA pin number
+  dir2b = _in2[1]; // phase 2 INB pin number
+
+  // enable_pin pin
+  enable_pin1 = en1;
+  enable_pin2 = en2;
+
+  // default power-supply value
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+  pwm_frequency = NOT_SET;
+
+}
+
+StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, int en1, int en2){
+  // Pin initialization
+  pwm1 = _pwm1; // phase 1 pwm pin number
+  dir1a = _dir1; // phase 1 direction pin
+  pwm2 = _pwm2; // phase 2 pwm pin number
+  dir2a = _dir2; // phase 2 direction pin
+  // these pins are not used
+  dir1b = NOT_SET;
+  dir2b = NOT_SET;
+
+  // enable_pin pin
+  enable_pin1 = en1;
+  enable_pin2 = en2;
+
+  // default power-supply value
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+  pwm_frequency = NOT_SET;
+
+}
+
+// enable motor driver
+void  StepperDriver2PWM::enable(){
+    // enable_pin the driver - if enable_pin pin available
+    if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH);
+    if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH);
+    // set zero to PWM
+    setPwm(0,0);
+}
+
+// disable motor driver
+void StepperDriver2PWM::disable()
+{
+  // set zero to PWM
+  setPwm(0, 0);
+  // disable the driver - if enable_pin pin available
+  if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW);
+  if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW);
+
+}
+
+// init hardware pins
+int StepperDriver2PWM::init() {
+  // PWM pins
+  pinMode(pwm1, OUTPUT);
+  pinMode(pwm2, OUTPUT);
+  pinMode(dir1a, OUTPUT);
+  pinMode(dir2a, OUTPUT);
+  if( _isset(dir1b) ) pinMode(dir1b, OUTPUT);
+  if( _isset(dir2b) ) pinMode(dir2b, OUTPUT);
+
+  if( _isset(enable_pin1) ) pinMode(enable_pin1, OUTPUT);
+  if( _isset(enable_pin2) ) pinMode(enable_pin2, OUTPUT);
+
+  // sanity check for the voltage limit configuration
+  if( !_isset(voltage_limit)  || voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;
+
+  // Set the pwm frequency to the pins
+  // hardware specific function - depending on driver and mcu
+  params = _configure2PWM(pwm_frequency, pwm1, pwm2);
+  initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED);
+  return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
+}
+
+
+// Set voltage to the pwm pin
+void StepperDriver2PWM::setPwm(float Ua, float Ub) {
+  float duty_cycle1(0.0f),duty_cycle2(0.0f);
+  // limit the voltage in driver
+  Ua = _constrain(Ua, -voltage_limit, voltage_limit);
+  Ub = _constrain(Ub, -voltage_limit, voltage_limit);
+  // hardware specific writing
+  duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0f,1.0f);
+  duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0f,1.0f);
+
+  // phase 1 direction
+  digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH);
+  if( _isset(dir1b) ) digitalWrite(dir1b, Ua >= 0 ? HIGH : LOW );
+  // phase 2 direction
+  digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH);
+  if( _isset(dir2b) ) digitalWrite(dir2b, Ub >= 0 ? HIGH : LOW );
+
+  // write to hardware
+  _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, params);
+}

firmware/lib/Arduino-FOC/src/drivers/StepperDriver2PWM.h

@@ -0,0 +1,68 @@
+#ifndef STEPPER_DRIVER_2PWM_h
+#define STEPPER_DRIVER_2PWM_h
+
+#include "../common/base_classes/StepperDriver.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "hardware_api.h"
+
+/**
+ 2 pwm stepper driver class
+*/
+class StepperDriver2PWM: public StepperDriver
+{
+  public:
+    /**
+      StepperMotor class constructor
+      @param pwm1  PWM1 phase pwm pin
+      @param in1   IN1A phase dir pin
+      @param pwm2  PWM2 phase pwm pin
+      @param in2   IN2A phase dir 
+      @param en1 enable pin phase 1 (optional input)
+      @param en2 enable pin phase 2 (optional input)
+    */
+    StepperDriver2PWM(int pwm1, int* in1, int pwm2, int* in2, int en1 = NOT_SET, int en2 = NOT_SET);
+    
+    /**
+      StepperMotor class constructor
+      @param pwm1  PWM1 phase pwm pin
+      @param dir1  DIR1 phase dir pin
+      @param pwm2  PWM2 phase pwm pin
+      @param dir2  DIR2 phase dir pin
+      @param en1 enable pin phase 1 (optional input)
+      @param en2 enable pin phase 2 (optional input)
+    */
+    StepperDriver2PWM(int pwm1, int dir1, int pwm2, int dir2, int en1 = NOT_SET, int en2 = NOT_SET);
+
+    /**  Motor hardware init function */
+  	int init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    // hardware variables
+    int pwm1; //!< phase 1 pwm pin number
+    int dir1a; //!< phase 1 INA pin number
+    int dir1b; //!< phase 1 INB pin number
+    int pwm2; //!< phase 2 pwm pin number
+    int dir2a; //!< phase 2 INA pin number
+    int dir2b; //!< phase 2 INB pin number
+    int enable_pin1; //!< enable pin number phase 1
+    int enable_pin2; //!< enable pin number phase 2
+
+    /** 
+     * Set phase voltages to the harware 
+     * 
+     * @param Ua phase A voltage
+     * @param Ub phase B voltage
+    */
+    void setPwm(float Ua, float Ub) override;
+
+  private:
+        
+};
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.cpp

@@ -0,0 +1,81 @@
+#include "StepperDriver4PWM.h"
+
+StepperDriver4PWM::StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B,int en1, int en2){
+  // Pin initialization
+  pwm1A = ph1A;
+  pwm1B = ph1B;
+  pwm2A = ph2A;
+  pwm2B = ph2B;
+
+  // enable_pin pin
+  enable_pin1 = en1;
+  enable_pin2 = en2;
+
+  // default power-supply value
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+  pwm_frequency = NOT_SET;
+
+}
+
+// enable motor driver
+void  StepperDriver4PWM::enable(){
+    // enable_pin the driver - if enable_pin pin available
+    if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH);
+    if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, HIGH);
+    // set zero to PWM
+    setPwm(0,0);
+}
+
+// disable motor driver
+void StepperDriver4PWM::disable()
+{
+  // set zero to PWM
+  setPwm(0, 0);
+  // disable the driver - if enable_pin pin available
+  if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW);
+  if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW);
+
+}
+
+// init hardware pins
+int StepperDriver4PWM::init() {
+
+  // PWM pins
+  pinMode(pwm1A, OUTPUT);
+  pinMode(pwm1B, OUTPUT);
+  pinMode(pwm2A, OUTPUT);
+  pinMode(pwm2B, OUTPUT);
+  if( _isset(enable_pin1) ) pinMode(enable_pin1, OUTPUT);
+  if( _isset(enable_pin2) ) pinMode(enable_pin2, OUTPUT);
+
+  // sanity check for the voltage limit configuration
+  if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;
+
+  // Set the pwm frequency to the pins
+  // hardware specific function - depending on driver and mcu
+  params = _configure4PWM(pwm_frequency, pwm1A, pwm1B, pwm2A, pwm2B);
+  initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED);  
+  return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
+}
+
+
+// Set voltage to the pwm pin
+void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) {
+  float duty_cycle1A(0.0f),duty_cycle1B(0.0f),duty_cycle2A(0.0f),duty_cycle2B(0.0f);
+  // limit the voltage in driver
+  Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit);
+  Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit);
+  // hardware specific writing
+  if( Ualpha > 0 )
+    duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f);
+  else
+    duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0f,1.0f);
+
+  if( Ubeta > 0 )
+    duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f);
+  else
+    duty_cycle2A = _constrain(abs(Ubeta)/voltage_power_supply,0.0f,1.0f);
+  // write to hardware
+  _writeDutyCycle4PWM(duty_cycle1A, duty_cycle1B, duty_cycle2A, duty_cycle2B, params);
+}

firmware/lib/Arduino-FOC/src/drivers/StepperDriver4PWM.h

@@ -0,0 +1,55 @@
+#ifndef STEPPER_DRIVER_4PWM_h
+#define STEPPER_DRIVER_4PWM_h
+
+#include "../common/base_classes/StepperDriver.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "hardware_api.h"
+
+/**
+ 4 pwm stepper driver class
+*/
+class StepperDriver4PWM: public StepperDriver
+{
+  public:
+    /**
+      StepperMotor class constructor
+      @param ph1A 1A phase pwm pin
+      @param ph1B 1B phase pwm pin
+      @param ph2A 2A phase pwm pin
+      @param ph2B 2B phase pwm pin
+      @param en1 enable pin phase 1 (optional input)
+      @param en2 enable pin phase 2 (optional input)
+    */
+    StepperDriver4PWM(int ph1A,int ph1B,int ph2A,int ph2B, int en1 = NOT_SET, int en2 = NOT_SET);
+    
+    /**  Motor hardware init function */
+  	int init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    // hardware variables
+  	int pwm1A; //!< phase 1A pwm pin number
+  	int pwm1B; //!< phase 1B pwm pin number
+  	int pwm2A; //!< phase 2A pwm pin number
+    int pwm2B; //!< phase 2B pwm pin number
+    int enable_pin1; //!< enable pin number phase 1
+    int enable_pin2; //!< enable pin number phase 2
+
+    /** 
+     * Set phase voltages to the harware 
+     * 
+     * @param Ua phase A voltage
+     * @param Ub phase B voltage
+    */
+    void setPwm(float Ua, float Ub) override;
+
+  private:
+        
+};
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_api.h

@@ -0,0 +1,180 @@
+#ifndef HARDWARE_UTILS_DRIVER_H
+#define HARDWARE_UTILS_DRIVER_H
+
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../communication/SimpleFOCDebug.h"
+#include "../common/base_classes/BLDCDriver.h"
+
+
+// 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 1-PWM, 2-PWM, 3-PWM, and 4-PWM modes
+#ifndef SIMPLEFOC_PWM_ACTIVE_HIGH
+#define SIMPLEFOC_PWM_ACTIVE_HIGH true
+#endif
+// used for 6-PWM mode, high-side
+#ifndef SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH
+#define SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH true
+#endif
+// used for 6-PWM mode, low-side
+#ifndef SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH
+#define SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH true
+#endif
+
+
+
+
+// flag returned if driver init fails
+#define SIMPLEFOC_DRIVER_INIT_FAILED ((void*)-1)
+
+// generic implementation of the hardware specific structure
+// containing all the necessary driver parameters
+// will be returned as a void pointer from the _configurexPWM functions
+// will be provided to the _writeDutyCyclexPWM() as a void pointer
+typedef struct GenericDriverParams {
+  int pins[6];
+  long pwm_frequency;
+  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
+ * - Stepper driver - 2PWM setting
+ * - hardware specific
+ * 
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pinA pinA bldc driver
+ * @param pinB pinB bldc driver
+ * 
+ * @return -1 if failed, or pointer to internal driver parameters struct if successful
+ */
+void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB);
+
+/** 
+ * Configuring PWM frequency, resolution and alignment
+ * - BLDC driver - 3PWM setting
+ * - hardware specific
+ * 
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pinA pinA bldc driver
+ * @param pinB pinB bldc driver
+ * @param pinC pinC bldc driver
+ * 
+ * @return -1 if failed, or pointer to internal driver parameters struct if successful
+ */
+void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC);
+
+/** 
+ * Configuring PWM frequency, resolution and alignment
+ * - Stepper driver - 4PWM setting
+ * - hardware specific
+ * 
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pin1A pin1A stepper driver
+ * @param pin1B pin1B stepper driver
+ * @param pin2A pin2A stepper driver
+ * @param pin2B pin2B stepper driver
+ * 
+ * @return -1 if failed, or pointer to internal driver parameters struct if successful
+ */
+void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B);
+
+/** 
+ * Configuring PWM frequency, resolution and alignment
+ * - BLDC driver - 6PWM setting
+ * - hardware specific
+ * 
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param dead_zone  duty cycle protection zone [0, 1] - both low and high side low - if applicable
+ * @param pinA_h pinA high-side bldc driver 
+ * @param pinA_l pinA low-side bldc driver 
+ * @param pinB_h pinA high-side bldc driver 
+ * @param pinB_l pinA low-side bldc driver 
+ * @param pinC_h pinA high-side bldc driver 
+ * @param pinC_l pinA low-side bldc driver 
+ * 
+ * @return -1 if failed, or pointer to internal driver parameters struct if successful
+ */
+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
+ * - 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 _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params);
+
+/** 
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - BLDC driver - 3PWM setting
+ * - hardware specific
+ * 
+ * @param dc_a  duty cycle phase A [0, 1]
+ * @param dc_b  duty cycle phase B [0, 1]
+ * @param dc_c  duty cycle phase C [0, 1]
+ * @param params  the driver parameters
+ */ 
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params);
+
+/** 
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - Stepper driver - 4PWM setting
+ * - hardware specific
+ * 
+ * @param dc_1a  duty cycle phase 1A [0, 1]
+ * @param dc_1b  duty cycle phase 1B [0, 1]
+ * @param dc_2a  duty cycle phase 2A [0, 1]
+ * @param dc_2b  duty cycle phase 2B [0, 1]
+ * @param params  the driver parameters
+ */ 
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params);
+
+
+/** 
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - BLDC driver - 6PWM setting
+ * - hardware specific
+ * 
+ * @param dc_a  duty cycle phase A [0, 1]
+ * @param dc_b  duty cycle phase B [0, 1]
+ * @param dc_c  duty cycle phase C [0, 1]
+ * @param phase_state  pointer to PhaseState[3] array
+ * @param params  the driver parameters
+ * 
+ */ 
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params);
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/atmega/atmega2560_mcu.cpp

@@ -0,0 +1,278 @@
+#include "../../hardware_api.h"
+
+#if defined(__AVR_ATmega2560__) || defined(AVR_ATmega1280)
+
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for Arduino/ATmega2560 or Arduino/ATmega1280")
+#pragma message("")
+
+
+#define _PWM_FREQUENCY 32000
+#define _PWM_FREQUENCY_MAX 32000
+#define _PWM_FREQUENCY_MIN 4000
+
+// set pwm frequency to 32KHz
+void _pinHighFrequency(const int pin, const long frequency){
+  bool high_fq = false;
+  // set 32kHz frequency if requested freq is higher than the middle of the range (14kHz)
+  // else set the 4kHz
+  if(frequency >= 0.5*(_PWM_FREQUENCY_MAX-_PWM_FREQUENCY_MIN))  high_fq=true; 
+  //  High PWM frequency
+  //  https://sites.google.com/site/qeewiki/books/avr-guide/timers-on-the-ATmega2560
+  //  https://forum.arduino.cc/index.php?topic=72092.0
+  if (pin == 13 || pin == 4  ) {
+      TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
+      if(high_fq) TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+      else TCCR0B = ((TCCR0B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+  }
+  else if (pin == 12 || pin == 11 )
+      if(high_fq) TCCR1B = ((TCCR1B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+      else TCCR1B = ((TCCR1B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+  else if (pin == 10 || pin == 9 )
+      if(high_fq) TCCR2B = ((TCCR2B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+      else TCCR2B = ((TCCR2B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+  else if (pin == 5 || pin == 3 || pin == 2)
+      if(high_fq) TCCR3B = ((TCCR3B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+      else TCCR3B = ((TCCR3B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+  else if (pin == 8 || pin == 7 || pin == 6)
+      if(high_fq) TCCR4B = ((TCCR4B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+      else TCCR4B = ((TCCR4B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+  else if (pin == 44 || pin == 45 || pin == 46)
+      if(high_fq) TCCR5B = ((TCCR5B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+      else TCCR5B = ((TCCR5B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware specific
+// supports Arduino/ATmega2560
+void* _configure1PWM(long pwm_frequency,const int pinA) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
+  else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA, pwm_frequency);
+  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 specific
+// supports Arduino/ATmega2560
+void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { 
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
+  else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA, pwm_frequency);
+  _pinHighFrequency(pinB, pwm_frequency);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware specific
+// supports Arduino/ATmega2560
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
+  else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA, pwm_frequency);
+  _pinHighFrequency(pinB, pwm_frequency);
+  _pinHighFrequency(pinC, pwm_frequency);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB, pinC },
+    .pwm_frequency = pwm_frequency
+  };
+  // _syncAllTimers();
+  return params;
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+// - hardware specific
+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 specific
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  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 specific
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware specific
+void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
+  else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pin1A,pwm_frequency);
+  _pinHighFrequency(pin1B,pwm_frequency);
+  _pinHighFrequency(pin2A,pwm_frequency);
+  _pinHighFrequency(pin2B,pwm_frequency);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pin1A, pin1B, pin2A, pin2B },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware specific
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params) {
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a);
+  analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b);
+}
+
+
+// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
+// supports Arduino/ATmega2560
+// https://ww1.microchip.com/downloads/en/devicedoc/atmel-2549-8-bit-avr-microcontroller-atmega640-1280-1281-2560-2561_datasheet.pdf
+// https://docs.arduino.cc/hacking/hardware/PinMapping2560
+int _configureComplementaryPair(const int pinH,const int pinL, long frequency) {
+  bool high_fq = false;
+  // set 32kHz frequency if requested freq is higher than the middle of the range (14kHz)
+  // else set the 4kHz
+  if(frequency >= 0.5*(_PWM_FREQUENCY_MAX-_PWM_FREQUENCY_MIN))  high_fq=true; 
+
+  // configure pin pairs
+  if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){
+    // configure the pwm phase-corrected mode
+    TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 13 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ;
+    else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ;
+    // set frequency
+    if(high_fq) TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+    else TCCR0B = ((TCCR0B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+  }else if( (pinH == 11 && pinL == 12 ) || (pinH == 12 && pinL == 11 ) ){
+    // set frequency
+    if(high_fq) TCCR1B = ((TCCR1B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+    else TCCR1B = ((TCCR1B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+    // configure complementary pwm on low side
+    if(pinH == 11 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ;
+    else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ;
+  }else if((pinH == 10 && pinL == 9 ) || (pinH == 9 && pinL == 10 ) ){
+    // set frequency
+    if(high_fq) TCCR2B = ((TCCR2B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+    else TCCR2B = ((TCCR2B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+    // configure complementary pwm on low side
+    if(pinH == 10 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ;
+    else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ;
+  }else if((pinH == 5 && pinL == 2 ) || (pinH == 2 && pinL == 5 ) ){
+    // set frequency
+    if(high_fq) TCCR3B = ((TCCR3B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+    else TCCR3B = ((TCCR3B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+    // configure complementary pwm on low side
+    if(pinH == 5 ) TCCR3A = 0b10110000 | (TCCR3A & 0b00001111) ;
+    else TCCR3A = 0b11100000 | (TCCR3A & 0b00001111) ;
+  }else if((pinH == 6 && pinL == 7 ) || (pinH == 7 && pinL == 6 ) ){
+    // set frequency
+    if(high_fq) TCCR4B = ((TCCR4B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+    else TCCR4B = ((TCCR4B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+    // configure complementary pwm on low side
+    if(pinH == 6 ) TCCR4A = 0b10110000 | (TCCR4A & 0b00001111) ;
+    else TCCR4A = 0b11100000 | (TCCR4A & 0b00001111) ;
+  }else if((pinH == 46 && pinL == 45 ) || (pinH == 45 && pinL == 46 ) ){
+    // set frequency
+    if(high_fq) TCCR5B = ((TCCR5B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+    else TCCR5B = ((TCCR5B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+    // configure complementary pwm on low side
+    if(pinH == 46 ) TCCR5A = 0b10110000 | (TCCR5A & 0b00001111) ;
+    else TCCR5A = 0b11100000 | (TCCR5A & 0b00001111) ;
+  }else{
+    return -1;
+  }
+  return 0;
+}
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver -  setting
+// - hardware specific
+// supports Arduino/ATmega2560
+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) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
+  else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
+  //  High PWM frequency
+  // - always max 32kHz
+  int ret_flag = 0;
+  ret_flag += _configureComplementaryPair(pinA_h, pinA_l, pwm_frequency);
+  ret_flag += _configureComplementaryPair(pinB_h, pinB_l, pwm_frequency);
+  ret_flag += _configureComplementaryPair(pinC_h, pinC_l, pwm_frequency);
+  if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED;
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l },
+    .pwm_frequency = pwm_frequency,
+    .dead_zone = dead_zone
+  };
+  // _syncAllTimers();
+  return params;
+}
+
+// function setting the
+void _setPwmPair(int pinH, int pinL, float val, int dead_time, PhaseState ps)
+{
+  int pwm_h = _constrain(val-dead_time/2,0,255);
+  int pwm_l = _constrain(val+dead_time/2,0,255);
+  // determine the phase state and set the pwm accordingly
+  // deactivate phases if needed
+  if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_LO)){
+    digitalWrite(pinH, LOW);
+  }else{
+    analogWrite(pinH, pwm_h);
+  }
+  if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_HI)){
+    digitalWrite(pinL, LOW);
+  }else{
+    if(pwm_l == 255 || pwm_l == 0)
+      digitalWrite(pinL, pwm_l ? LOW : HIGH);
+    else
+      analogWrite(pinL, pwm_l);
+  }
+
+}
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+//  - BLDC driver - 6PWM setting
+//  - hardware specific
+// supports Arduino/ATmega328
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params){
+  _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[0]);
+  _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[1]);
+  _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[2]);
+}
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/atmega/atmega328_mcu.cpp

@@ -0,0 +1,255 @@
+#include "../../hardware_api.h"
+
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__)
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for Arduino/ATmega328 ATmega168 ATmega328PB")
+#pragma message("")
+
+#define _PWM_FREQUENCY 32000
+#define _PWM_FREQUENCY_MAX 32000
+#define _PWM_FREQUENCY_MIN 4000
+
+// set pwm frequency to 32KHz
+void _pinHighFrequency(const int pin, const long frequency){
+  bool high_fq = false;
+  // set 32kHz frequency if requested freq is higher than the middle of the range (14kHz)
+  // else set the 4kHz
+  if(frequency >= 0.5*(_PWM_FREQUENCY_MAX-_PWM_FREQUENCY_MIN))  high_fq=true; 
+  //  High PWM frequency
+  //  https://www.arxterra.com/9-atmega328p-timers/
+  if (pin == 5 || pin == 6  ) {
+      TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
+      if(high_fq) TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+      else TCCR0B = ((TCCR0B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+  }else if (pin == 9 || pin == 10 ){
+      if(high_fq) TCCR1B = ((TCCR1B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+      else TCCR1B = ((TCCR1B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+  }else if (pin == 3 || pin == 11){
+      if(high_fq) TCCR2B = ((TCCR2B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+      else TCCR2B = ((TCCR2B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+  }
+}
+
+void _syncAllTimers(){
+  GTCCR = (1<<TSM)|(1<<PSRASY)|(1<<PSRSYNC); // halt all timers 
+  // set all timers to the same value
+  TCNT0 = 0; // set timer0 to 0
+  TCNT1H = 0; // set timer1 high byte to 0
+  TCNT1L = 0; // set timer1 low byte to 0
+  TCNT2 = 0; // set timer2 to 0
+  GTCCR = 0; // release all timers
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware specific
+// supports Arduino/ATmega328
+void* _configure1PWM(long pwm_frequency,const int pinA) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
+  else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA, pwm_frequency);
+  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 specific
+// supports Arduino/ATmega328
+void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { 
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
+  else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA, pwm_frequency);
+  _pinHighFrequency(pinB, pwm_frequency);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware specific
+// supports Arduino/ATmega328
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
+  else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA, pwm_frequency);
+  _pinHighFrequency(pinB, pwm_frequency);
+  _pinHighFrequency(pinC, pwm_frequency);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB, pinC },
+    .pwm_frequency = pwm_frequency
+  };
+  _syncAllTimers();
+  return params;
+}
+
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+// - hardware specific
+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 specific
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  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 specific
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware specific
+// supports Arduino/ATmega328
+void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
+  else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pin1A,pwm_frequency);
+  _pinHighFrequency(pin1B,pwm_frequency);
+  _pinHighFrequency(pin2A,pwm_frequency);
+  _pinHighFrequency(pin2B,pwm_frequency);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pin1A, pin1B, pin2A, pin2B },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware specific
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a);
+  analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b);
+}
+
+
+// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
+int _configureComplementaryPair(const int pinH, const int pinL, long frequency) {
+  bool high_fq = false;
+  // set 32kHz frequency if requested freq is higher than the middle of the range (14kHz)
+  // else set the 4kHz
+  if(frequency >= 0.5*(_PWM_FREQUENCY_MAX-_PWM_FREQUENCY_MIN))  high_fq=true; 
+
+  // configure pins
+  if( (pinH == 5 && pinL == 6 ) || (pinH == 6 && pinL == 5 ) ){
+    // configure the pwm phase-corrected mode
+    TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 6 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ;
+    else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ;
+    // set frequency
+    if(high_fq) TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+    else TCCR0B = ((TCCR0B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+  }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){
+    // set frequency
+    if(high_fq) TCCR1B = ((TCCR1B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+    else TCCR1B = ((TCCR1B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+    // configure complementary pwm on low side
+    if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ;
+    else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ;
+  }else if((pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){
+    // set frequency
+    if(high_fq) TCCR2B = ((TCCR2B & 0b11110000) | 0x01); // set prescaler to 1 - 32kHz
+    else TCCR2B = ((TCCR2B & 0b11110000) | 0x02); // set prescaler to 2 - 4kHz
+    // configure complementary pwm on low side
+    if(pinH == 11 ) TCCR2A = 0b10110000 | (TCCR2A & 0b00001111) ;
+    else TCCR2A = 0b11100000 | (TCCR2A & 0b00001111) ;
+  }else{
+    return -1;
+  }
+  return 0;
+}
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver -  setting
+// - hardware specific
+// supports Arduino/ATmega328
+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) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 32kHz
+  else pwm_frequency = _constrain(pwm_frequency, _PWM_FREQUENCY_MIN, _PWM_FREQUENCY_MAX); // constrain to 4-32kHz max
+  //  High PWM frequency
+  // - always max 32kHz
+  int ret_flag = 0;
+  ret_flag += _configureComplementaryPair(pinA_h, pinA_l, pwm_frequency);
+  ret_flag += _configureComplementaryPair(pinB_h, pinB_l, pwm_frequency);
+  ret_flag += _configureComplementaryPair(pinC_h, pinC_l, pwm_frequency);
+  if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED;
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l },
+    .pwm_frequency = pwm_frequency,
+    .dead_zone = dead_zone
+  };
+  _syncAllTimers();
+  return params;
+}
+
+// function setting the
+void _setPwmPair(int pinH, int pinL, float val, int dead_time, PhaseState ps)
+{
+  int pwm_h = _constrain(val-dead_time/2,0,255);
+  int pwm_l = _constrain(val+dead_time/2,0,255);
+  // determine the phase state and set the pwm accordingly
+  // deactivate phases if needed
+  if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_LO)){
+    digitalWrite(pinH, LOW);
+  }else{
+    analogWrite(pinH, pwm_h);
+  }
+  if((ps == PhaseState::PHASE_OFF) || (ps == PhaseState::PHASE_HI)){
+    digitalWrite(pinL, LOW);
+  }else{
+    if(pwm_l == 255 || pwm_l == 0)
+      digitalWrite(pinL, pwm_l ? LOW : HIGH);
+    else
+      analogWrite(pinL, pwm_l);
+  }
+
+
+}
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+//  - BLDC driver - 6PWM setting
+//  - hardware specific
+// supports Arduino/ATmega328
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params){
+  _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[0]);
+  _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[1]);
+  _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0, phase_state[2]);
+}
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/atmega/atmega32u4_mcu.cpp

@@ -0,0 +1,222 @@
+
+#include "../../hardware_api.h"
+
+#if defined(__AVR_ATmega32U4__)
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for Arduino/ATmega32U4")
+#pragma message("")
+
+// set pwm frequency to 32KHz
+void _pinHighFrequency(const int pin){
+  //  High PWM frequency
+  // reference： http://r6500.blogspot.com/2014/12/fast-pwm-on-arduino-leonardo.html
+  if (pin == 3 || pin == 11 ) {
+    TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
+    TCCR0B = ((TCCR0B & 0b11110000) | 0x01); // set prescaler to 1
+  }
+  else if (pin == 9 || pin == 10 )
+    TCCR1B = ((TCCR1B & 0b11111000) | 0x01);     // set prescaler to 1
+  else if (pin == 5 )
+    TCCR3B = ((TCCR3B & 0b11111000) | 0x01);     // set prescaler to 1
+  else if ( pin == 6 || pin == 13 ) {  // a bit more complicated 10 bit timer
+    // PLL Configuration
+    PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz
+    TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz
+    TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
+    
+    if (pin == 6) TCCR4A = 0x82; // activate channel A - pin 13
+    else if (pin == 13) TCCR4C |= 0x09;  // Activate channel D - pin 6
+  }  
+
+}
+
+
+// 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
+void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA);
+  _pinHighFrequency(pinB);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pinA);
+  _pinHighFrequency(pinB);
+  _pinHighFrequency(pinC);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB, pinC },
+    .pwm_frequency = pwm_frequency
+  };
+  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
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  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
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, int pinA, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void* _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+   //  High PWM frequency
+   // - always max 32kHz
+  _pinHighFrequency(pin1A);
+  _pinHighFrequency(pin1B);
+  _pinHighFrequency(pin2A);
+  _pinHighFrequency(pin2B);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pin1A, pin1B, pin2A, pin2B },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// function setting the pwm duty cycle to the hardware  
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a);
+  analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b);
+}
+
+
+
+
+// function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm
+int _configureComplementaryPair(int pinH, int pinL) {
+  if( (pinH == 3 && pinL == 11 ) || (pinH == 11 && pinL == 3 ) ){
+    // configure the pwm phase-corrected mode
+    TCCR0A = ((TCCR0A & 0b11111100) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 11 ) TCCR0A = 0b10110000 | (TCCR0A & 0b00001111) ;
+    else TCCR0A = 0b11100000 | (TCCR0A & 0b00001111) ;
+    // set prescaler to 1 - 32kHz freq
+    TCCR0B = ((TCCR0B & 0b11110000) | 0x01);
+  }else if( (pinH == 9 && pinL == 10 ) || (pinH == 10 && pinL == 9 ) ){
+    // set prescaler to 1 - 32kHz freq
+    TCCR1B = ((TCCR1B & 0b11111000) | 0x01);
+    // configure complementary pwm on low side
+    if(pinH == 9 ) TCCR1A = 0b10110000 | (TCCR1A & 0b00001111) ;
+    else TCCR1A = 0b11100000 | (TCCR1A & 0b00001111) ;
+  }else if((pinH == 6 && pinL == 13 ) || (pinH == 13 && pinL == 6 ) ){
+    // PLL Configuration
+    PLLFRQ= ((PLLFRQ & 0b11001111) | 0x20); // Use 96MHz / 1.5 = 64MHz
+    TCCR4B = ((TCCR4B & 0b11110000) | 0xB); // configure prescaler to get 64M/2/1024 = 31.25 kHz
+    TCCR4D = ((TCCR4D & 0b11111100) | 0x01); // configure the pwm phase-corrected mode
+
+    // configure complementary pwm on low side
+    if(pinH == 13 ){
+      TCCR4A = 0x82; // activate channel A - pin 13
+      TCCR4C |= 0x0d;  // Activate complementary channel D - pin 6
+    }else {
+      TCCR4C |= 0x09;  // Activate channel D - pin 6
+      TCCR4A = 0xc2; // activate complementary channel A - pin 13
+    }
+  }else{
+    return -1;
+  }
+  return 0;
+}
+
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+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) {
+  //  High PWM frequency
+  // - always max 32kHz
+  int ret_flag = 0;
+  ret_flag += _configureComplementaryPair(pinA_h, pinA_l);
+  ret_flag += _configureComplementaryPair(pinB_h, pinB_l);
+  ret_flag += _configureComplementaryPair(pinC_h, pinC_l);
+  if (ret_flag!=0) return SIMPLEFOC_DRIVER_INIT_FAILED;
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l },
+    .pwm_frequency = pwm_frequency,
+    .dead_zone = dead_zone
+  };
+  return params;
+}
+
+// function setting the 
+void _setPwmPair(int pinH, int pinL, float val, int dead_time)
+{
+  int pwm_h = _constrain(val-dead_time/2,0,255);
+  int pwm_l = _constrain(val+dead_time/2,0,255);
+  
+  analogWrite(pinH, pwm_h);
+  if(pwm_l == 255 || pwm_l == 0)
+    digitalWrite(pinL, pwm_l ? LOW : HIGH);
+  else
+    analogWrite(pinL, pwm_l);
+}
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+//  - BLDC driver - 6PWM setting
+//  - hardware specific
+// supports Arudino/ATmega328 
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params){
+  _setPwmPair(((GenericDriverParams*)params)->pins[0], ((GenericDriverParams*)params)->pins[1], dc_a*255.0, ((GenericDriverParams*)params)->dead_zone*255.0);
+  _setPwmPair(((GenericDriverParams*)params)->pins[2], ((GenericDriverParams*)params)->pins[3], dc_b*255.0, ((GenericDriverParams*)params)->dead_zone*255.0);
+  _setPwmPair(((GenericDriverParams*)params)->pins[4], ((GenericDriverParams*)params)->pins[5], dc_c*255.0, ((GenericDriverParams*)params)->dead_zone*255.0);
+
+  _UNUSED(phase_state);
+}
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/due_mcu.cpp

@@ -0,0 +1,449 @@
+#include "../hardware_api.h" 
+
+#if defined(__arm__) && defined(__SAM3X8E__)
+
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for Arduino/Due")
+#pragma message("")
+
+
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
+#define _PWM_RES_MIN 255 // 50khz
+
+// pwm frequency and max duty cycle
+static unsigned long _pwm_frequency;
+static int _max_pwm_value = 1023;
+
+// array mapping the timer values to the interrupt handlers
+static IRQn_Type irq_type[] = {TC0_IRQn, TC0_IRQn, TC1_IRQn, TC1_IRQn, TC2_IRQn, TC2_IRQn, TC3_IRQn, TC3_IRQn, TC4_IRQn, TC4_IRQn, TC5_IRQn, TC5_IRQn, TC6_IRQn, TC6_IRQn, TC7_IRQn, TC7_IRQn, TC8_IRQn, TC8_IRQn};
+// current counter values 
+static volatile uint32_t pwm_counter_vals[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+
+// variables copied from wiring_analog.cpp for arduino due
+static uint8_t PWMEnabled = 0;
+static uint8_t TCChanEnabled[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
+static const uint32_t channelToChNo[] = { 0, 0, 1, 1, 2, 2, 0, 0, 1, 1, 2, 2, 0, 0, 1, 1, 2, 2 };
+static const uint32_t channelToAB[]   = { 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0 };
+static Tc *channelToTC[] = {
+  TC0, TC0, TC0, TC0, TC0, TC0,
+  TC1, TC1, TC1, TC1, TC1, TC1,
+  TC2, TC2, TC2, TC2, TC2, TC2 };
+static const uint32_t channelToId[] = { 0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8 };
+
+
+// function setting the CMR register
+static void TC_SetCMR_ChannelA(Tc *tc, uint32_t chan, uint32_t v){  tc->TC_CHANNEL[chan].TC_CMR = (tc->TC_CHANNEL[chan].TC_CMR & 0xFFF0FFFF) | v;}
+static void TC_SetCMR_ChannelB(Tc *tc, uint32_t chan, uint32_t v){ tc->TC_CHANNEL[chan].TC_CMR = (tc->TC_CHANNEL[chan].TC_CMR & 0xF0FFFFFF) | v; }
+
+
+// function which starts and syncs the timers 
+// if the pin is the true PWM pin this function does not do anything
+void syncTimers(uint32_t ulPin1,uint32_t ulPin2, uint32_t ulPin3 = -1, uint32_t ulPin4 = -1){
+  uint32_t chNo1,chNo2,chNo3,chNo4;
+  Tc *chTC1 = nullptr,*chTC2 = nullptr,*chTC3 = nullptr,*chTC4 = nullptr;
+
+  // configure timer channel for the first pin if it is a timer pin
+  uint32_t attr = g_APinDescription[ulPin1].ulPinAttribute;
+  if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
+    ETCChannel channel1 = g_APinDescription[ulPin1].ulTCChannel;
+    chNo1 = channelToChNo[channel1];
+    chTC1 = channelToTC[channel1];
+    TCChanEnabled[channelToId[channel1]] = 1;
+  }
+    
+  // configure timer channel for the first pin if it is a timer pin
+  attr = g_APinDescription[ulPin2].ulPinAttribute;
+  if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
+    ETCChannel channel2 = g_APinDescription[ulPin2].ulTCChannel;
+    chNo2 = channelToChNo[channel2];
+    chTC2 = channelToTC[channel2];
+    TCChanEnabled[channelToId[channel2]] = 1;
+  }
+  if(ulPin3 > 0 ){
+    // configure timer channel for the first pin if it is a timer pin
+    attr = g_APinDescription[ulPin3].ulPinAttribute;
+    if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
+      ETCChannel channel3 = g_APinDescription[ulPin3].ulTCChannel;
+      chNo3 = channelToChNo[channel3];
+      chTC3 = channelToTC[channel3];
+      TCChanEnabled[channelToId[channel3]] = 1;
+    }
+  }
+  if(ulPin4  > 0 ){
+    // configure timer channel for the first pin if it is a timer pin
+    attr = g_APinDescription[ulPin4].ulPinAttribute;
+    if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) {
+      ETCChannel channel4 = g_APinDescription[ulPin4].ulTCChannel;
+      chNo4 = channelToChNo[channel4];
+      chTC4 = channelToTC[channel4];
+      TCChanEnabled[channelToId[channel4]] = 1;
+    }
+  }
+  // start timers and make them synced
+  if(chTC1){ 
+    TC_Start(chTC1, chNo1);
+    chTC1->TC_BCR = TC_BCR_SYNC;
+  }
+  if(chTC2){  
+    TC_Start(chTC2, chNo2);
+    chTC2->TC_BCR = TC_BCR_SYNC;
+  }
+  if(chTC3 && ulPin3){ 
+    TC_Start(chTC3, chNo3);
+    chTC3->TC_BCR = TC_BCR_SYNC;
+  }
+  if(chTC4 && ulPin4){ 
+    TC_Start(chTC4, chNo4);
+    chTC4->TC_BCR = TC_BCR_SYNC;
+  }
+}
+
+// function configuring the pwm frequency for given pin
+// possible to supply the pwm pin and the timer pin 
+void initPWM(uint32_t ulPin, uint32_t pwm_freq){
+  // check which pin type
+  uint32_t attr = g_APinDescription[ulPin].ulPinAttribute;
+  if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) { // if pwm pin
+
+    if (!PWMEnabled) {
+      // PWM Startup code
+      pmc_enable_periph_clk(PWM_INTERFACE_ID);
+			// this function does not work too well - I'll rewrite it
+      // PWMC_ConfigureClocks(PWM_FREQUENCY * _max_pwm_value, 0, VARIANT_MCK);
+
+      // finding the divisors an prescalers form FindClockConfiguration function
+      uint32_t divisors[11] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024};
+      uint8_t divisor = 0;
+      uint32_t prescaler;
+
+      /* Find prescaler and divisor values */
+      prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value);
+      while ((prescaler > 255) && (divisor < 11)) {
+          divisor++;
+          prescaler = (VARIANT_MCK / divisors[divisor]) / (pwm_freq*_max_pwm_value);
+      }
+      // update the divisor*prescaler value 
+      prescaler = prescaler | (divisor << 8);
+
+      // now calculate the real resolution timer period necessary (pwm resolution)
+      // pwm_res = bus_freq / (pwm_freq * (prescaler))
+      _max_pwm_value = (double)VARIANT_MCK / (double)pwm_freq / (double)(prescaler);
+      // set the prescaler value
+      PWM->PWM_CLK = prescaler;
+
+      PWMEnabled = 1;
+    }
+
+    uint32_t chan = g_APinDescription[ulPin].ulPWMChannel;
+    if ((g_pinStatus[ulPin] & 0xF) != PIN_STATUS_PWM) {
+      // Setup PWM for this pin
+      PIO_Configure(g_APinDescription[ulPin].pPort,
+          g_APinDescription[ulPin].ulPinType,
+          g_APinDescription[ulPin].ulPin,
+          g_APinDescription[ulPin].ulPinConfiguration);
+      // PWM_CMR_CALG - center align
+      // PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, PWM_CMR_CALG, 0);
+      PWMC_ConfigureChannel(PWM_INTERFACE, chan, PWM_CMR_CPRE_CLKA, 0, 0);
+      PWMC_SetPeriod(PWM_INTERFACE, chan, _max_pwm_value);
+      PWMC_SetDutyCycle(PWM_INTERFACE, chan, 0);
+      PWMC_EnableChannel(PWM_INTERFACE, chan);
+      g_pinStatus[ulPin] = (g_pinStatus[ulPin] & 0xF0) | PIN_STATUS_PWM;
+    }
+    return;
+  }
+
+  if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin
+    // We use MCLK/2 as clock.
+    const uint32_t TC = VARIANT_MCK / 2 / pwm_freq ;
+    // Setup Timer for this pin
+    ETCChannel channel = g_APinDescription[ulPin].ulTCChannel;
+    uint32_t chNo = channelToChNo[channel];
+    uint32_t chA  = channelToAB[channel];
+    Tc *chTC = channelToTC[channel];
+    uint32_t interfaceID = channelToId[channel];
+
+      if (!TCChanEnabled[interfaceID]) {
+        pmc_enable_periph_clk(TC_INTERFACE_ID + interfaceID);
+        TC_Configure(chTC, chNo,
+          TC_CMR_TCCLKS_TIMER_CLOCK1 |
+          TC_CMR_WAVE |         // Waveform mode
+          TC_CMR_WAVSEL_UP_RC | // Counter running up and reset when equals to RC
+          TC_CMR_EEVT_XC0 |     // Set external events from XC0 (this setup TIOB as output)
+          TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR |
+          TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR);
+        TC_SetRC(chTC, chNo, TC);
+      } 
+
+    // disable the counter on start
+    if (chA){
+      TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);
+    }else{
+      TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_SET);
+    }
+    // configure input-ouput structure
+    if ((g_pinStatus[ulPin] & 0xF) != PIN_STATUS_PWM) {
+      PIO_Configure(g_APinDescription[ulPin].pPort,
+          g_APinDescription[ulPin].ulPinType,
+          g_APinDescription[ulPin].ulPin,
+          g_APinDescription[ulPin].ulPinConfiguration);
+      g_pinStatus[ulPin] = (g_pinStatus[ulPin] & 0xF0) | PIN_STATUS_PWM;
+    }
+    // enable interrupts
+    chTC->TC_CHANNEL[chNo].TC_IER = TC_IER_CPAS       // interrupt on RA compare match
+                          | TC_IER_CPBS         // interrupt on RB compare match
+                          | TC_IER_CPCS;        // interrupt on RC compare match
+    chTC->TC_CHANNEL[chNo].TC_IDR = ~TC_IER_CPAS       // interrupt on RA compare match
+                            & ~TC_IER_CPBS         // interrupt on RB compare match
+                            & ~ TC_IER_CPCS;        // interrupt on RC compare match
+    // enable interrupts for this timer
+    NVIC_EnableIRQ(irq_type[channel]);                  
+    return;
+  }
+}
+
+// pwm setting function
+// it sets the duty cycle for pwm pin or timer pin
+void setPwm(uint32_t ulPin, uint32_t ulValue) {
+  // check pin type
+  uint32_t attr = g_APinDescription[ulPin].ulPinAttribute;
+  if ((attr & PIN_ATTR_PWM) == PIN_ATTR_PWM) { // if pwm
+    uint32_t chan = g_APinDescription[ulPin].ulPWMChannel;
+    PWMC_SetDutyCycle(PWM_INTERFACE, chan, ulValue);
+    return;
+  }
+
+  if ((attr & PIN_ATTR_TIMER) == PIN_ATTR_TIMER) { // if timer pin
+    // get the timer variables 
+    ETCChannel channel = g_APinDescription[ulPin].ulTCChannel;
+    Tc *chTC = channelToTC[channel];
+    uint32_t chNo = channelToChNo[channel];
+    if(!ulValue) {
+      // if the value 0 disable counter
+      if (channelToAB[channel])
+        TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_CLEAR);
+      else
+        TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_CLEAR);
+    }else{
+      // if the value not zero
+      // calculate clock
+      const uint32_t TC = VARIANT_MCK / 2 / _pwm_frequency;
+      // Map value to Timer ranges 0..max_duty_cycle => 0..TC
+      // Setup Timer for this pin
+      ulValue = ulValue * TC ;
+      pwm_counter_vals[channel] = ulValue / _max_pwm_value;
+      // enable counter
+      if (channelToAB[channel])
+        TC_SetCMR_ChannelA(chTC, chNo, TC_CMR_ACPA_CLEAR | TC_CMR_ACPC_SET);
+      else
+        TC_SetCMR_ChannelB(chTC, chNo, TC_CMR_BCPB_CLEAR | TC_CMR_BCPC_SET);
+    }
+    
+    return;
+  }
+}
+
+// interrupt handlers for seamless pwm duty-cycle setting
+void TC0_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC0, 0);
+  // update the counters
+  if(pwm_counter_vals[0]) TC_SetRA(TC0, 0, pwm_counter_vals[0]);
+  if(pwm_counter_vals[1]) TC_SetRB(TC0, 0, pwm_counter_vals[1]);
+}
+
+void TC1_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC0, 1);
+  // update the counters
+  if(pwm_counter_vals[2]) TC_SetRA(TC0, 1, pwm_counter_vals[2]);
+  if(pwm_counter_vals[3]) TC_SetRB(TC0, 1, pwm_counter_vals[3]);
+}
+
+void TC2_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC0, 2);
+  // update the counters
+  if(pwm_counter_vals[4]) TC_SetRA(TC0, 2, pwm_counter_vals[4]);
+  if(pwm_counter_vals[5]) TC_SetRB(TC0, 2, pwm_counter_vals[5]);
+}
+void TC3_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC1, 0);
+  // update the counters
+  if(pwm_counter_vals[6]) TC_SetRA(TC1, 0, pwm_counter_vals[6]);
+  if(pwm_counter_vals[7]) TC_SetRB(TC1, 0, pwm_counter_vals[7]);
+}
+
+void TC4_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC1, 1);
+  // update the counters
+  if(pwm_counter_vals[8]) TC_SetRA(TC1, 1, pwm_counter_vals[8]);
+  if(pwm_counter_vals[9]) TC_SetRB(TC1, 1, pwm_counter_vals[9]);
+}
+
+void TC5_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC1, 2);
+  // update the counters
+  if(pwm_counter_vals[10]) TC_SetRA(TC1, 2, pwm_counter_vals[10]);
+  if(pwm_counter_vals[11]) TC_SetRB(TC1, 2, pwm_counter_vals[11]);
+}
+void TC6_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC2, 0);
+  // update the counters
+  if(pwm_counter_vals[12]) TC_SetRA(TC2, 0, pwm_counter_vals[12]);
+  if(pwm_counter_vals[13]) TC_SetRB(TC2, 0, pwm_counter_vals[13]);
+}
+
+void TC7_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC2, 1);
+  // update the counters
+  if(pwm_counter_vals[14]) TC_SetRA(TC2, 1, pwm_counter_vals[14]);
+  if(pwm_counter_vals[15]) TC_SetRB(TC2, 1, pwm_counter_vals[15]);
+}
+
+void TC8_Handler()
+{
+  // read/clear interrupt status
+  TC_GetStatus(TC2, 2);
+  // update the counters
+  if(pwm_counter_vals[16]) TC_SetRA(TC2, 2, pwm_counter_vals[16]);
+  if(pwm_counter_vals[17]) TC_SetRB(TC2, 2, pwm_counter_vals[17]);
+}
+
+
+
+
+
+// implementation of the hardware_api.cpp 
+// ---------------------------------------------------------------------------------------------------------------------------------
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware specific
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  // save the pwm frequency
+  _pwm_frequency = pwm_frequency;
+  // cinfigure pwm pins
+  initPWM(pinA, _pwm_frequency);
+  initPWM(pinB, _pwm_frequency);
+  initPWM(pinC, _pwm_frequency);
+  // sync the timers if possible
+  syncTimers(pinA, pinB, pinC);
+
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB, pinC },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+
+
+
+// Configuring PWM frequency, resolution and alignment
+//- Stepper driver - 2PWM setting
+// - hardware specific
+void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+  if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  // save the pwm frequency
+  _pwm_frequency = pwm_frequency;
+  // cinfigure pwm pins
+  initPWM(pinA, _pwm_frequency);
+  initPWM(pinB, _pwm_frequency);
+  // sync the timers if possible
+  syncTimers(pinA, pinB);
+
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  if(!pwm_frequency || !_isset(pwm_frequency)) pwm_frequency = _PWM_FREQUENCY; // default frequency 50khz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max
+  // save the pwm frequency
+  _pwm_frequency = pwm_frequency;
+  // cinfigure pwm pins
+  initPWM(pinA, _pwm_frequency);
+  initPWM(pinB, _pwm_frequency);
+  initPWM(pinC, _pwm_frequency);
+  initPWM(pinD, _pwm_frequency);
+  // sync the timers if possible
+  syncTimers(pinA, pinB, pinC, pinD);
+
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB, pinC, pinD },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* param){
+  // transform duty cycle from [0,1] to [0,_max_pwm_value]
+  GenericDriverParams* p = (GenericDriverParams*)param;
+  setPwm(p->pins[0], _max_pwm_value*dc_a);
+  setPwm(p->pins[1], _max_pwm_value*dc_b);
+  setPwm(p->pins[2], _max_pwm_value*dc_c);
+}
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* param){
+  // transform duty cycle from [0,1] to [0,_max_pwm_value]
+  GenericDriverParams* p = (GenericDriverParams*)param;
+  setPwm(p->pins[0], _max_pwm_value*dc_1a);
+  setPwm(p->pins[1], _max_pwm_value*dc_1b);
+  setPwm(p->pins[2], _max_pwm_value*dc_2a);
+  setPwm(p->pins[3], _max_pwm_value*dc_2b);
+}
+
+
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+// - Stepper driver - 2PWM setting
+// - hardware specific
+void  _writeDutyCycle2PWM(float dc_a,  float dc_b, void* param){
+  // transform duty cycle from [0,1] to [0,_max_pwm_value]
+  GenericDriverParams* p = (GenericDriverParams*)param;
+  setPwm(p->pins[0], _max_pwm_value*dc_a);
+  setPwm(p->pins[1], _max_pwm_value*dc_b);
+}
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/esp32/esp32_driver_mcpwm.h

@@ -0,0 +1,96 @@
+#ifndef ESP32_DRIVER_MCPWM_H
+#define ESP32_DRIVER_MCPWM_H
+
+#include "../../hardware_api.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC)
+
+
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for ESP32 MCPWM driver")
+#pragma message("")
+
+
+#include "driver/mcpwm.h"
+#include "soc/mcpwm_reg.h"
+#include "soc/mcpwm_struct.h"
+
+// empty motor slot
+#define _EMPTY_SLOT -20
+#define _TAKEN_SLOT -21
+
+// ABI bus frequency - would be better to take it from somewhere
+// but I did nto find a good exposed variable
+#define _MCPWM_FREQ 160e6f
+
+// preferred pwm resolution default
+#define _PWM_RES_DEF 4096
+// min resolution
+#define _PWM_RES_MIN 3000
+// max resolution
+#define _PWM_RES_MAX 8000
+// pwm frequency
+#define _PWM_FREQUENCY 25000 // default
+#define _PWM_FREQUENCY_MAX 50000 // mqx
+
+// structure containing motor slot configuration
+// this library supports up to 4 motors
+typedef struct {
+  int pinA;
+  mcpwm_dev_t* mcpwm_num;
+  mcpwm_unit_t mcpwm_unit;
+  mcpwm_operator_t mcpwm_operator;
+  mcpwm_io_signals_t mcpwm_a;
+  mcpwm_io_signals_t mcpwm_b;
+  mcpwm_io_signals_t mcpwm_c;
+} bldc_3pwm_motor_slots_t;
+
+typedef struct {
+  int pin1A;
+  mcpwm_dev_t* mcpwm_num;
+  mcpwm_unit_t mcpwm_unit;
+  mcpwm_operator_t mcpwm_operator1;
+  mcpwm_operator_t mcpwm_operator2;
+  mcpwm_io_signals_t mcpwm_1a;
+  mcpwm_io_signals_t mcpwm_1b;
+  mcpwm_io_signals_t mcpwm_2a;
+  mcpwm_io_signals_t mcpwm_2b;
+} stepper_4pwm_motor_slots_t;
+
+typedef struct {
+  int pin1pwm;
+  mcpwm_dev_t* mcpwm_num;
+  mcpwm_unit_t mcpwm_unit;
+  mcpwm_operator_t mcpwm_operator;
+  mcpwm_io_signals_t mcpwm_a;
+  mcpwm_io_signals_t mcpwm_b;
+} stepper_2pwm_motor_slots_t;
+
+typedef struct {
+  int pinAH;
+  mcpwm_dev_t* mcpwm_num;
+  mcpwm_unit_t mcpwm_unit;
+  mcpwm_operator_t mcpwm_operator1;
+  mcpwm_operator_t mcpwm_operator2;
+  mcpwm_io_signals_t mcpwm_ah;
+  mcpwm_io_signals_t mcpwm_bh;
+  mcpwm_io_signals_t mcpwm_ch;
+  mcpwm_io_signals_t mcpwm_al;
+  mcpwm_io_signals_t mcpwm_bl;
+  mcpwm_io_signals_t mcpwm_cl;
+} bldc_6pwm_motor_slots_t;
+
+
+typedef struct ESP32MCPWMDriverParams {
+  long pwm_frequency;
+  mcpwm_dev_t* mcpwm_dev;
+  mcpwm_unit_t mcpwm_unit;
+  mcpwm_operator_t mcpwm_operator1;
+  mcpwm_operator_t mcpwm_operator2;
+  float deadtime;
+} ESP32MCPWMDriverParams;
+
+
+#endif
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/esp32/esp32_ledc_mcu.cpp

@@ -0,0 +1,185 @@
+#include "../../hardware_api.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && ( !defined(SOC_MCPWM_SUPPORTED) || defined(SIMPLEFOC_ESP32_USELEDC) )
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for ESP32 LEDC driver")
+#pragma message("")
+
+#include "driver/ledc.h"
+
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 38000 // 38khz max to be able to have 10 bit pwm resolution
+#define _PWM_RES_BIT 10  // 10 bir resolution
+#define _PWM_RES 1023 // 2^10-1 = 1023-1
+
+
+// empty motor slot
+#define _EMPTY_SLOT -20
+#define _TAKEN_SLOT -21
+
+// figure out how many ledc channels are avaible
+// esp32   - 2x8=16
+// esp32s2 - 8
+// esp32c3 - 6
+#include "soc/soc_caps.h"
+#ifdef SOC_LEDC_SUPPORT_HS_MODE
+#define LEDC_CHANNELS           (SOC_LEDC_CHANNEL_NUM<<1)
+#else
+#define LEDC_CHANNELS           (SOC_LEDC_CHANNEL_NUM)
+#endif
+
+
+// current channel stack index
+// support for multiple motors
+// esp32 has 16 channels
+// esp32s2 has 8 channels
+// esp32c3 has 6 channels
+int channel_index = 0;
+
+
+
+
+typedef struct ESP32LEDCDriverParams {
+  int channels[6];
+  long pwm_frequency;
+} ESP32LEDCDriverParams;
+
+
+
+
+
+//  configure High PWM frequency
+void _setHighFrequency(const long freq, const int pin, const int channel){
+  ledcSetup(channel, freq, _PWM_RES_BIT );
+  ledcAttachPin(pin, 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
+
+  // check if enough channels available
+  if ( channel_index + 2 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  int ch1 = channel_index++;
+  int ch2 = channel_index++;
+  _setHighFrequency(pwm_frequency, pinA, ch1);
+  _setHighFrequency(pwm_frequency, pinB, ch2);
+
+  ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams {
+    .channels = { ch1, ch2 },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+
+
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  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 + 3 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  int ch1 = channel_index++;
+  int ch2 = channel_index++;
+  int ch3 = channel_index++;
+  _setHighFrequency(pwm_frequency, pinA, ch1);
+  _setHighFrequency(pwm_frequency, pinB, ch2);
+  _setHighFrequency(pwm_frequency, pinC, ch3);
+
+  ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams {
+    .channels = { ch1, ch2, ch3 },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+
+
+void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  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 + 4 >= LEDC_CHANNELS ) return SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  int ch1 = channel_index++;
+  int ch2 = channel_index++;
+  int ch3 = channel_index++;
+  int ch4 = channel_index++;
+  _setHighFrequency(pwm_frequency, pinA, ch1);
+  _setHighFrequency(pwm_frequency, pinB, ch2);
+  _setHighFrequency(pwm_frequency, pinC, ch3);
+  _setHighFrequency(pwm_frequency, pinD, ch4);
+
+  ESP32LEDCDriverParams* params = new ESP32LEDCDriverParams {
+    .channels = { ch1, ch2, ch3, ch4 },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+
+
+
+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));
+}
+
+
+
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, 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));
+  ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_c, 0, _PWM_RES));
+}
+
+
+
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+  ledcWrite(((ESP32LEDCDriverParams*)params)->channels[0], _constrain(_PWM_RES*dc_1a, 0, _PWM_RES));
+  ledcWrite(((ESP32LEDCDriverParams*)params)->channels[1], _constrain(_PWM_RES*dc_1b, 0, _PWM_RES));
+  ledcWrite(((ESP32LEDCDriverParams*)params)->channels[2], _constrain(_PWM_RES*dc_2a, 0, _PWM_RES));
+  ledcWrite(((ESP32LEDCDriverParams*)params)->channels[3], _constrain(_PWM_RES*dc_2b, 0, _PWM_RES));
+}
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/esp32/esp32_mcu.cpp

@@ -0,0 +1,431 @@
+#include "esp32_driver_mcpwm.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(SIMPLEFOC_ESP32_USELEDC)
+
+#ifndef SIMPLEFOC_ESP32_HW_DEADTIME
+  #define SIMPLEFOC_ESP32_HW_DEADTIME true // TODO: Change to false when sw-deadtime & phase_state is approved ready for general use.
+#endif
+
+// define bldc motor slots array
+bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] =  {
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}  // 4th motor will be MCPWM1 channel B
+  };
+
+// define stepper motor slots array
+bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] =  {
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0
+  };
+
+// define 4pwm stepper motor slots array
+stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] =  {
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1
+  };
+
+// define 2pwm stepper motor slots array
+stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] =  {
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A
+  {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A
+  {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_B, MCPWM0B, MCPWM1B}  // 4th motor will be MCPWM1 channel B
+  };
+
+
+// configuring high frequency pwm timer
+// a lot of help from this post from Paul Gauld
+// https://hackaday.io/project/169905-esp-32-bldc-robot-actuator-controller
+// a short tutorial for v2.0.1+
+// https://kzhead.info/sun/q6uFktWgkYeMeZ8/esp32-arduino.html 
+//
+void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num,  mcpwm_unit_t mcpwm_unit, float dead_zone = NOT_SET){
+
+  mcpwm_config_t pwm_config;
+  pwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // Up-down counter (triangle wave)
+  pwm_config.duty_mode = (_isset(dead_zone) || SIMPLEFOC_PWM_ACTIVE_HIGH == true) ? MCPWM_DUTY_MODE_0 : MCPWM_DUTY_MODE_1; // Normally Active HIGH (MCPWM_DUTY_MODE_0)
+  pwm_config.frequency  = 2*pwm_frequency; // set the desired freq - just a placeholder for now https://github.com/simplefoc/Arduino-FOC/issues/76
+  mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config);    //Configure PWM0A & PWM0B with above settings
+  mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config);    //Configure PWM1A & PWM1B with above settings
+  mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config);    //Configure PWM2A & PWM2B with above settings
+
+  if (_isset(dead_zone)){
+    // dead zone is configured
+
+    // When using hardware deadtime, setting the phase_state parameter is not supported.
+    #if SIMPLEFOC_ESP32_HW_DEADTIME == true
+      float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone;
+      mcpwm_deadtime_type_t pwm_mode;
+      if      ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true)  && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE;}  // Normal, noninverting driver
+      else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true)  && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_HIGH_MODE;}             // Inverted lowside driver
+      else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true)) {pwm_mode = MCPWM_ACTIVE_LOW_MODE;}              // Inverted highside driver
+      else if ((SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) && (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false)){pwm_mode = MCPWM_ACTIVE_LOW_COMPLIMENT_MODE;}   // Inverted low- & highside driver. Caution: This may short the FETs on reset of the ESP32, as both pins get pulled low!
+      mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, pwm_mode, dead_time/2.0, dead_time/2.0);
+      mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, pwm_mode, dead_time/2.0, dead_time/2.0);
+      mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, pwm_mode, dead_time/2.0, dead_time/2.0);
+    #else // Software deadtime
+      for (int i = 0; i < 3; i++){
+        if      (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_0);}  // Normal, noninverted highside
+        else if (SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_A, MCPWM_DUTY_MODE_1);} // Inverted highside driver
+        
+        if      (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == true) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_1);}  // Normal, complementary lowside
+        else if (SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH == false) {mcpwm_set_duty_type(mcpwm_unit, (mcpwm_timer_t) i, MCPWM_GEN_B, MCPWM_DUTY_MODE_0);} // Inverted lowside driver
+      }
+    #endif
+
+  }
+  _delay(100);
+
+  mcpwm_stop(mcpwm_unit, MCPWM_TIMER_0);
+  mcpwm_stop(mcpwm_unit, MCPWM_TIMER_1);
+  mcpwm_stop(mcpwm_unit, MCPWM_TIMER_2);
+
+  // manual configuration due to the lack of config flexibility in mcpwm_init()
+  mcpwm_num->clk_cfg.clk_prescale = 0;
+  // calculate prescaler and period
+  // step 1: calculate the prescaler using the default pwm resolution
+  // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1
+  int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (double)pwm_frequency) - 1;
+  // constrain prescaler
+  prescaler = _constrain(prescaler, 0, 128);
+  // now calculate the real resolution timer period necessary (pwm resolution)
+  // pwm_res = bus_freq / (pwm_freq * (prescaler + 1))
+  int resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(prescaler + 1);
+  // if pwm resolution too low lower the prescaler
+  if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 )
+    resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(--prescaler + 1);
+  resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX);
+
+  // set prescaler
+  mcpwm_num->timer[0].timer_cfg0.timer_prescale = prescaler;
+  mcpwm_num->timer[1].timer_cfg0.timer_prescale = prescaler;
+  mcpwm_num->timer[2].timer_cfg0.timer_prescale = prescaler;
+  _delay(1);
+  //set period
+  mcpwm_num->timer[0].timer_cfg0.timer_period = resolution_corrected;
+  mcpwm_num->timer[1].timer_cfg0.timer_period = resolution_corrected;
+  mcpwm_num->timer[2].timer_cfg0.timer_period = resolution_corrected;
+  _delay(1);
+  mcpwm_num->timer[0].timer_cfg0.timer_period_upmethod = 0;
+  mcpwm_num->timer[1].timer_cfg0.timer_period_upmethod = 0;
+  mcpwm_num->timer[2].timer_cfg0.timer_period_upmethod = 0;
+  _delay(1);
+  // _delay(1);
+  //restart the timers
+  mcpwm_start(mcpwm_unit, MCPWM_TIMER_0);
+  mcpwm_start(mcpwm_unit, MCPWM_TIMER_1);
+  mcpwm_start(mcpwm_unit, MCPWM_TIMER_2);
+  _delay(1);
+
+  mcpwm_sync_config_t sync_conf = {
+    .sync_sig = MCPWM_SELECT_TIMER0_SYNC,
+    .timer_val = 0,
+    .count_direction = MCPWM_TIMER_DIRECTION_UP
+  };
+  mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_0, &sync_conf);
+  mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_1, &sync_conf);
+  mcpwm_sync_configure(mcpwm_unit, MCPWM_TIMER_2, &sync_conf);
+
+  // Enable sync event for all timers to be the TEZ of timer0
+  mcpwm_set_timer_sync_output(mcpwm_unit, MCPWM_TIMER_0, MCPWM_SWSYNC_SOURCE_TEZ);
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+// supports Arudino/ATmega328, STM32 and ESP32
+void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max
+
+  stepper_2pwm_motor_slots_t m_slot = {};
+
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters
+  int slot_num;
+  for(slot_num = 0; slot_num < 4; slot_num++){
+    if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = pinA;
+      m_slot = esp32_stepper_2pwm_motor_slots[slot_num];
+      break;
+    }
+  }
+
+  // disable all the slots with the same MCPWM
+  // disable 3pwm bldc motor which would go in the same slot
+  esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
+  if( slot_num < 2 ){
+    // slot 0 of the 4pwm stepper
+    esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
+  }else{
+    // slot 1 of the stepper
+    esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
+    // slot 1 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
+  }
+  // configure pins
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB);
+
+  // configure the timer
+  _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num,  m_slot.mcpwm_unit);
+
+  ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams {
+    .pwm_frequency = pwm_frequency,
+    .mcpwm_dev = m_slot.mcpwm_num,
+    .mcpwm_unit = m_slot.mcpwm_unit,
+    .mcpwm_operator1 = m_slot.mcpwm_operator
+  };
+  return params;
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+// supports Arudino/ATmega328, STM32 and ESP32
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max
+
+  bldc_3pwm_motor_slots_t m_slot = {};
+
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters
+  int slot_num;
+  for(slot_num = 0; slot_num < 4; slot_num++){
+    if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      esp32_bldc_3pwm_motor_slots[slot_num].pinA = pinA;
+      m_slot = esp32_bldc_3pwm_motor_slots[slot_num];
+      break;
+    }
+  }
+  // disable all the slots with the same MCPWM
+  // disable 2pwm steppr motor which would go in the same slot
+  esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT;
+  if( slot_num < 2 ){
+    // slot 0 of the 4pwm stepper
+    esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
+  }else{
+    // slot 1 of the stepper
+    esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
+    // slot 1 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
+  }
+  // configure pins
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_a, pinA);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_b, pinB);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_c, pinC);
+
+  // configure the timer
+  _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num,  m_slot.mcpwm_unit);
+
+  ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams {
+    .pwm_frequency = pwm_frequency,
+    .mcpwm_dev = m_slot.mcpwm_num,
+    .mcpwm_unit = m_slot.mcpwm_unit,
+    .mcpwm_operator1 = m_slot.mcpwm_operator
+  };
+  return params;
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max
+  stepper_4pwm_motor_slots_t m_slot = {};
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters
+  int slot_num;
+  for(slot_num = 0; slot_num < 2; slot_num++){
+    if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      esp32_stepper_4pwm_motor_slots[slot_num].pin1A = pinA;
+      m_slot = esp32_stepper_4pwm_motor_slots[slot_num];
+      break;
+    }
+  }
+  // disable all the slots with the same MCPWM
+  if( slot_num == 0 ){
+    // slots 0 and 1 of the 3pwm bldc
+    esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
+    esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
+    // slots 0 and 1 of the 2pwm stepper taken
+    esp32_stepper_2pwm_motor_slots[0].pin1pwm = _TAKEN_SLOT;
+    esp32_stepper_2pwm_motor_slots[1].pin1pwm = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
+  }else{
+    // slots 2 and 3 of the 3pwm bldc
+    esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
+    esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
+    // slots 2 and 3 of the 2pwm stepper taken
+    esp32_stepper_2pwm_motor_slots[2].pin1pwm = _TAKEN_SLOT;
+    esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT;
+    // slot 1 of the 6pwm bldc
+    esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
+  }
+
+  // configure pins
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1b, pinB);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2a, pinC);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_2b, pinD);
+
+  // configure the timer
+  _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num,  m_slot.mcpwm_unit);
+
+  ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams {
+    .pwm_frequency = pwm_frequency,
+    .mcpwm_dev = m_slot.mcpwm_num,
+    .mcpwm_unit = m_slot.mcpwm_unit,
+    .mcpwm_operator1 = m_slot.mcpwm_operator1,
+    .mcpwm_operator2 = m_slot.mcpwm_operator2
+  };
+  return params;
+}
+
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+//  ESP32 uses MCPWM
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params){
+  // se the PWM on the slot timers
+  // transform duty cycle from [0,1] to [0,100]
+  mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0);
+  mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0);
+}
+
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+//  ESP32 uses MCPWM
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params){
+  // se the PWM on the slot timers
+  // transform duty cycle from [0,1] to [0,100]
+  mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_a*100.0);
+  mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_b*100.0);
+  mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_c*100.0);
+}
+
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+//  ESP32 uses MCPWM
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+  // se the PWM on the slot timers
+  // transform duty cycle from [0,1] to [0,100]
+  mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1a*100.0);
+  mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator1, dc_1b*100.0);
+  mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2a*100.0);
+  mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, ((ESP32MCPWMDriverParams*)params)->mcpwm_operator2, dc_2b*100.0);
+}
+
+
+
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+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){
+
+  if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency
+  else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency
+  bldc_6pwm_motor_slots_t m_slot = {};
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters
+  int slot_num;
+  for(slot_num = 0; slot_num < 2; slot_num++){
+    if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      esp32_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h;
+      m_slot = esp32_bldc_6pwm_motor_slots[slot_num];
+      break;
+    }
+  }
+  // if no slots available
+  if(slot_num >= 2) return SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  // disable all the slots with the same MCPWM
+  if( slot_num == 0 ){
+    // slots 0 and 1 of the 3pwm bldc
+    esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
+    esp32_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    esp32_stepper_4pwm_motor_slots[0].pin1A = _TAKEN_SLOT;
+  }else{
+    // slots 2 and 3 of the 3pwm bldc
+    esp32_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
+    esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
+    // slot 1 of the 6pwm bldc
+    esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT;
+  }
+
+  // configure pins
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_al, pinA_l);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bh, pinB_h);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_bl, pinB_l);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ch, pinC_h);
+  mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_cl, pinC_l);
+
+  // configure the timer
+  _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num,  m_slot.mcpwm_unit, dead_zone);
+  // return
+  ESP32MCPWMDriverParams* params = new ESP32MCPWMDriverParams {
+    .pwm_frequency = pwm_frequency,
+    .mcpwm_dev = m_slot.mcpwm_num,
+    .mcpwm_unit = m_slot.mcpwm_unit,
+    .mcpwm_operator1 = m_slot.mcpwm_operator1,
+    .mcpwm_operator2 = m_slot.mcpwm_operator2,
+    .deadtime = _isset(dead_zone) ? dead_zone : 0
+  };
+  return params;
+}
+
+
+
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+// - BLDC driver - 6PWM setting
+// - hardware specific
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params){
+      // set the PWM on the slot timers
+      // transform duty cycle from [0,1] to [0,100.0]
+      #if SIMPLEFOC_ESP32_HW_DEADTIME == true
+        // Hardware deadtime does deadtime insertion internally
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, dc_a*100.0f);
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, dc_a*100.0f);
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, dc_b*100.0f);
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, dc_b*100.0f);
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, dc_c*100.0f);
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, dc_c*100.0f);
+        _UNUSED(phase_state);
+      #else                           
+        float deadtime = 0.5f*((ESP32MCPWMDriverParams*)params)->deadtime;
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_A, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_HI) ? _constrain(dc_a-deadtime, 0.0f, 1.0f) * 100.0f : 0);
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_0, MCPWM_OPR_B, (phase_state[0] == PHASE_ON || phase_state[0] == PHASE_LO) ? _constrain(dc_a+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f);
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_A, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_HI) ? _constrain(dc_b-deadtime, 0.0f, 1.0f) * 100.0f : 0);
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_1, MCPWM_OPR_B, (phase_state[1] == PHASE_ON || phase_state[1] == PHASE_LO) ? _constrain(dc_b+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f);
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_A, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_HI) ? _constrain(dc_c-deadtime, 0.0f, 1.0f) * 100.0f : 0);
+        mcpwm_set_duty(((ESP32MCPWMDriverParams*)params)->mcpwm_unit, MCPWM_TIMER_2, MCPWM_OPR_B, (phase_state[2] == PHASE_ON || phase_state[2] == PHASE_LO) ? _constrain(dc_c+deadtime, 0.0f, 1.0f) * 100.0f : 100.0f);
+      #endif
+}
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/esp8266_mcu.cpp

@@ -0,0 +1,121 @@
+#include "../hardware_api.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP8266)
+
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for ESP8266")
+#pragma message("")
+
+
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
+//  configure High PWM frequency
+void _setHighFrequency(const long freq, const int pin){
+  analogWrite(pin, 0);
+  analogWriteFreq(freq);
+}
+
+
+// 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
+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
+  _setHighFrequency(pwm_frequency, pinA);
+  _setHighFrequency(pwm_frequency, pinB);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  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);
+  _setHighFrequency(pwm_frequency, pinB);
+  _setHighFrequency(pwm_frequency, pinC);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB, pinC },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  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);
+  _setHighFrequency(pwm_frequency, pinB);
+  _setHighFrequency(pwm_frequency, pinC);
+  _setHighFrequency(pwm_frequency, pinD);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB, pinC, pinD },
+    .pwm_frequency = pwm_frequency
+  };
+  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
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  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
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a);
+  analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b);
+}
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/generic_mcu.cpp

@@ -0,0 +1,125 @@
+
+#include "../hardware_api.h"
+
+// if the mcu doen't have defiend analogWrite
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && !defined(analogWrite)
+  __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
+// in generic case dont do anything
+__attribute__((weak)) void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+// in generic case dont do anything
+__attribute__((weak)) void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB, pinC },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+// in generic case dont do anything
+__attribute__((weak)) void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pin1A, pin1B, pin2A, pin2B },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+__attribute__((weak)) 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){
+  _UNUSED(pwm_frequency);
+  _UNUSED(dead_zone);
+  _UNUSED(pinA_h);
+  _UNUSED(pinA_l);
+  _UNUSED(pinB_h);
+  _UNUSED(pinB_l);
+  _UNUSED(pinC_h);
+  _UNUSED(pinC_l);
+
+  return SIMPLEFOC_DRIVER_INIT_FAILED;
+}
+
+
+// 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
+__attribute__((weak)) void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  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
+__attribute__((weak)) void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+__attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a);
+  analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b);
+}
+
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+// - BLDC driver - 6PWM setting
+// - hardware specific
+__attribute__((weak)) void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params){
+  _UNUSED(dc_a);
+  _UNUSED(dc_b);
+  _UNUSED(dc_c);
+  _UNUSED(phase_state);
+  _UNUSED(params);
+}

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/nrf52_mcu.cpp

@@ -0,0 +1,397 @@
+
+#include "../hardware_api.h"
+
+#if defined(NRF52_SERIES)
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for NRF52")
+#pragma message("")
+
+
+#define PWM_CLK (16000000)
+#define PWM_FREQ (40000)
+#define PWM_RESOLUTION (PWM_CLK/PWM_FREQ)
+#define PWM_MAX_FREQ (62500)
+#define DEAD_ZONE (250) // in ns
+#define DEAD_TIME (DEAD_ZONE  / (PWM_RESOLUTION * 0.25 * 62.5)) // 62.5ns resolution of PWM
+
+#ifdef NRF_PWM3
+#define PWM_COUNT 4
+#else
+#define PWM_COUNT 3
+#endif
+
+// empty motor slot 
+#define _EMPTY_SLOT (-0xAA)
+#define _TAKEN_SLOT (-0x55)
+
+int pwm_range;
+
+
+static NRF_PWM_Type* pwms[PWM_COUNT] = {
+  NRF_PWM0,
+  NRF_PWM1,
+  NRF_PWM2,
+  #ifdef  NRF_PWM3
+  NRF_PWM3
+  #endif
+};
+
+typedef struct {
+  int pinA;
+  NRF_PWM_Type* mcpwm;
+  uint16_t mcpwm_channel_sequence[4];
+} bldc_3pwm_motor_slots_t;
+
+typedef struct {
+  int pin1A;
+  NRF_PWM_Type* mcpwm;
+  uint16_t mcpwm_channel_sequence[4];
+} stepper_motor_slots_t;
+
+typedef struct {
+  int pinAH;
+  NRF_PWM_Type* mcpwm1;
+  NRF_PWM_Type* mcpwm2;
+  uint16_t mcpwm_channel_sequence[8];
+} bldc_6pwm_motor_slots_t;
+
+// define bldc motor slots array
+bldc_3pwm_motor_slots_t nrf52_bldc_3pwm_motor_slots[4] =  { 
+  {_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be PWM0 
+  {_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 2nd motor will be PWM1 
+  {_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 3rd motor will be PWM2 
+  {_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 4th motor will be PWM3 
+  };
+
+// define stepper motor slots array
+stepper_motor_slots_t nrf52_stepper_motor_slots[4] =  { 
+  {_EMPTY_SLOT, pwms[0], {0,0,0,0}},// 1st motor will be on PWM0
+  {_EMPTY_SLOT, pwms[1], {0,0,0,0}},// 1st motor will be on PWM1
+  {_EMPTY_SLOT, pwms[2], {0,0,0,0}},// 1st motor will be on PWM2
+  {_EMPTY_SLOT, pwms[3], {0,0,0,0}} // 1st motor will be on PWM3
+  };  
+
+// define BLDC motor slots array
+bldc_6pwm_motor_slots_t nrf52_bldc_6pwm_motor_slots[2] =  { 
+  {_EMPTY_SLOT, pwms[0], pwms[1], {0,0,0,0,0,0,0,0}},// 1st motor will be on PWM0 & PWM1
+  {_EMPTY_SLOT, pwms[2], pwms[3], {0,0,0,0,0,0,0,0}} // 2nd motor will be on PWM1 & PWM2
+  };
+
+
+
+typedef struct NRF52DriverParams {
+  union {
+    bldc_3pwm_motor_slots_t* slot3pwm;
+    bldc_6pwm_motor_slots_t* slot6pwm;
+    stepper_motor_slots_t* slotstep;
+  } slot;
+  long pwm_frequency;
+  float dead_time;
+} NRF52DriverParams;
+
+
+
+
+// configuring high frequency pwm timer
+void _configureHwPwm(NRF_PWM_Type* mcpwm1,  NRF_PWM_Type* mcpwm2){
+
+  mcpwm1->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos);
+  mcpwm1->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos);
+  mcpwm1->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos); 
+  mcpwm1->COUNTERTOP = pwm_range; //pwm freq.
+  mcpwm1->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos);
+  mcpwm1->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos);
+  mcpwm1->SEQ[0].REFRESH  = 0;
+  mcpwm1->SEQ[0].ENDDELAY = 0;
+
+  if(mcpwm1 != mcpwm2){
+    mcpwm2->ENABLE = (PWM_ENABLE_ENABLE_Enabled << PWM_ENABLE_ENABLE_Pos);
+    mcpwm2->PRESCALER = (PWM_PRESCALER_PRESCALER_DIV_1 << PWM_PRESCALER_PRESCALER_Pos);
+    mcpwm2->MODE = (PWM_MODE_UPDOWN_UpAndDown << PWM_MODE_UPDOWN_Pos); 
+    mcpwm2->COUNTERTOP = pwm_range; //pwm freq.
+    mcpwm2->LOOP = (PWM_LOOP_CNT_Disabled << PWM_LOOP_CNT_Pos);
+    mcpwm2->DECODER = ((uint32_t)PWM_DECODER_LOAD_Individual << PWM_DECODER_LOAD_Pos) | ((uint32_t)PWM_DECODER_MODE_RefreshCount << PWM_DECODER_MODE_Pos);
+    mcpwm2->SEQ[0].REFRESH  = 0;
+    mcpwm2->SEQ[0].ENDDELAY = 0;
+  }else{
+    mcpwm1->MODE = (PWM_MODE_UPDOWN_Up << PWM_MODE_UPDOWN_Pos);
+  }
+}
+
+
+
+// can we support it using the generic driver on this MCU? Commented out to fall back to generic driver for 2-pwm
+// void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+//   return SIMPLEFOC_DRIVER_INIT_FAILED; // not supported
+// }
+
+
+
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+
+  if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800
+  else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256  
+
+  pwm_range = (PWM_CLK / pwm_frequency);
+  
+  int pA = digitalPinToPinName(pinA); //g_ADigitalPinMap[pinA];
+  int pB = digitalPinToPinName(pinB); //g_ADigitalPinMap[pinB];
+  int pC = digitalPinToPinName(pinC); //g_ADigitalPinMap[pinC];
+
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters 
+  int slot_num;
+  for(slot_num = 0; slot_num < 4; slot_num++){
+    if(nrf52_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      nrf52_bldc_3pwm_motor_slots[slot_num].pinA = pinA;
+      break;
+    }
+  }
+  // if no slots available
+  if(slot_num >= 4) return SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  // disable all the slots with the same MCPWM 
+  if(slot_num < 2){
+    // slot 0 of the stepper
+    nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
+    //NRF_PPI->CHEN &= ~1UL;
+  }else{
+    // slot 1 of the stepper
+    nrf52_stepper_motor_slots[slot_num].pin1A = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
+    //NRF_PPI->CHEN &= ~2UL;
+  }
+
+  // configure pwm outputs
+  
+  nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA;
+  nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB;
+  nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC;
+   
+  nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm_channel_sequence[0];
+  nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4;
+
+  // configure the pwm
+  _configureHwPwm(nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm, nrf52_bldc_3pwm_motor_slots[slot_num].mcpwm);
+
+  NRF52DriverParams* params = new NRF52DriverParams();
+  params->slot.slot3pwm = &(nrf52_bldc_3pwm_motor_slots[slot_num]);
+  params->pwm_frequency = pwm_frequency;
+  return params;
+}
+
+
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD) {
+
+  if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz for a resolution of 800
+  else pwm_frequency = _constrain(pwm_frequency, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max for a resolution of 256
+
+  pwm_range = (PWM_CLK / pwm_frequency);
+  
+  int pA = digitalPinToPinName(pinA); //g_ADigitalPinMap[pinA];
+  int pB = digitalPinToPinName(pinB); //g_ADigitalPinMap[pinB];
+  int pC = digitalPinToPinName(pinC); //g_ADigitalPinMap[pinC];
+  int pD = digitalPinToPinName(pinD); //g_ADigitalPinMap[pinD];
+  
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters 
+  int slot_num;
+  for(slot_num = 0; slot_num < 4; slot_num++){
+    if(nrf52_stepper_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      nrf52_stepper_motor_slots[slot_num].pin1A = pinA;
+      break;
+    }
+  }
+  // if no slots available
+  if (slot_num >= 4) return SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  // disable all the slots with the same MCPWM 
+  if (slot_num < 2){
+    // slots 0 and 1 of the 3pwm bldc
+    nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
+    // slot 0 of the 6pwm bldc
+    nrf52_bldc_6pwm_motor_slots[0].pinAH = _TAKEN_SLOT;
+    //NRF_PPI->CHEN &= ~1UL;
+  }else{
+    // slots 2 and 3 of the 3pwm bldc
+    nrf52_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT;
+    // slot 1 of the 6pwm bldc
+    nrf52_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT;
+    //NRF_PPI->CHEN &= ~2UL;
+  } 
+
+  // configure pwm outputs
+  
+  nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[0] = pA;
+  nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[1] = pB;
+  nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[2] = pC;
+  nrf52_stepper_motor_slots[slot_num].mcpwm->PSEL.OUT[3] = pD;
+
+  nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].PTR = (uint32_t)&nrf52_stepper_motor_slots[slot_num].mcpwm_channel_sequence[0];
+  nrf52_stepper_motor_slots[slot_num].mcpwm->SEQ[0].CNT = 4;
+
+  // configure the pwm
+  _configureHwPwm(nrf52_stepper_motor_slots[slot_num].mcpwm, nrf52_stepper_motor_slots[slot_num].mcpwm);
+
+  NRF52DriverParams* params = new NRF52DriverParams();
+  params->slot.slotstep = &(nrf52_stepper_motor_slots[slot_num]);
+  params->pwm_frequency = pwm_frequency;
+  return params;  
+}
+
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params){
+  // transform duty cycle from [0,1] to [0,range]
+  bldc_3pwm_motor_slots_t* p = ((NRF52DriverParams*)params)->slot.slot3pwm;
+  p->mcpwm_channel_sequence[0] = (int)(dc_a * pwm_range) | 0x8000;
+  p->mcpwm_channel_sequence[1] = (int)(dc_b * pwm_range) | 0x8000;
+  p->mcpwm_channel_sequence[2] = (int)(dc_c * pwm_range) | 0x8000;
+
+  p->mcpwm->TASKS_SEQSTART[0] = 1;
+}
+
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+
+  stepper_motor_slots_t* p = ((NRF52DriverParams*)params)->slot.slotstep;
+  p->mcpwm_channel_sequence[0] = (int)(dc_1a * pwm_range) | 0x8000;
+  p->mcpwm_channel_sequence[1] = (int)(dc_1b * pwm_range) | 0x8000;
+  p->mcpwm_channel_sequence[2] = (int)(dc_2a * pwm_range) | 0x8000;
+  p->mcpwm_channel_sequence[3] = (int)(dc_2b * pwm_range) | 0x8000;
+
+  p->mcpwm->TASKS_SEQSTART[0] = 1;
+}
+
+/* Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+*/
+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){
+  
+  if( !pwm_frequency || pwm_frequency == NOT_SET) pwm_frequency = PWM_FREQ; // default frequency 20khz - centered pwm has twice lower frequency for a resolution of 400
+  else pwm_frequency = _constrain(pwm_frequency*2, 0, PWM_MAX_FREQ); // constrain to 62.5kHz max => 31.25kHz for a resolution of 256  
+
+  pwm_range = (PWM_CLK / pwm_frequency);
+  pwm_range /= 2; // scale the frequency (centered PWM)
+
+  float dead_time;
+  if (dead_zone != NOT_SET){
+    dead_time = dead_zone/2;
+  }else{
+    dead_time = DEAD_TIME/2; 
+  }
+  
+  int pA_l = digitalPinToPinName(pinA_l); //g_ADigitalPinMap[pinA_l];
+  int pA_h = digitalPinToPinName(pinA_h); //g_ADigitalPinMap[pinA_h];
+  int pB_l = digitalPinToPinName(pinB_l); //g_ADigitalPinMap[pinB_l];
+  int pB_h = digitalPinToPinName(pinB_h); //g_ADigitalPinMap[pinB_h];
+  int pC_l = digitalPinToPinName(pinC_l); //g_ADigitalPinMap[pinC_l];
+  int pC_h = digitalPinToPinName(pinC_h); //g_ADigitalPinMap[pinC_h];
+
+
+  // determine which motor are we connecting
+  // and set the appropriate configuration parameters 
+  int slot_num;
+  for(slot_num = 0; slot_num < 2; slot_num++){
+    if(nrf52_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot
+      nrf52_bldc_6pwm_motor_slots[slot_num].pinAH = pinA_h;
+      break;
+    }
+  }
+  // if no slots available
+  if(slot_num >= 2) return SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  // disable all the slots with the same MCPWM 
+  if( slot_num == 0 ){
+    // slots 0 and 1 of the 3pwm bldc
+    nrf52_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT;
+    nrf52_bldc_3pwm_motor_slots[1].pinA = _TAKEN_SLOT;
+    // slot 0 and 1 of the stepper
+    nrf52_stepper_motor_slots[0].pin1A = _TAKEN_SLOT;
+    nrf52_stepper_motor_slots[1].pin1A = _TAKEN_SLOT;
+  }else{
+    // slots 2 and 3 of the 3pwm bldc
+    nrf52_bldc_3pwm_motor_slots[2].pinA = _TAKEN_SLOT;
+    nrf52_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT;
+    // slot 1 of the stepper
+    nrf52_stepper_motor_slots[2].pin1A = _TAKEN_SLOT;
+    nrf52_stepper_motor_slots[3].pin1A = _TAKEN_SLOT;
+  } 
+
+  // Configure pwm outputs
+  
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[0] = pA_h;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[1] = pA_l;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[2] = pB_h;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->PSEL.OUT[3] = pB_l;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[0];
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->SEQ[0].CNT = 4;
+
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[0] = pC_h;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->PSEL.OUT[1] = pC_l;
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].PTR = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm_channel_sequence[4];
+  nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->SEQ[0].CNT = 4;
+
+  // Initializing the PPI peripheral for sync the pwm slots
+
+  NRF_PPI->CH[slot_num].EEP = (uint32_t)&NRF_EGU0->EVENTS_TRIGGERED[0];
+  NRF_PPI->CH[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1->TASKS_SEQSTART[0];
+  NRF_PPI->FORK[slot_num].TEP = (uint32_t)&nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2->TASKS_SEQSTART[0];
+  NRF_PPI->CHEN =  1UL << slot_num;
+  
+  // configure the pwm type
+  _configureHwPwm(nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm1, nrf52_bldc_6pwm_motor_slots[slot_num].mcpwm2);
+  
+  NRF52DriverParams* params = new NRF52DriverParams();
+  params->slot.slot6pwm = &(nrf52_bldc_6pwm_motor_slots[slot_num]);
+  params->pwm_frequency = pwm_frequency;
+  params->dead_time = dead_time;
+  return params; 
+}
+
+
+
+
+/* Function setting the duty cycle to the pwm pin
+//  - BLDC driver - 6PWM setting
+//  - hardware specific
+*/
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params){
+      bldc_6pwm_motor_slots_t* p = ((NRF52DriverParams*)params)->slot.slot6pwm;
+      float dead_time = ((NRF52DriverParams*)params)->dead_time;
+      p->mcpwm_channel_sequence[0] = (int)(_constrain(dc_a-dead_time,0,1)*pwm_range) | 0x8000;
+      p->mcpwm_channel_sequence[1] = (int)(_constrain(dc_a+dead_time,0,1)*pwm_range);
+      p->mcpwm_channel_sequence[2] = (int)(_constrain(dc_b-dead_time,0,1)*pwm_range) | 0x8000;
+      p->mcpwm_channel_sequence[3] = (int)(_constrain(dc_b+dead_time,0,1)*pwm_range);
+      p->mcpwm_channel_sequence[4] = (int)(_constrain(dc_c-dead_time,0,1)*pwm_range) | 0x8000;
+      p->mcpwm_channel_sequence[5] = (int)(_constrain(dc_c+dead_time,0,1)*pwm_range);     
+      NRF_EGU0->TASKS_TRIGGER[0] = 1;
+
+      _UNUSED(phase_state);
+}
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/portenta_h7_mcu.cpp

@@ -0,0 +1,545 @@
+
+#include "../hardware_api.h"
+
+#if defined(TARGET_PORTENTA_H7)
+
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for Arduino/Portenta_H7")
+#pragma message("")
+
+
+#include "pwmout_api.h"
+#include "pinDefinitions.h"
+#include "platform/mbed_critical.h"
+#include "platform/mbed_power_mgmt.h"
+#include "platform/mbed_assert.h"
+#include "PeripheralPins.h"
+#include "pwmout_device.h"
+
+// default pwm parameters
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
+
+// // 6pwm parameters
+// #define _HARDWARE_6PWM 1
+// #define _SOFTWARE_6PWM 0
+// #define _ERROR_6PWM -1
+
+
+
+typedef struct PortentaDriverParams {
+  pwmout_t pins[4];
+  long pwm_frequency;
+//  float dead_zone;
+} PortentaDriverParams;
+
+
+
+/* Convert STM32 Cube HAL channel to LL channel */
+uint32_t _TIM_ChannelConvert_HAL2LL(uint32_t channel, pwmout_t *obj)
+{
+#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED)
+    if (obj->inverted) {
+        switch (channel) {
+            case TIM_CHANNEL_1  :
+                return LL_TIM_CHANNEL_CH1N;
+            case TIM_CHANNEL_2  :
+                return LL_TIM_CHANNEL_CH2N;
+            case TIM_CHANNEL_3  :
+                return LL_TIM_CHANNEL_CH3N;
+#if defined(LL_TIM_CHANNEL_CH4N)
+            case TIM_CHANNEL_4  :
+                return LL_TIM_CHANNEL_CH4N;
+#endif
+            default : /* Optional */
+                return 0;
+        }
+    } else
+#endif
+    {
+        switch (channel) {
+            case TIM_CHANNEL_1  :
+                return LL_TIM_CHANNEL_CH1;
+            case TIM_CHANNEL_2  :
+                return LL_TIM_CHANNEL_CH2;
+            case TIM_CHANNEL_3  :
+                return LL_TIM_CHANNEL_CH3;
+            case TIM_CHANNEL_4  :
+                return LL_TIM_CHANNEL_CH4;
+            default : /* Optional */
+                return 0;
+        }
+    }
+}
+
+
+
+// int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){
+//   return _pwm_init(obj, digitalPinToPinName(pin), frequency);
+// }
+
+int _pwm_init(pwmout_t *obj, uint32_t pin, long frequency){
+    int peripheral = (int)pinmap_peripheral(digitalPinToPinName(pin), PinMap_PWM);
+    int function = (int)pinmap_find_function(digitalPinToPinName(pin), PinMap_PWM);
+
+    const PinMap static_pinmap = {digitalPinToPinName(pin), peripheral, function};
+
+    pwmout_init_direct(obj, &static_pinmap);
+
+    TIM_HandleTypeDef TimHandle;
+    TimHandle.Instance = (TIM_TypeDef *)(obj->pwm);
+    RCC_ClkInitTypeDef RCC_ClkInitStruct;
+    uint32_t PclkFreq = 0;
+    uint32_t APBxCLKDivider = RCC_HCLK_DIV1;
+    uint8_t i = 0;
+
+
+    __HAL_TIM_DISABLE(&TimHandle);
+
+    // Get clock configuration
+    // Note: PclkFreq contains here the Latency (not used after)
+    HAL_RCC_GetClockConfig(&RCC_ClkInitStruct, &PclkFreq);
+
+    /*  Parse the pwm / apb mapping table to find the right entry */
+    while (pwm_apb_map_table[i].pwm != obj->pwm) i++;
+    // sanity check
+    if (pwm_apb_map_table[i].pwm == 0) return -1;
+    
+
+    if (pwm_apb_map_table[i].pwmoutApb == PWMOUT_ON_APB1) {
+        PclkFreq = HAL_RCC_GetPCLK1Freq();
+        APBxCLKDivider = RCC_ClkInitStruct.APB1CLKDivider;
+    } else {
+#if !defined(PWMOUT_APB2_NOT_SUPPORTED)
+        PclkFreq = HAL_RCC_GetPCLK2Freq();
+        APBxCLKDivider = RCC_ClkInitStruct.APB2CLKDivider;
+#endif
+    }
+
+    long period_us = 500000.0/((float)frequency);
+    /* By default use, 1us as SW pre-scaler */
+    obj->prescaler = 1;
+    // TIMxCLK = PCLKx when the APB prescaler = 1 else TIMxCLK = 2 * PCLKx
+    if (APBxCLKDivider == RCC_HCLK_DIV1) {
+        TimHandle.Init.Prescaler = (((PclkFreq) / 1000000)) - 1; // 1 us tick
+    } else {
+        TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000)) - 1; // 1 us tick
+    }
+    TimHandle.Init.Period = (period_us - 1);
+
+    /*  In case period or pre-scalers are out of range, loop-in to get valid values */
+    while ((TimHandle.Init.Period > 0xFFFF) || (TimHandle.Init.Prescaler > 0xFFFF)) {
+        obj->prescaler = obj->prescaler * 2;
+        if (APBxCLKDivider == RCC_HCLK_DIV1) {
+            TimHandle.Init.Prescaler = (((PclkFreq) / 1000000) * obj->prescaler) - 1;
+        } else {
+            TimHandle.Init.Prescaler = (((PclkFreq * 2) / 1000000) * obj->prescaler) - 1;
+        }
+        TimHandle.Init.Period = (period_us - 1) / obj->prescaler;
+        /*  Period decreases and prescaler increases over loops, so check for
+         *  possible out of range cases */
+        if ((TimHandle.Init.Period < 0xFFFF) && (TimHandle.Init.Prescaler > 0xFFFF)) {
+            break;
+        }
+    }
+
+    TimHandle.Init.ClockDivision = 0;
+    TimHandle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; // center aligned
+
+    if (HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) {
+        return -1;
+    }
+    
+    TIM_OC_InitTypeDef sConfig;
+    // Configure channels
+    sConfig.OCMode       = TIM_OCMODE_PWM1;
+    sConfig.Pulse        = obj->pulse / obj->prescaler;
+    sConfig.OCPolarity   = TIM_OCPOLARITY_HIGH;
+    sConfig.OCFastMode   = TIM_OCFAST_DISABLE;
+#if defined(TIM_OCIDLESTATE_RESET)
+    sConfig.OCIdleState  = TIM_OCIDLESTATE_RESET;
+#endif
+#if defined(TIM_OCNIDLESTATE_RESET)
+    sConfig.OCNPolarity  = TIM_OCNPOLARITY_HIGH;
+    sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+#endif
+
+    int channel = 0;
+    switch (obj->channel) {
+        case 1:
+            channel = TIM_CHANNEL_1;
+            break;
+        case 2:
+            channel = TIM_CHANNEL_2;
+            break;
+        case 3:
+            channel = TIM_CHANNEL_3;
+            break;
+        case 4:
+            channel = TIM_CHANNEL_4;
+            break;
+        default:
+            return -1;
+    }
+    
+    if (LL_TIM_CC_IsEnabledChannel(TimHandle.Instance, _TIM_ChannelConvert_HAL2LL(channel, obj)) == 0) {
+        // If channel is not enabled, proceed to channel configuration
+        if (HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfig, channel) != HAL_OK) {
+            return -1;
+        }
+    } 
+
+    // Save for future use
+    obj->period = period_us;
+#if !defined(PWMOUT_INVERTED_NOT_SUPPORTED)
+    if (obj->inverted) {
+        HAL_TIMEx_PWMN_Start(&TimHandle, channel);
+    } else
+#endif
+    {
+        HAL_TIM_PWM_Start(&TimHandle, channel);
+    }
+    
+    return 0;
+}
+
+// setting pwm to hardware pin - instead analogWrite()
+void _pwm_write(pwmout_t *obj, float value){
+  TIM_HandleTypeDef TimHandle;
+  int channel = 0;
+
+  TimHandle.Instance = (TIM_TypeDef *)(obj->pwm);
+  
+  if (value < (float)0.0) {
+      value = 0.0;
+  } else if (value > (float)1.0) {
+      value = 1.0;
+  }
+
+  obj->pulse = (uint32_t)((float)obj->period * value + 0.5);
+
+  switch (obj->channel) {
+      case 1:
+          channel = TIM_CHANNEL_1;
+          break;
+      case 2:
+          channel = TIM_CHANNEL_2;
+          break;
+      case 3:
+          channel = TIM_CHANNEL_3;
+          break;
+      case 4:
+          channel = TIM_CHANNEL_4;
+          break;
+      default:
+          return;
+  }
+  
+  // If channel already enabled, only update compare value to avoid glitch
+  __HAL_TIM_SET_COMPARE(&TimHandle, channel, obj->pulse / obj->prescaler);
+} 
+
+// init low side pin
+// HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin)
+// {
+//   PinName pin = digitalPinToPinName(ulPin);
+//   TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM);
+//   uint32_t index = get_timer_index(Instance);
+
+//   if (HardwareTimer_Handle[index] == NULL) {
+//     HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM));
+//     HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
+//     HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
+//     TIM_OC_InitTypeDef sConfigOC = TIM_OC_InitTypeDef();
+//     sConfigOC.OCMode = TIM_OCMODE_PWM2;
+//     sConfigOC.Pulse = 100;
+//     sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
+//     sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+//     sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
+//     sConfigOC.OCIdleState = TIM_OCIDLESTATE_SET;
+//     sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+//     uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
+//     HAL_TIM_PWM_ConfigChannel(&(HardwareTimer_Handle[index]->handle), &sConfigOC, channel);
+//   }
+//   HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+//   uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin, PinMap_PWM));
+//   HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, pin);
+//   HT->setOverflow(PWM_freq, HERTZ_FORMAT);
+//   HT->pause();
+//   HT->refresh();
+//   return HT;
+// }
+
+
+// align the timers to end the init
+void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2){
+    TIM_HandleTypeDef TimHandle1, TimHandle2;
+    TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm);
+    TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm);
+    __HAL_TIM_DISABLE(&TimHandle1);
+    __HAL_TIM_DISABLE(&TimHandle2);
+    __HAL_TIM_ENABLE(&TimHandle1); 
+    __HAL_TIM_ENABLE(&TimHandle2); 
+}
+
+// align the timers to end the init
+void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3){
+    TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3;
+    TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm);
+    TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm);
+    TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm);
+    __HAL_TIM_DISABLE(&TimHandle1);
+    __HAL_TIM_DISABLE(&TimHandle2);
+    __HAL_TIM_DISABLE(&TimHandle3);
+    __HAL_TIM_ENABLE(&TimHandle1); 
+    __HAL_TIM_ENABLE(&TimHandle2); 
+    __HAL_TIM_ENABLE(&TimHandle3); 
+}
+
+// align the timers to end the init
+void _alignPWMTimers(pwmout_t *t1, pwmout_t *t2, pwmout_t *t3, pwmout_t *t4){
+    TIM_HandleTypeDef TimHandle1, TimHandle2, TimHandle3, TimHandle4;
+    TimHandle1.Instance = (TIM_TypeDef *)(t1->pwm);
+    TimHandle2.Instance = (TIM_TypeDef *)(t2->pwm);
+    TimHandle3.Instance = (TIM_TypeDef *)(t3->pwm);
+    TimHandle4.Instance = (TIM_TypeDef *)(t4->pwm);
+    __HAL_TIM_DISABLE(&TimHandle1);
+    __HAL_TIM_DISABLE(&TimHandle2);
+    __HAL_TIM_DISABLE(&TimHandle3);
+    __HAL_TIM_DISABLE(&TimHandle4);
+    __HAL_TIM_ENABLE(&TimHandle1); 
+    __HAL_TIM_ENABLE(&TimHandle2); 
+    __HAL_TIM_ENABLE(&TimHandle3);  
+    __HAL_TIM_ENABLE(&TimHandle4); 
+}
+
+// // configure hardware 6pwm interface only one timer with inverted channels
+// HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l)
+// {
+//   PinName uhPinName = digitalPinToPinName(pinA_h);
+//   PinName ulPinName = digitalPinToPinName(pinA_l);
+//   PinName vhPinName = digitalPinToPinName(pinB_h);
+//   PinName vlPinName = digitalPinToPinName(pinB_l);
+//   PinName whPinName = digitalPinToPinName(pinC_h);
+//   PinName wlPinName = digitalPinToPinName(pinC_l);
+
+//   TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM);
+
+//   uint32_t index = get_timer_index(Instance);
+
+//   if (HardwareTimer_Handle[index] == NULL) {
+//     HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM));
+//     HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
+//     HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
+//     ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT);
+//   }
+//   HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName);
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName);
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName);
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(vlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vlPinName);
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(whPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, whPinName);
+//   HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName);
+
+//   // dead time is set in nanoseconds
+//   uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone;
+//   uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns);
+//   LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear!
+//   LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N);
+
+//   HT->pause();
+//   HT->refresh();
+//   HT->resume();
+//   return HT;
+// }
+
+
+// // returns 0 if each pair of pwm channels has same channel
+// // returns 1 all the channels belong to the same timer - hardware inverted channels
+// // returns -1 if neither - avoid configuring - error!!!
+// int _interfaceType(const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){
+//   PinName nameAH = digitalPinToPinName(pinA_h);
+//   PinName nameAL = digitalPinToPinName(pinA_l);
+//   PinName nameBH = digitalPinToPinName(pinB_h);
+//   PinName nameBL = digitalPinToPinName(pinB_l);
+//   PinName nameCH = digitalPinToPinName(pinC_h);
+//   PinName nameCL = digitalPinToPinName(pinC_l);
+//   int tim1 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAH, PinMap_PWM));
+//   int tim2 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameAL, PinMap_PWM));
+//   int tim3 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBH, PinMap_PWM));
+//   int tim4 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameBL, PinMap_PWM));
+//   int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM));
+//   int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM));
+//   if(tim1 == tim2 && tim2==tim3 && tim3==tim4  && tim4==tim5 && tim5==tim6)
+//     return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer
+//   else if(tim1 == tim2 && tim3==tim4  && tim5==tim6)
+//     return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer
+//   else
+//     return _ERROR_6PWM; // might be error avoid configuration
+// }
+
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+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
+  
+  PortentaDriverParams* params = new PortentaDriverParams();
+  params->pwm_frequency = pwm_frequency;
+
+  core_util_critical_section_enter();
+  _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency);
+  _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency);
+  // allign the timers
+  _alignPWMTimers(&(params->pins[0]), &(params->pins[1]));
+  core_util_critical_section_exit();
+  return params;
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  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
+
+  PortentaDriverParams* params = new PortentaDriverParams();
+  params->pwm_frequency = pwm_frequency;
+
+  core_util_critical_section_enter();
+  _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency);
+  _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency);
+  _pwm_init(&(params->pins[2]), pinC, (long)pwm_frequency);
+  // allign the timers
+  _alignPWMTimers(&(params->pins[0]), &(params->pins[1]), &(params->pins[2]));
+  core_util_critical_section_exit();
+
+  return params;
+}
+
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  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
+
+  PortentaDriverParams* params = new PortentaDriverParams();
+  params->pwm_frequency = pwm_frequency;
+
+  core_util_critical_section_enter();
+  _pwm_init(&(params->pins[0]), pinA, (long)pwm_frequency);
+  _pwm_init(&(params->pins[1]), pinB, (long)pwm_frequency);
+  _pwm_init(&(params->pins[2]), pinC, (long)pwm_frequency);
+  _pwm_init(&(params->pins[3]), pinD, (long)pwm_frequency);
+  // allign the timers
+  _alignPWMTimers(&(params->pins[0]), &(params->pins[1]), &(params->pins[2]), &(params->pins[3]));
+  core_util_critical_section_exit();
+
+  return params;
+}
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+//- hardware speciffic
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params){
+    core_util_critical_section_enter();
+    _pwm_write(&(((PortentaDriverParams*)params)->pins[0]), (float)dc_a);
+    _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_b);
+    core_util_critical_section_exit();
+}
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+//- hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params){
+    core_util_critical_section_enter();
+    _pwm_write(&(((PortentaDriverParams*)params)->pins[0]), (float)dc_a);
+    _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_b);
+    _pwm_write(&(((PortentaDriverParams*)params)->pins[2]), (float)dc_c);
+    core_util_critical_section_exit();
+}
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+//- hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+    core_util_critical_section_enter();
+    _pwm_write(&(((PortentaDriverParams*)params)->pins[0]), (float)dc_1a);
+    _pwm_write(&(((PortentaDriverParams*)params)->pins[1]), (float)dc_1b);
+    _pwm_write(&(((PortentaDriverParams*)params)->pins[2]), (float)dc_2a);
+    _pwm_write(&(((PortentaDriverParams*)params)->pins[3]), (float)dc_2b);
+    core_util_critical_section_exit();
+}
+
+
+// 6-PWM currently not supported, defer to generic, which also doesn't support it ;-)
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+//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){
+  // 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 |%0kHz max
+  // // center-aligned frequency is uses two periods
+  // pwm_frequency *=2;
+
+  // // find configuration
+  // int config = _interfaceType(pinA_h, pinA_l,  pinB_h, pinB_l, pinC_h, pinC_l);
+  // // configure accordingly
+  // switch(config){
+  //   case _ERROR_6PWM:
+  //     return -1; // fail
+  //     break;
+  //   case _HARDWARE_6PWM:
+  //     _initHardware6PWMInterface(pwm_frequency, dead_zone, pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
+  //     break;
+  //   case _SOFTWARE_6PWM:
+  //     HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinA_h);
+  //     _initPinPWMLow(pwm_frequency, pinA_l);
+  //     HardwareTimer* HT2 = _initPinPWMHigh(pwm_frequency,pinB_h);
+  //     _initPinPWMLow(pwm_frequency, pinB_l);
+  //     HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency,pinC_h);
+  //     _initPinPWMLow(pwm_frequency, pinC_l);
+  //     _alignPWMTimers(HT1, HT2, HT3);
+  //     break;
+  // }
+//   return -1; // success
+// }
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+// - BLDC driver - 6PWM setting
+// - hardware specific
+//void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, void* params){
+  // // find configuration
+  // int config = _interfaceType(pinA_h, pinA_l,  pinB_h, pinB_l, pinC_h, pinC_l);
+  // // set pwm accordingly
+  // switch(config){
+  //   case _HARDWARE_6PWM:
+  //     _setPwm(pinA_h, _PWM_RANGE*dc_a, _PWM_RESOLUTION);
+  //     _setPwm(pinB_h, _PWM_RANGE*dc_b, _PWM_RESOLUTION);
+  //     _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION);
+  //     break;
+  //   case _SOFTWARE_6PWM:
+  //     _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION);
+  //     break;
+  // }
+//}
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/renesas/renesas.cpp

@@ -0,0 +1,609 @@
+
+#include "./renesas.h"
+
+#if defined(ARDUINO_UNOR4_WIFI) || defined(ARDUINO_UNOR4_MINIMA)
+
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for Arduino/Renesas (UNO R4)")
+#pragma message("")
+
+
+
+#include "communication/SimpleFOCDebug.h"
+#include "FspTimer.h"
+
+#define GPT_OPEN                                         (0x00475054ULL)
+
+/*
+  We use the GPT timers, there are 2 channels (32 bit) + 6 channels (16 bit)
+  Each channel has 2 outputs (GTIOCAx and GTIOCBx) which can be complimentary.
+
+  So each timer channel can handle one half-bridge, using either a single (3-PWM) or
+  two complimentary PWM signals (6-PWM).
+
+  For 1-PWM through 4-PWM, we need as many channels as PWM signals, and we can use 
+  either output A or B of the timer (we can set the polarity) - but not both.
+
+  For 6-PWM we need 3 channels, and use both outputs A and B of each channel, then
+  we can do hardware dead-time.
+  Or we can use seperate channels for high and low side, with software dead-time.
+  Each phase can be either hardware (1 channel) or software (2 channels) dead-time
+  individually, they don't all have to be one or the other.
+
+  Selected channels can be started together, so we can keep the phases in sync for
+  low-side current sensing and software 6-PWM.
+
+  The event system should permit low side current sensing without needing interrupts,
+  but this will be handled by the current sense driver.
+
+  Supported:
+    - arbitrary PWM frequencies between 1Hz (minimum we can set with our integer based API)
+      and around 48kHz (more would be possible but the range will be low)
+    - PWM range at 24kHz (default) is 1000
+    - PWM range at 48kHz is 500
+    - polarity setting is supported, in all modes
+    - phase state setting is supported, in 3-PWM, 6-PWM hardware dead-time and 6-PWM software dead-time
+
+  TODOs:
+    - change setDutyCycle to use register access for speed
+    - add event system support for low-side current sensing
+    - perhaps add support to reserve timers used also in 
+      Arduino Pwm.h code, for compatibility with analogWrite()
+    - check if there is a better way for phase-state setting
+ */
+
+
+// track which channels are used
+bool channel_used[GPT_HOWMANY] = { false };
+
+
+struct RenesasTimerConfig {
+  timer_cfg_t timer_cfg;
+  gpt_instance_ctrl_t ctrl;
+  gpt_extended_cfg_t ext_cfg;
+  gpt_extended_pwm_cfg_t pwm_cfg;
+  gpt_io_pin_t duty_pin;
+};
+
+struct ClockDivAndRange {
+  timer_source_div_t clk_div;
+  uint32_t range;
+};
+
+ClockDivAndRange getClockDivAndRange(uint32_t pwm_frequency, uint8_t timer_channel) {
+  ClockDivAndRange result;
+  uint32_t max_count = (timer_channel < GTP32_HOWMANY)? 4294967295 : 65535;
+  uint32_t freq_hz = R_FSP_SystemClockHzGet(FSP_PRIV_CLOCK_PCLKD);
+  float range = (float) freq_hz / ((float) pwm_frequency * 2.0f);
+  if(range / 1.0 < max_count) {
+      result.range = (uint32_t) (range / 1.0);
+      result.clk_div = TIMER_SOURCE_DIV_1;
+  }
+  else if (range / 2.0 < max_count) {
+      result.range = (uint32_t) (range / 2.0);
+      result.clk_div = TIMER_SOURCE_DIV_2;
+  }
+  else if(range / 4.0 < max_count) {
+      result.range = (uint32_t) (range / 4.0);
+      result.clk_div = TIMER_SOURCE_DIV_4;
+  }
+  else if(range / 8.0 < max_count) {
+      result.range = (uint32_t) (range / 8.0 );
+      result.clk_div = TIMER_SOURCE_DIV_8;
+  }
+  else if(range / 16.0 < max_count) {
+      result.range = (uint32_t) (range / 16.0 );
+      result.clk_div = TIMER_SOURCE_DIV_16;
+  }
+  else if (range / 32.0 < max_count) {
+      result.range = (uint32_t) (range / 32.0 );
+      result.clk_div = TIMER_SOURCE_DIV_32;
+  }
+  else if(range / 64.0 < max_count) {
+      result.range = (uint32_t) (range / 64.0 );
+      result.clk_div = TIMER_SOURCE_DIV_64;
+  }
+  else if(range / 128.0 < max_count) {
+      result.range = (uint32_t) (range / 128.0 );
+      result.clk_div = TIMER_SOURCE_DIV_128;
+  }
+  else if(range / 256.0 < max_count) {
+      result.range = (uint32_t) (range / 256.0 );
+      result.clk_div = TIMER_SOURCE_DIV_256;
+  }
+  else if(range / 512.0 < max_count) {
+      result.range = (uint32_t) (range / 512.0 );
+      result.clk_div = TIMER_SOURCE_DIV_512;
+  }
+  else if(range / 1024.0 < max_count) {
+      result.range = (uint32_t) (range / 1024.0 );
+      result.clk_div = TIMER_SOURCE_DIV_1024;
+  }
+  else {
+      SimpleFOCDebug::println("DRV: PWM frequency too low");
+  }
+  return result;
+};
+
+
+bool configureTimerPin(RenesasHardwareDriverParams* params, uint8_t index, bool active_high, bool complementary = false, bool complementary_active_high = true) {
+  uint8_t pin = params->pins[index];
+  uint8_t pin_C;
+  std::array<uint16_t, 3> pinCfgs = getPinCfgs(pin, PIN_CFG_REQ_PWM);
+  std::array<uint16_t, 3> pinCfgs_C;
+  if(pinCfgs[0] == 0) {
+    SIMPLEFOC_DEBUG("DRV: no PWM on pin ", pin);
+    return false;
+  }
+  if (IS_PIN_AGT_PWM(pinCfgs[0])) {
+    SIMPLEFOC_DEBUG("DRV: AGT timer not supported");
+    return false;
+  }
+  // get the timer channel
+  uint8_t timer_channel = GET_CHANNEL(pinCfgs[0]);
+  // check its not used
+  if (channel_used[timer_channel]) {
+    SIMPLEFOC_DEBUG("DRV: channel in use");
+    return false;
+  }
+
+  if (complementary) {
+    pin_C = params->pins[index+1];
+    pinCfgs_C = getPinCfgs(pin_C, PIN_CFG_REQ_PWM);
+    if(pinCfgs_C[0] == 0) {
+      SIMPLEFOC_DEBUG("DRV: no PWM on pin ", pin_C);
+      return false;
+    }
+    if (IS_PIN_AGT_PWM(pinCfgs_C[0]) || GET_CHANNEL(pinCfgs_C[0])!=timer_channel) {
+      SIMPLEFOC_DEBUG("DRV: comp. channel different");
+      return false;
+    }
+  }
+  TimerPWMChannel_t pwm_output = IS_PWM_ON_A(pinCfgs[0]) ? CHANNEL_A : CHANNEL_B;
+  if (complementary) {
+    TimerPWMChannel_t pwm_output_C = IS_PWM_ON_A(pinCfgs_C[0]) ? CHANNEL_A : CHANNEL_B;
+    if (pwm_output_C != CHANNEL_A || pwm_output != CHANNEL_B) {
+      SIMPLEFOC_DEBUG("DRV: output A/B mismatch");
+      return false;
+    }
+  }
+
+  // configure GPIO pin
+  fsp_err_t err = R_IOPORT_PinCfg(&g_ioport_ctrl, g_pin_cfg[pin].pin, (uint32_t) (IOPORT_CFG_PERIPHERAL_PIN | IOPORT_PERIPHERAL_GPT1));
+  if ((err == FSP_SUCCESS) && complementary)
+    err = R_IOPORT_PinCfg(&g_ioport_ctrl, g_pin_cfg[pin_C].pin, (uint32_t) (IOPORT_CFG_PERIPHERAL_PIN | IOPORT_PERIPHERAL_GPT1));
+  if (err != FSP_SUCCESS) {
+    SIMPLEFOC_DEBUG("DRV: pin config failed");
+    return false;
+  }
+
+
+  // configure timer channel - frequency / top value
+  ClockDivAndRange timings = getClockDivAndRange(params->pwm_frequency, timer_channel);
+  #if defined(SIMPLEFOC_RENESAS_DEBUG)
+  SimpleFOCDebug::println("---PWM Config---");
+  SimpleFOCDebug::println("DRV: pwm pin: ", pin);
+  if (complementary)
+    SimpleFOCDebug::println("DRV: compl. pin: ", pin_C);
+  SimpleFOCDebug::println("DRV: pwm channel: ", timer_channel);
+  SimpleFOCDebug::print("DRV: pwm A/B: "); SimpleFOCDebug::println(complementary?"A+B":((pwm_output==CHANNEL_A)?"A":"B"));
+  SimpleFOCDebug::println("DRV: pwm freq: ", (int)params->pwm_frequency);
+  SimpleFOCDebug::println("DRV: pwm range: ", (int)timings.range);
+  SimpleFOCDebug::println("DRV: pwm clkdiv: ", timings.clk_div);
+  #endif
+
+  RenesasTimerConfig* t = new RenesasTimerConfig();
+  // configure timer channel - count mode
+  t->timer_cfg.channel = timer_channel;
+  t->timer_cfg.mode = TIMER_MODE_TRIANGLE_WAVE_SYMMETRIC_PWM;
+  t->timer_cfg.source_div = timings.clk_div;
+  t->timer_cfg.period_counts = timings.range;
+  t->timer_cfg.duty_cycle_counts = 0;
+  t->timer_cfg.p_callback = nullptr;
+  t->timer_cfg.p_context = nullptr;
+  t->timer_cfg.p_extend = &(t->ext_cfg);
+  t->timer_cfg.cycle_end_ipl = BSP_IRQ_DISABLED;
+  t->timer_cfg.cycle_end_irq = FSP_INVALID_VECTOR;
+
+  t->ext_cfg.p_pwm_cfg = &(t->pwm_cfg);
+  t->pwm_cfg.trough_ipl = BSP_IRQ_DISABLED;
+  t->pwm_cfg.trough_irq = FSP_INVALID_VECTOR;
+  t->pwm_cfg.poeg_link = GPT_POEG_LINK_POEG0;
+  t->pwm_cfg.output_disable = GPT_OUTPUT_DISABLE_NONE;
+  t->pwm_cfg.adc_trigger = GPT_ADC_TRIGGER_NONE;
+  t->pwm_cfg.dead_time_count_up = 0;
+  t->pwm_cfg.dead_time_count_down = 0;
+  t->pwm_cfg.adc_a_compare_match = 0;
+  t->pwm_cfg.adc_b_compare_match = 0;
+  t->pwm_cfg.interrupt_skip_source = GPT_INTERRUPT_SKIP_SOURCE_NONE;
+  t->pwm_cfg.interrupt_skip_count = GPT_INTERRUPT_SKIP_COUNT_0;
+  t->pwm_cfg.interrupt_skip_adc = GPT_INTERRUPT_SKIP_ADC_NONE;
+  t->pwm_cfg.gtioca_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED;
+  t->pwm_cfg.gtiocb_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED;
+  // configure dead-time if both outputs are used
+  if (complementary) {
+    uint32_t dt = params->dead_zone * timings.range;
+    t->pwm_cfg.dead_time_count_up = dt;
+    t->pwm_cfg.dead_time_count_down = dt;
+  }
+
+  // configure timer channel - outputs and polarity
+  t->ext_cfg.gtior_setting.gtior = 0L;
+  if (!complementary) {
+    if(pwm_output == CHANNEL_A) {
+        t->duty_pin = GPT_IO_PIN_GTIOCA;
+        t->ext_cfg.gtioca.output_enabled = true;
+        t->ext_cfg.gtiocb.output_enabled = false;
+        t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (active_high ? 0x00 : 0x10);
+        t->ext_cfg.gtior_setting.gtior_b.oadflt = active_high ? 0x00 : 0x01;
+        // t->ext_cfg.gtior_setting.gtior_b.oahld = 0x0;
+        // t->ext_cfg.gtior_setting.gtior_b.oadf = 0x0;
+        // t->ext_cfg.gtior_setting.gtior_b.nfaen = 0x0;
+    } 
+    else {
+        t->duty_pin = GPT_IO_PIN_GTIOCB;
+        t->ext_cfg.gtiocb.output_enabled = true;
+        t->ext_cfg.gtioca.output_enabled = false;
+        t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (active_high ? 0x00 : 0x10);
+        t->ext_cfg.gtior_setting.gtior_b.obdflt = active_high ? 0x00 : 0x01;
+    }
+  }
+  else {
+    t->duty_pin = GPT_IO_PIN_GTIOCA_AND_GTIOCB;
+    t->ext_cfg.gtioca.output_enabled = true;
+    t->ext_cfg.gtiocb.output_enabled = true;
+    t->ext_cfg.gtior_setting.gtior_b.gtioa = 0x03 | (!complementary_active_high ? 0x00 : 0x10);
+    t->ext_cfg.gtior_setting.gtior_b.oadflt = !complementary_active_high ? 0x00 : 0x01;
+    t->ext_cfg.gtior_setting.gtior_b.gtiob = 0x03 | (active_high ? 0x00 : 0x10);
+    t->ext_cfg.gtior_setting.gtior_b.obdflt = active_high ? 0x00 : 0x01;
+  }
+
+  // lets stop the timer in case its running
+  if (GPT_OPEN == t->ctrl.open) {
+    if (R_GPT_Stop(&(t->ctrl)) != FSP_SUCCESS) {
+      SIMPLEFOC_DEBUG("DRV: timer stop failed");
+      return false;
+    }
+  }
+
+  memset(&(t->ctrl), 0, sizeof(gpt_instance_ctrl_t));
+  err = R_GPT_Open(&(t->ctrl),&(t->timer_cfg));
+  if ((err != FSP_ERR_ALREADY_OPEN) && (err != FSP_SUCCESS)) {
+    SIMPLEFOC_DEBUG("DRV: open failed");
+    return false;
+  }
+  #if defined(SIMPLEFOC_RESENSAS_DEBUG)
+  if (err == FSP_ERR_ALREADY_OPEN) {
+    SimpleFOCDebug::println("DRV: timer already open");
+  }
+  #endif
+  err = R_GPT_PeriodSet(&(t->ctrl), t->timer_cfg.period_counts);
+  if (err != FSP_SUCCESS) {
+    SIMPLEFOC_DEBUG("DRV: period set failed");
+    return false;
+  }  
+  err = R_GPT_OutputEnable(&(t->ctrl), t->duty_pin);
+  if (err != FSP_SUCCESS) {
+    SIMPLEFOC_DEBUG("DRV: pin enable failed");
+    return false;
+  }
+
+  channel_used[timer_channel] = true;
+  params->timer_config[index] = t;
+  params->channels[index] = timer_channel;
+  if (complementary) {
+    params->timer_config[index+1] = t;
+    params->channels[index+1] = timer_channel;
+  }
+
+  return true;
+}
+
+
+// start the timer channels for the motor, synchronously
+bool startTimerChannels(RenesasHardwareDriverParams* params, int num_channels) {
+  uint32_t mask = 0;
+  for (int i = 0; i < num_channels; i++) {
+    // RenesasTimerConfig* t = params->timer_config[i];
+    // if (R_GPT_Start(&(t->ctrl)) != FSP_SUCCESS) {
+    //   SIMPLEFOC_DEBUG("DRV: timer start failed");
+    //   return false;
+    // }
+    mask |= (1 << params->channels[i]);
+#if defined(SIMPLEFOC_RENESAS_DEBUG)
+    SimpleFOCDebug::println("DRV: starting timer: ", params->channels[i]);
+#endif
+  }
+  params->timer_config[0]->ctrl.p_reg->GTSTR |= mask;
+  #if defined(SIMPLEFOC_RENESAS_DEBUG)
+    SimpleFOCDebug::println("DRV: timers started");
+  #endif
+  return true;
+}
+
+
+// check if the given pins are on the same timer channel
+bool isHardware6Pwm(const int pin1, const int pin2) {
+  std::array<uint16_t, 3> pinCfgs1 = getPinCfgs(pin1, PIN_CFG_REQ_PWM);
+  std::array<uint16_t, 3> pinCfgs2 = getPinCfgs(pin2, PIN_CFG_REQ_PWM);
+  if(pinCfgs1[0] == 0 || pinCfgs2[0] == 0)
+    return false;
+  if (IS_PIN_AGT_PWM(pinCfgs1[0]) || IS_PIN_AGT_PWM(pinCfgs2[0]))
+    return false;
+  uint8_t timer_channel1 = GET_CHANNEL(pinCfgs1[0]);
+  uint8_t timer_channel2 = GET_CHANNEL(pinCfgs2[0]);
+  return timer_channel1==timer_channel2;
+}
+
+
+
+void* _configure1PWM(long pwm_frequency, const int pinA) {
+  RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams;
+  params->pins[0] = pinA;
+  params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency;
+  bool success = true;
+  success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH);
+  if (success)
+    success = startTimerChannels(params, 1);
+  if (!success)
+    return SIMPLEFOC_DRIVER_INIT_FAILED;
+  return params;
+}
+
+
+void* _configure2PWM(long pwm_frequency,const int pinA, const int pinB) {
+  RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams;
+  params->pins[0] = pinA; params->pins[1] = pinB;
+  params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency;
+  bool success = true;
+  success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH);
+  success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH);
+  if (!success)
+    success &= startTimerChannels(params, 2);
+  if (!success)
+    return SIMPLEFOC_DRIVER_INIT_FAILED;
+  return params;
+}
+
+
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams;
+  params->pins[0] = pinA; params->pins[1] = pinB; params->pins[2] = pinC;
+  params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency;
+  bool success = true;
+  success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH);
+  success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH);
+  success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_ACTIVE_HIGH);
+  if (success)
+    success = startTimerChannels(params, 3);
+  if (!success)
+    return SIMPLEFOC_DRIVER_INIT_FAILED;
+  return params;
+}
+
+
+void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+  RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams;
+  params->pins[0] = pin1A; params->pins[1] = pin1B; params->pins[2] = pin2A; params->pins[3] = pin2B;
+  params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency;
+  bool success = true;
+  success = configureTimerPin(params, 0, SIMPLEFOC_PWM_ACTIVE_HIGH);
+  success &= configureTimerPin(params, 1, SIMPLEFOC_PWM_ACTIVE_HIGH);
+  success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_ACTIVE_HIGH);
+  success &= configureTimerPin(params, 3, SIMPLEFOC_PWM_ACTIVE_HIGH);
+  if (success)
+    success = startTimerChannels(params, 4);
+  if (!success)
+    return SIMPLEFOC_DRIVER_INIT_FAILED;
+  return params;
+}
+
+
+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){
+  RenesasHardwareDriverParams* params = new RenesasHardwareDriverParams;
+  params->pins[0] = pinA_h; params->pins[1] = pinA_l; params->pins[2] = pinB_h; params->pins[3] = pinB_l; params->pins[4] = pinC_h; params->pins[5] = pinC_l;
+  params->pwm_frequency = (pwm_frequency==NOT_SET)?RENESAS_DEFAULT_PWM_FREQUENCY:pwm_frequency;
+  params->dead_zone = (dead_zone==NOT_SET)?RENESAS_DEFAULT_DEAD_ZONE:dead_zone;
+
+  bool success = true;
+  if (isHardware6Pwm(pinA_h, pinA_l))
+    success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH);
+  else {
+    success &= configureTimerPin(params, 0, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH);
+    success &= configureTimerPin(params, 1, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH); // reverse polarity on low side gives desired active high/low behaviour
+  }
+
+  if (isHardware6Pwm(pinB_h, pinB_l))
+    success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH);
+  else {
+    success &= configureTimerPin(params, 2, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH);
+    success &= configureTimerPin(params, 3, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH);
+  }
+
+  if (isHardware6Pwm(pinC_h, pinC_l))
+    success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH, true, SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH);
+  else {
+    success &= configureTimerPin(params, 4, SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH);
+    success &= configureTimerPin(params, 5, !SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH);
+  }
+
+  if (success)
+    success = startTimerChannels(params, 6);
+  if (!success)
+    return SIMPLEFOC_DRIVER_INIT_FAILED;
+  return params;
+}
+
+
+
+
+void _writeDutyCycle1PWM(float dc_a, void* params){
+  RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0];
+  uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts));
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+}
+
+
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params){
+  RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0];
+  uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts));
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+  t = ((RenesasHardwareDriverParams*)params)->timer_config[1];
+  duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts));
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+}
+
+
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params){
+  RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0];
+  uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts));
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+  t = ((RenesasHardwareDriverParams*)params)->timer_config[1];
+  duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts));
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+  t = ((RenesasHardwareDriverParams*)params)->timer_config[2];
+  duty_cycle_counts = (uint32_t)(dc_c * (float)(t->timer_cfg.period_counts));
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+}
+
+
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+  RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0];
+  uint32_t duty_cycle_counts = (uint32_t)(dc_1a * (float)(t->timer_cfg.period_counts));
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+  t = ((RenesasHardwareDriverParams*)params)->timer_config[1];
+  duty_cycle_counts = (uint32_t)(dc_1b * (float)(t->timer_cfg.period_counts));
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+  t = ((RenesasHardwareDriverParams*)params)->timer_config[2];
+  duty_cycle_counts = (uint32_t)(dc_2a * (float)(t->timer_cfg.period_counts));
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+  t = ((RenesasHardwareDriverParams*)params)->timer_config[3];
+  duty_cycle_counts = (uint32_t)(dc_2b * (float)(t->timer_cfg.period_counts));
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+}
+
+
+
+void _setSinglePhaseState(RenesasTimerConfig* hi, RenesasTimerConfig* lo, PhaseState state) {
+  gpt_gtior_setting_t gtior;
+  gtior.gtior = hi->ctrl.p_reg->GTIOR;
+  bool on = (state==PHASE_ON) || (state==PHASE_HI);
+
+  if (hi->duty_pin == GPT_IO_PIN_GTIOCA_AND_GTIOCB) {
+    bool ch = false;
+    if (gtior.gtior_b.obe != on) {
+      gtior.gtior_b.obe = on;
+      ch = true;
+    } // B is high side
+    on = (state==PHASE_ON) || (state==PHASE_LO);
+    if (gtior.gtior_b.oae != on) {
+      gtior.gtior_b.oae = on;
+      ch = true;
+    }
+    if (ch)
+      hi->ctrl.p_reg->GTIOR = gtior.gtior;
+    return;
+  }
+
+  if (hi->duty_pin == GPT_IO_PIN_GTIOCA) {
+    if (gtior.gtior_b.oae != on) {
+      gtior.gtior_b.oae = on;
+      hi->ctrl.p_reg->GTIOR = gtior.gtior;
+    }
+  }
+  else if (hi->duty_pin == GPT_IO_PIN_GTIOCB) {
+    if (gtior.gtior_b.obe != on) {
+      gtior.gtior_b.obe = on;
+      hi->ctrl.p_reg->GTIOR = gtior.gtior;
+    }
+  }
+
+  gtior.gtior = lo->ctrl.p_reg->GTIOR;
+  on = (state==PHASE_ON) || (state==PHASE_LO);
+  if (lo->duty_pin == GPT_IO_PIN_GTIOCA) {
+    if (gtior.gtior_b.oae != on) {
+      gtior.gtior_b.oae = on;
+      lo->ctrl.p_reg->GTIOR = gtior.gtior;
+    }
+  }
+  else if (lo->duty_pin == GPT_IO_PIN_GTIOCB) {
+    if (gtior.gtior_b.obe != on) {
+      gtior.gtior_b.obe = on;
+      lo->ctrl.p_reg->GTIOR = gtior.gtior;
+    }
+  }
+
+}
+
+
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params){
+  RenesasTimerConfig* t = ((RenesasHardwareDriverParams*)params)->timer_config[0];
+  RenesasTimerConfig* t1 = ((RenesasHardwareDriverParams*)params)->timer_config[1];
+  uint32_t dt = (uint32_t)(((RenesasHardwareDriverParams*)params)->dead_zone * (float)(t->timer_cfg.period_counts));
+  uint32_t duty_cycle_counts = (uint32_t)(dc_a * (float)(t->timer_cfg.period_counts));
+  bool hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[0] == ((RenesasHardwareDriverParams*)params)->channels[1];
+  uint32_t dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0;
+  _setSinglePhaseState(t, t1, phase_state[0]);
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts - dt_act, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+  if (!hw_deadtime) {
+    if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) {
+        // error
+    }    
+  }
+
+  t = ((RenesasHardwareDriverParams*)params)->timer_config[2];
+  t1 = ((RenesasHardwareDriverParams*)params)->timer_config[3];
+  duty_cycle_counts = (uint32_t)(dc_b * (float)(t->timer_cfg.period_counts));
+  hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[2] == ((RenesasHardwareDriverParams*)params)->channels[3];
+  dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0;
+  _setSinglePhaseState(t, t1, phase_state[1]);
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts - dt_act, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+  if (!hw_deadtime) {
+    if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) {
+        // error
+    }    
+  }
+
+  t = ((RenesasHardwareDriverParams*)params)->timer_config[4];
+  t1 = ((RenesasHardwareDriverParams*)params)->timer_config[5];
+  duty_cycle_counts = (uint32_t)(dc_c * (float)(t->timer_cfg.period_counts));
+  hw_deadtime = ((RenesasHardwareDriverParams*)params)->channels[4] == ((RenesasHardwareDriverParams*)params)->channels[5];
+  dt_act = (duty_cycle_counts>0 && !hw_deadtime)?dt:0;
+  _setSinglePhaseState(t, t1, phase_state[2]);
+  if (R_GPT_DutyCycleSet(&(t->ctrl), duty_cycle_counts, t->duty_pin) != FSP_SUCCESS) {
+      // error
+  }
+  if (!hw_deadtime) {
+    if (R_GPT_DutyCycleSet(&(t1->ctrl), duty_cycle_counts + dt_act, t1->duty_pin) != FSP_SUCCESS) {
+        // error
+    }    
+  }
+
+}
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/renesas/renesas.h

@@ -0,0 +1,28 @@
+#pragma once
+
+
+#include "../../hardware_api.h"
+
+
+#if defined(ARDUINO_UNOR4_WIFI) || defined(ARDUINO_UNOR4_MINIMA)
+
+// uncomment to enable debug output from Renesas driver
+// can set this as build-flag in Arduino IDE or PlatformIO
+#define SIMPLEFOC_RENESAS_DEBUG
+
+#define RENESAS_DEFAULT_PWM_FREQUENCY 24000
+#define RENESAS_DEFAULT_DEAD_ZONE 0.05f
+
+struct RenesasTimerConfig;
+
+typedef struct RenesasHardwareDriverParams {
+    uint8_t pins[6];
+    uint8_t channels[6];
+    RenesasTimerConfig* timer_config[6];
+    long pwm_frequency;
+    float dead_zone;
+} RenesasHardwareDriverParams;
+
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/rp2040/rp2040_mcu.cpp

@@ -0,0 +1,237 @@
+
+/**
+ * Support for the RP2040 MCU, as found on the Raspberry Pi Pico.
+ */
+#if defined(TARGET_RP2040)
+
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for RP2040")
+#pragma message("")
+
+
+#define SIMPLEFOC_DEBUG_RP2040
+
+#include "../../hardware_api.h"
+#include "./rp2040_mcu.h"
+#include "hardware/pwm.h"
+#include "hardware/clocks.h"
+
+#define _PWM_FREQUENCY 24000
+#define _PWM_FREQUENCY_MAX 66000
+#define _PWM_FREQUENCY_MIN 1
+
+
+
+// until I can figure out if this can be quickly read from some register, keep it here.
+// it also serves as a marker for what slices are already used.
+uint16_t wrapvalues[NUM_PWM_SLICES];
+
+
+// TODO add checks which channels are already used...
+
+void setupPWM(int pin, long pwm_frequency, bool invert, RP2040DriverParams* params, uint8_t index) {
+	gpio_set_function(pin, GPIO_FUNC_PWM);
+	uint slice = pwm_gpio_to_slice_num(pin);
+	uint chan = pwm_gpio_to_channel(pin);
+	params->pins[index] = pin;
+	params->slice[index] = slice;
+	params->chan[index] = chan;
+	uint32_t sysclock_hz = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_SYS) * 1000;
+	uint32_t factor = 4096 * 2 * pwm_frequency;
+	uint32_t div = sysclock_hz / factor;
+	if (sysclock_hz % factor !=0) div+=1;
+	if (div < 16) div = 16;
+	uint32_t wrapvalue = (sysclock_hz * 8) / div / pwm_frequency - 1;
+#ifdef SIMPLEFOC_DEBUG_RP2040
+	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(" divisor ");
+	SimpleFOCDebug::print((int)(div>>4));
+	SimpleFOCDebug::print(".");
+	SimpleFOCDebug::print((int)(div&0xF));
+	SimpleFOCDebug::print(" top value ");
+	SimpleFOCDebug::println((int)wrapvalue);
+#endif
+	if (wrapvalue < 999)
+		SimpleFOCDebug::println("Warning: PWM resolution is low.");
+	pwm_set_clkdiv_int_frac(slice, div>>4, div&0xF);
+	pwm_set_phase_correct(slice, true);
+	pwm_set_wrap(slice, wrapvalue);
+	wrapvalues[slice] = wrapvalue;
+	if (invert) {
+		if (chan==0)
+			hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_A_INV_LSB, PWM_CH0_CSR_A_INV_BITS);
+		else
+			hw_write_masked(&pwm_hw->slice[slice].csr, 0x1 << PWM_CH0_CSR_B_INV_LSB, PWM_CH0_CSR_B_INV_BITS);
+	}
+	pwm_set_chan_level(slice, chan, 0); // switch off initially
+}
+
+
+void syncSlices() {
+	for (uint i=0;i<NUM_PWM_SLICES;i++) {
+		pwm_set_enabled(i, false);
+		pwm_set_counter(i, 0);
+	}
+	// enable all slices
+	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, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 0);
+	setupPWM(pinB, pwm_frequency, !SIMPLEFOC_PWM_ACTIVE_HIGH, params, 1);
+	syncSlices();
+	return params;
+}
+
+
+
+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, !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;
+}
+
+
+
+
+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, !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;
+}
+
+
+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, !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;
+}
+
+
+
+
+
+void writeDutyCycle(float val, uint slice, uint chan) {
+	pwm_set_chan_level(slice, chan, (wrapvalues[slice]+1) * val);
+}
+
+
+
+
+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]);
+	writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]);
+}
+
+
+
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params) {
+	writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]);
+	writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]);
+	writeDutyCycle(dc_c, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]);
+}
+
+
+
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params) {
+	writeDutyCycle(dc_1a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]);
+	writeDutyCycle(dc_1b, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]);
+	writeDutyCycle(dc_2a, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]);
+	writeDutyCycle(dc_2b, ((RP2040DriverParams*)params)->slice[3], ((RP2040DriverParams*)params)->chan[3]);
+}
+
+inline float swDti(float val, float dt) {
+	float ret = dt+val;
+	if (ret>1.0) ret = 1.0f;
+	return ret;
+}
+
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params) {
+	if (phase_state[0]==PhaseState::PHASE_ON || phase_state[0]==PhaseState::PHASE_HI)
+		writeDutyCycle(dc_a, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]);
+	else
+		writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[0], ((RP2040DriverParams*)params)->chan[0]);
+	if (phase_state[0]==PhaseState::PHASE_ON || phase_state[0]==PhaseState::PHASE_LO)
+		writeDutyCycle(swDti(dc_a, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]);
+	else
+		writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[1], ((RP2040DriverParams*)params)->chan[1]);
+
+	if (phase_state[1]==PhaseState::PHASE_ON || phase_state[1]==PhaseState::PHASE_HI)
+		writeDutyCycle(dc_b, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]);
+	else
+		writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[2], ((RP2040DriverParams*)params)->chan[2]);
+	if (phase_state[1]==PhaseState::PHASE_ON || phase_state[1]==PhaseState::PHASE_LO)
+		writeDutyCycle(swDti(dc_b, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[3], ((RP2040DriverParams*)params)->chan[3]);
+	else
+		writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[3], ((RP2040DriverParams*)params)->chan[3]);
+
+	if (phase_state[2]==PhaseState::PHASE_ON || phase_state[2]==PhaseState::PHASE_HI)
+		writeDutyCycle(dc_c, ((RP2040DriverParams*)params)->slice[4], ((RP2040DriverParams*)params)->chan[4]);
+	else
+		writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[4], ((RP2040DriverParams*)params)->chan[4]);
+	if (phase_state[2]==PhaseState::PHASE_ON || phase_state[2]==PhaseState::PHASE_LO)
+		writeDutyCycle(swDti(dc_c, ((RP2040DriverParams*)params)->dead_zone), ((RP2040DriverParams*)params)->slice[5], ((RP2040DriverParams*)params)->chan[5]);
+	else
+		writeDutyCycle(0.0f, ((RP2040DriverParams*)params)->slice[5], ((RP2040DriverParams*)params)->chan[5]);
+
+	_UNUSED(phase_state);
+}
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/rp2040/rp2040_mcu.h

@@ -0,0 +1,22 @@
+
+
+#pragma once
+
+#include "Arduino.h"
+
+#if defined(TARGET_RP2040)
+
+
+
+typedef struct RP2040DriverParams {
+  int pins[6];
+  uint slice[6];
+  uint chan[6];
+  long pwm_frequency;
+  float dead_zone;
+} RP2040DriverParams;
+
+
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/samd/samd21_mcu.cpp

@@ -0,0 +1,353 @@
+
+
+
+#include "./samd_mcu.h"
+
+
+#ifdef _SAMD21_
+
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for SAMD21")
+#pragma message("")
+
+
+#ifndef TCC3_CH0
+#define TCC3_CH0 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH1
+#define TCC3_CH1 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH2
+#define TCC3_CH2 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH3
+#define TCC3_CH3 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH4
+#define TCC3_CH4 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH5
+#define TCC3_CH5 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH6
+#define TCC3_CH6 NOT_ON_TIMER
+#endif
+#ifndef TCC3_CH7
+#define TCC3_CH7 NOT_ON_TIMER
+#endif
+#ifndef TC6_CH0
+#define TC6_CH0 NOT_ON_TIMER
+#endif
+#ifndef TC6_CH1
+#define TC6_CH1 NOT_ON_TIMER
+#endif
+#ifndef TC7_CH0
+#define TC7_CH0 NOT_ON_TIMER
+#endif
+#ifndef TC7_CH1
+#define TC7_CH1 NOT_ON_TIMER
+#endif
+
+
+
+#define NUM_WO_ASSOCIATIONS 48
+
+/*
+ * For SAM D21 A/B/C/D Variant Devices and SAM DA1 A/B Variant Devices
+ * Good for SAMD2xE, SAMD2xG and SAMD2xJ devices. Other SAMD21s currently not supported in arduino anyway?
+ *
+ * Note: only the pins which have timers associated are listed in this table.
+ * You can use the values from g_APinDescription.ulPort and g_APinDescription.ulPin to find the correct row in the table.
+ *
+ * See Microchip Technology datasheet DS40001882F-page 30
+ */
+struct wo_association WO_associations[] = {
+
+		{ PORTA,  0, TCC2_CH0, 		0, NOT_ON_TIMER, 	0},
+		{ PORTA,  1, TCC2_CH1, 		1, NOT_ON_TIMER, 	0},
+		{ PORTA,  2, NOT_ON_TIMER, 	0, TCC3_CH0, 		0},
+		{ PORTA,  3, NOT_ON_TIMER,	0, TCC3_CH1, 		1},
+		// PB04, PB05, PB06, PB07 - no timers
+		{ PORTB,  8, TC4_CH0, 		0, TCC3_CH6, 		6},
+		{ PORTB,  9, TC4_CH1, 		1, TCC3_CH7, 		7},
+		{ PORTA,  4, TCC0_CH0, 		0, TCC3_CH2, 		2},
+		{ PORTA,  5, TCC0_CH1, 		1, TCC3_CH3, 		3},
+		{ PORTA,  6, TCC1_CH0, 		0, TCC3_CH4, 		4},
+		{ PORTA,  7, TCC1_CH1, 		1, TCC3_CH5, 		5},
+		{ PORTA,  8, TCC0_CH0, 		0, TCC1_CH2, 		2},
+		{ PORTA,  9, TCC0_CH1, 		1, TCC1_CH3, 		3},
+		{ PORTA, 10, TCC1_CH0, 		0, TCC0_CH2, 		2},
+		{ PORTA, 11, TCC1_CH1, 		1, TCC0_CH3, 		3},
+		{ PORTB, 10, TC5_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTB, 11, TC5_CH1, 		1, TCC0_CH5, 		5},
+		{ PORTB, 12, TC4_CH0, 		0, TCC0_CH6, 		6},
+		{ PORTB, 13, TC4_CH1, 		1, TCC0_CH7, 		7},
+		{ PORTB, 14, TC5_CH0, 		0, NOT_ON_TIMER,	0},
+		{ PORTB, 15, TC5_CH1, 		1, NOT_ON_TIMER, 	0},
+		{ PORTA, 12, TCC2_CH0, 		0, TCC0_CH6, 		6},
+		{ PORTA, 13, TCC2_CH1, 		1, TCC0_CH7, 		7},
+		{ PORTA, 14, TC3_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTA, 15, TC3_CH1, 		1, TCC0_CH5, 		5},
+		{ PORTA, 16, TCC2_CH0, 		0, TCC0_CH6, 		6},
+		{ PORTA, 17, TCC2_CH1, 		1, TCC0_CH7, 		7},
+		{ PORTA, 18, TC3_CH0, 		0, TCC0_CH2, 		2},
+		{ PORTA, 19, TC3_CH1, 		1, TCC0_CH3, 		3},
+		{ PORTB, 16, TC6_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTB, 17, TC6_CH1, 		1, TCC0_CH5, 		5},
+		{ PORTA, 20, TC7_CH0, 		0, TCC0_CH6, 		6},
+		{ PORTA, 21, TC7_CH1, 		1, TCC0_CH7, 		7},
+		{ PORTA, 22, TC4_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTA, 23, TC4_CH1, 		1, TCC0_CH5, 		5},
+		{ PORTA, 24, TC5_CH0, 		0, TCC1_CH2, 		2},
+		{ PORTA, 25, TC5_CH1, 		1, TCC1_CH3, 		3},
+		{ PORTB, 22, TC7_CH0, 		0, TCC3_CH0, 		0},
+		{ PORTB, 23, TC7_CH1, 		1, TCC3_CH1, 		1},
+		{ PORTA, 27, NOT_ON_TIMER, 	0, TCC3_CH6, 		6},
+		{ PORTA, 28, NOT_ON_TIMER, 	0, TCC3_CH7, 		7},
+		{ PORTA, 30, TCC1_CH0, 		0, TCC3_CH4, 		4},
+		{ PORTA, 31, TCC1_CH1, 		1, TCC3_CH5, 		5},
+		{ PORTB, 30, TCC0_CH0, 		0, TCC1_CH2, 		2},
+		{ PORTB, 31, TCC0_CH1, 		1, TCC1_CH3, 		3},
+		{ PORTB,  0, TC7_CH0, 		0, NOT_ON_TIMER, 	0},
+		{ PORTB,  1, TC7_CH1, 		1, NOT_ON_TIMER, 	0},
+		{ PORTB,  2, TC6_CH0, 		0, TCC3_CH2, 		2},
+		{ PORTB,  3, TC6_CH1, 		1, TCC3_CH3, 		3}
+};
+wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0};
+
+
+
+struct wo_association& getWOAssociation(EPortType port, uint32_t pin) {
+	for (int i=0;i<NUM_WO_ASSOCIATIONS;i++) {
+		if (WO_associations[i].port==port && WO_associations[i].pin==pin)
+			return WO_associations[i];
+	}
+	return ASSOCIATION_NOT_FOUND;
+};
+
+
+
+EPioType getPeripheralOfPermutation(int permutation, int pin_position) {
+	return ((permutation>>pin_position)&0x01)==0x1?PIO_TIMER_ALT:PIO_TIMER;
+}
+
+
+
+
+
+void syncTCC(Tcc* TCCx) {
+  while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);       // Wait for synchronization of registers between the clock domains
+}
+
+
+
+/**
+ * Configure Clock 4 - we want all simplefoc PWMs to use the same clock. This ensures that
+ * any compatible pin combination can be used without having to worry about configuring different
+ * clocks.
+ */
+void configureSAMDClock() {
+
+	// TODO investigate using the FDPLL96M clock to get 96MHz timer clocks... this
+	//      would enable 48KHz PWM clocks, and setting the frequency between 24Khz with resolution 2000, to 48KHz with resolution 1000
+
+	if (!SAMDClockConfigured) {
+		SAMDClockConfigured = true;						// mark clock as configured
+		for (int i=0;i<TCC_INST_NUM;i++)				// mark all the TCCs as not yet configured
+			tccConfigured[i] = false;
+		REG_GCLK_GENDIV = GCLK_GENDIV_DIV(1) |          // Divide the 48MHz clock source by divisor N=1: 48MHz/1=48MHz
+						GCLK_GENDIV_ID(4);            	// Select Generic Clock (GCLK) 4
+		while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization
+
+		REG_GCLK_GENCTRL = GCLK_GENCTRL_IDC |           // Set the duty cycle to 50/50 HIGH/LOW
+						 GCLK_GENCTRL_GENEN |         	// Enable GCLK4
+						 GCLK_GENCTRL_SRC_DFLL48M |   	// Set the 48MHz clock source
+						// GCLK_GENCTRL_SRC_FDPLL |   	// Set the 96MHz clock source
+						 GCLK_GENCTRL_ID(4);          	// Select GCLK4
+		while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_DEBUG("SAMD: Configured clock...");
+#endif
+	}
+}
+
+
+
+
+/**
+ * Configure a TCC unit
+ * pwm_frequency is fixed at 24kHz for now. We could go slower, but going
+ * faster won't be possible without sacrificing resolution.
+ */
+void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) {
+
+	long pwm_resolution = (24000000) / pwm_frequency;
+	if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION) 
+		pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION;
+	if (pwm_resolution<SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION) 
+		pwm_resolution = SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION;
+	// store for later use
+	tccConfig.pwm_res = pwm_resolution;
+
+	// TODO for the moment we ignore the frequency...
+	if (!tccConfigured[tccConfig.tcc.tccn]) {
+		uint32_t GCLK_CLKCTRL_ID_ofthistcc = -1;
+		switch (tccConfig.tcc.tccn>>1) {
+		case 0: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC0_TCC1); break; //GCLK_CLKCTRL_ID_TCC0_TCC1;
+		case 1: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC2_TC3); break;  //GCLK_CLKCTRL_ID_TCC2_TC3;
+		case 2: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC4_TC5); break;   //GCLK_CLKCTRL_ID_TC4_TC5;
+		case 3: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC6_TC7); break;
+		default: return;
+		}
+
+		// Feed GCLK4 to TCC
+		REG_GCLK_CLKCTRL = (uint16_t)  GCLK_CLKCTRL_CLKEN |         // Enable GCLK4
+									   GCLK_CLKCTRL_GEN_GCLK4 |     // Select GCLK4
+									   GCLK_CLKCTRL_ID_ofthistcc;   // Feed GCLK4 to tcc
+		while (GCLK->STATUS.bit.SYNCBUSY);              			// Wait for synchronization
+
+		tccConfigured[tccConfig.tcc.tccn] = true;
+
+		if (tccConfig.tcc.tccn>=TCC_INST_NUM) {
+			Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo);
+			// disable
+			tc->COUNT8.CTRLA.bit.ENABLE = 0;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+			// unfortunately we need the 8-bit counter mode to use the PER register...
+			tc->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM ;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+			// meaning prescaler of 8, since TC-Unit has no up/down mode, and has resolution of 250 rather than 1000...
+			tc->COUNT8.CTRLA.bit.PRESCALER = TC_CTRLA_PRESCALER_DIV8_Val ;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+			// period is 250, period cannot be higher than 256!
+			tc->COUNT8.PER.reg = SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+			// initial duty cycle is 0
+			tc->COUNT8.CC[tccConfig.tcc.chan].reg = 0;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+			// enable
+			tc->COUNT8.CTRLA.bit.ENABLE = 1;
+			while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+#ifdef SIMPLEFOC_SAMD_DEBUG
+			SIMPLEFOC_DEBUG("SAMD: Initialized TC ", tccConfig.tcc.tccn);
+#endif
+		}
+		else {
+			Tcc* tcc = (Tcc*)GetTC(tccConfig.tcc.chaninfo);
+
+			uint8_t invenMask = ~(1<<tccConfig.tcc.chan);	// negate (invert) the signal if needed
+			uint8_t invenVal = negate?(1<<tccConfig.tcc.chan):0;
+			tcc->DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal;
+			syncTCC(tcc); // wait for sync
+
+			tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVEB_WAVEGENB_DSBOTH;   // Set wave form configuration
+			while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync
+
+			if (hw6pwm>0.0) {
+				tcc->WEXCTRL.vec.DTIEN |= (1<<tccConfig.tcc.chan);
+				tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1);
+				tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1);
+				syncTCC(tcc); // wait for sync
+			}
+
+			tcc->PER.reg = pwm_resolution - 1;                 // Set counter Top using the PER register
+			while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync
+
+			// set all channels to 0%
+			uint8_t chanCount = (tccConfig.tcc.tccn==1||tccConfig.tcc.tccn==2)?2:4;
+			for (int i=0;i<chanCount;i++) {
+				tcc->CC[i].reg = 0;					// start off at 0% duty cycle
+				uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+i);
+				while ( (tcc->SYNCBUSY.reg & chanbit) > 0 );
+			}
+
+			// Enable TC
+			tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz
+			while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync
+
+#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG)
+			SimpleFOCDebug::print("SAMD:     Initialized TCC ");
+			SimpleFOCDebug::print(tccConfig.tcc.tccn);
+			SimpleFOCDebug::print("-");
+			SimpleFOCDebug::print(tccConfig.tcc.chan);
+			SimpleFOCDebug::print("[");
+			SimpleFOCDebug::print(tccConfig.wo);
+			SimpleFOCDebug::print("]  pwm res ");
+			SimpleFOCDebug::print((int)pwm_resolution);
+			SimpleFOCDebug::println();
+#endif
+		}
+	}
+	else if (tccConfig.tcc.tccn<TCC_INST_NUM) {
+		// set invert bit for TCC channels in SW 6-PWM...
+		Tcc* tcc = (Tcc*)GetTC(tccConfig.tcc.chaninfo);
+
+		tcc->CTRLA.bit.ENABLE = 0;
+		while ( tcc->SYNCBUSY.bit.ENABLE == 1 );
+
+		uint8_t invenMask = ~(1<<tccConfig.tcc.chan);	// negate (invert) the signal if needed
+		uint8_t invenVal = negate?(1<<tccConfig.tcc.chan):0;
+		tcc->DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal;
+		syncTCC(tcc); // wait for sync
+
+		if (hw6pwm>0.0) {
+			tcc->WEXCTRL.vec.DTIEN |= (1<<tccConfig.tcc.chan);
+			tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1);
+			tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1);
+			syncTCC(tcc); // wait for sync
+		}
+
+		tcc->CTRLA.bit.ENABLE = 1;
+		while ( tcc->SYNCBUSY.bit.ENABLE == 1 );
+
+#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG)
+		SimpleFOCDebug::print("SAMD: (Re-)Initialized TCC ");
+		SimpleFOCDebug::print(tccConfig.tcc.tccn);
+		SimpleFOCDebug::print("-");
+		SimpleFOCDebug::print(tccConfig.tcc.chan);
+		SimpleFOCDebug::print("[");
+		SimpleFOCDebug::print(tccConfig.wo);
+		SimpleFOCDebug::print("]  pwm res ");
+		SimpleFOCDebug::print((int)pwm_resolution);
+		SimpleFOCDebug::println();
+#endif
+	}
+
+
+}
+
+
+
+
+
+void writeSAMDDutyCycle(tccConfiguration* info, float dc) {
+	uint8_t tccn = GetTCNumber(info->tcc.chaninfo);
+	uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo);
+	if (tccn<TCC_INST_NUM) {
+		Tcc* tcc = (Tcc*)GetTC(info->tcc.chaninfo);
+		// set via CC
+//		tcc->CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc);
+//		uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan);
+//		while ( (tcc->SYNCBUSY.reg & chanbit) > 0 );
+		// set via CCB
+		//while ( (tcc->SYNCBUSY.vec.CC & (0x1<<chan)) > 0 );
+		tcc->CCB[chan].reg = (uint32_t)((info->pwm_res-1) * dc);
+//		while ( (tcc->SYNCBUSY.vec.CCB & (0x1<<chan)) > 0 );
+//		tcc->STATUS.vec.CCBV |= (0x1<<chan);
+//		while ( tcc->SYNCBUSY.bit.STATUS > 0 );
+//		tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val);
+//		while ( tcc->SYNCBUSY.bit.CTRLB > 0 );
+	}
+	else {
+		Tc* tc = (Tc*)GetTC(info->tcc.chaninfo);
+		tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc);
+		while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+	}
+}
+
+
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/samd/samd51_mcu.cpp

@@ -0,0 +1,351 @@
+
+
+#include "./samd_mcu.h"
+
+
+#if defined(_SAMD51_)||defined(_SAME51_)
+
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for SAMD51/SAME51")
+#pragma message("")
+
+
+
+// expected frequency on DPLL, since we don't configure it ourselves. Typically this is the CPU frequency.
+// for custom boards or overclockers you can override it using this define.
+#ifndef SIMPLEFOC_SAMD51_DPLL_FREQ
+#define SIMPLEFOC_SAMD51_DPLL_FREQ 120000000
+#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
+
+
+
+//	TCC#   Channels   WO_NUM   Counter size   Fault   Dithering   Output matrix   DTI   SWAP   Pattern generation
+//	0         6         8         24-bit       Yes       Yes       Yes            Yes   Yes    Yes
+//	1         4         8         24-bit       Yes       Yes       Yes            Yes   Yes    Yes
+//	2         3         3         16-bit       Yes       -         Yes            -     -      -
+//	3         2         2         16-bit       Yes       -         -              -     -      -
+//	4         2         2         16-bit       Yes       -         -              -     -      -
+
+
+#define NUM_WO_ASSOCIATIONS 72
+
+struct wo_association WO_associations[] = {
+
+		{ PORTB,   9, TC4_CH1, 		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTA,   4, TC0_CH0, 		0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTA,   5, TC0_CH1, 		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTA,   6, TC1_CH0, 		0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTA,   7, TC1_CH1, 		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTC,   4, NOT_ON_TIMER, 0, TCC0_CH0, 	    0, NOT_ON_TIMER, 	0},
+		// PC05, PC06, PC07 -> no timers
+		{ PORTA,   8, TC0_CH0, 		0, TCC0_CH0,     	0, TCC1_CH0, 		4},
+		{ PORTA,   9, TC0_CH1, 		1, TCC0_CH1,     	1, TCC1_CH1, 		5},
+		{ PORTA,  10, TC1_CH0, 		0, TCC0_CH2,     	2, TCC1_CH2, 		6},
+		{ PORTA,  11, TC1_CH1, 		1, TCC0_CH3,     	3, TCC1_CH3, 		7},
+		{ PORTB,  10, TC5_CH0, 		0, TCC0_CH4, 		4, TCC1_CH0, 		0},
+		{ PORTB,  11, TC5_CH1, 		1, TCC0_CH5,  		5, TCC1_CH1, 		1},
+		{ PORTB,  12, TC4_CH0, 		0, TCC3_CH0, 		0, TCC0_CH0, 		0},
+		{ PORTB,  13, TC4_CH1, 		1, TCC3_CH1, 		1, TCC0_CH1, 		1},
+		{ PORTB,  14, TC5_CH0, 		0, TCC4_CH0, 		0, TCC0_CH2, 		2},
+		{ PORTB,  15, TC5_CH1, 		1, TCC4_CH1, 		1, TCC0_CH3, 		3},
+		{ PORTD,   8, NOT_ON_TIMER,	0, TCC0_CH1,     	1, NOT_ON_TIMER, 	0},
+		{ PORTD,   9, NOT_ON_TIMER,	0, TCC0_CH2,     	2, NOT_ON_TIMER, 	0},
+		{ PORTD,  10, NOT_ON_TIMER,	0, TCC0_CH3,     	3, NOT_ON_TIMER, 	0},
+		{ PORTD,  11, NOT_ON_TIMER,	0, TCC0_CH4, 		4, NOT_ON_TIMER, 	0},
+		{ PORTD,  12, NOT_ON_TIMER,	0, TCC0_CH5,  		5, NOT_ON_TIMER, 	0},
+		{ PORTC,  10, NOT_ON_TIMER,	0, TCC0_CH0,     	0, TCC1_CH0, 		4},
+		{ PORTC,  11, NOT_ON_TIMER,	0, TCC0_CH1,     	1, TCC1_CH1, 		5},
+		{ PORTC,  12, NOT_ON_TIMER,	0, TCC0_CH2, 		2, TCC1_CH2, 		6},
+		{ PORTC,  13, NOT_ON_TIMER,	0, TCC0_CH3, 		3, TCC1_CH3, 		7},
+		{ PORTC,  14, NOT_ON_TIMER,	0, TCC0_CH4, 		4, TCC1_CH0, 		0},
+		{ PORTC,  15, NOT_ON_TIMER,	0, TCC0_CH5, 		5, TCC1_CH1, 		1},
+		{ PORTA,  12, TC2_CH0,		0, TCC0_CH0, 		6, TCC1_CH2, 		2},
+		{ PORTA,  13, TC2_CH1,		1, TCC0_CH1, 		7, TCC1_CH3, 		3},
+		{ PORTA,  14, TC3_CH0,		0, TCC2_CH0, 		0, TCC1_CH2, 		2},
+		{ PORTA,  15, TC3_CH1,		1, TCC2_CH1, 		1, TCC1_CH3, 		3},
+		{ PORTA,  16, TC2_CH0,		0, TCC1_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTA,  17, TC2_CH1,		1, TCC1_CH1, 		1, TCC0_CH5, 		5},
+		{ PORTA,  18, TC3_CH0,		0, TCC1_CH2, 		2, TCC0_CH0, 		6},
+		{ PORTA,  19, TC3_CH1,		1, TCC1_CH3, 		3, TCC0_CH1, 		7},
+		{ PORTC,  16, NOT_ON_TIMER,	0, TCC0_CH0, 		0, NOT_ON_TIMER, 	0}, // PDEC0
+		{ PORTC,  17, NOT_ON_TIMER,	0, TCC0_CH1, 		1, NOT_ON_TIMER, 	0}, // PDEC1
+		{ PORTC,  18, NOT_ON_TIMER,	0, TCC0_CH2, 		2, NOT_ON_TIMER, 	0}, // PDEC2
+		{ PORTC,  19, NOT_ON_TIMER,	0, TCC0_CH3, 		3, NOT_ON_TIMER, 	0},
+		{ PORTC,  20, NOT_ON_TIMER,	0, TCC0_CH4, 		4, NOT_ON_TIMER, 	0},
+		{ PORTC,  21, NOT_ON_TIMER,	0, TCC0_CH5, 		5, NOT_ON_TIMER, 	0},
+		{ PORTC,  22, NOT_ON_TIMER,	0, TCC0_CH0, 		6, NOT_ON_TIMER, 	0},
+		{ PORTC,  23, NOT_ON_TIMER,	0, TCC0_CH1, 		7, NOT_ON_TIMER, 	0},
+		{ PORTD,  20, NOT_ON_TIMER,	0, TCC1_CH0, 		0, NOT_ON_TIMER, 	0},
+		{ PORTD,  21, NOT_ON_TIMER,	0, TCC1_CH1, 		1, NOT_ON_TIMER, 	0},
+		{ PORTB,  16, TC6_CH0,		0, TCC3_CH0, 		0, TCC0_CH4, 		4},
+		{ PORTB,  17, TC6_CH1,		1, TCC3_CH1, 		1, TCC0_CH5,	 	5},
+		{ PORTB,  18, NOT_ON_TIMER,	0, TCC1_CH0, 		0, NOT_ON_TIMER, 	0}, // PDEC0
+		{ PORTB,  19, NOT_ON_TIMER,	0, TCC1_CH1, 		1, NOT_ON_TIMER, 	0}, // PDEC1
+		{ PORTB,  20, NOT_ON_TIMER,	0, TCC1_CH2, 		2, NOT_ON_TIMER, 	0}, // PDEC2
+		{ PORTB,  21, NOT_ON_TIMER,	0, TCC1_CH3, 		3, NOT_ON_TIMER, 	0},
+		{ PORTA,  20, TC7_CH0,		0, TCC1_CH0, 		4, TCC0_CH0, 		0},
+		{ PORTA,  21, TC7_CH1,		1, TCC1_CH1, 		5, TCC0_CH1, 		1},
+		{ PORTA,  22, TC4_CH0,		0, TCC1_CH2, 		6, TCC0_CH2, 		2},
+		{ PORTA,  23, TC4_CH1,		1, TCC1_CH3, 		7, TCC0_CH3, 		3},
+		{ PORTA,  24, TC5_CH0,		0, TCC2_CH2, 		2, NOT_ON_TIMER, 	0}, // PDEC0
+		{ PORTA,  25, TC5_CH1,		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0}, // PDEC1
+		{ PORTB,  22, TC7_CH0,		0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0}, // PDEC2
+		{ PORTB,  23, TC7_CH1,		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER,	0}, // PDEC0
+		{ PORTB,  24, NOT_ON_TIMER,	0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0}, // PDEC1
+		{ PORTB,  25, NOT_ON_TIMER,	0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0}, // PDEC2
+		{ PORTB,  26, NOT_ON_TIMER,	0, TCC1_CH2, 		2, NOT_ON_TIMER, 	0},
+		{ PORTB,  27, NOT_ON_TIMER,	0, TCC1_CH3, 		3, NOT_ON_TIMER, 	0},
+		{ PORTB,  28, NOT_ON_TIMER,	0, TCC1_CH0, 		4, NOT_ON_TIMER, 	0},
+		{ PORTB,  29, NOT_ON_TIMER,	0, TCC1_CH1, 		5, NOT_ON_TIMER,	0},
+		// PC24-PC28, PA27, RESET -> no TC/TCC peripherals
+		{ PORTA,  30, TC6_CH0,		0, TCC2_CH0, 		0, NOT_ON_TIMER, 	0},
+		{ PORTA,  31, TC6_CH1,		1, TCC2_CH1, 		1, NOT_ON_TIMER, 	0},
+		{ PORTB,  30, TC0_CH0,		0, TCC4_CH0, 		0, TCC0_CH0, 		6},
+		{ PORTB,  31, TC0_CH1,		1, TCC4_CH1, 		1, TCC0_CH1, 		7},
+		// PC30, PC31 -> no TC/TCC peripherals
+		{ PORTB,   0, TC7_CH0,		0, NOT_ON_TIMER, 	0, NOT_ON_TIMER, 	0},
+		{ PORTB,   1, TC7_CH1,		1, NOT_ON_TIMER, 	0, NOT_ON_TIMER,	0},
+		{ PORTB,   2, TC6_CH0,		0, TCC2_CH2, 		2, NOT_ON_TIMER,	0},
+
+};
+
+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;i<NUM_WO_ASSOCIATIONS;i++) {
+		if (WO_associations[i].port==port && WO_associations[i].pin==pin)
+			return WO_associations[i];
+	}
+	return ASSOCIATION_NOT_FOUND;
+};
+
+
+EPioType getPeripheralOfPermutation(int permutation, int pin_position) {
+	return ((permutation>>pin_position)&0x01)==0x1?PIO_TCC_PDEC:PIO_TIMER_ALT;
+}
+
+
+
+void syncTCC(Tcc* TCCx) {
+	while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK);
+}
+
+
+
+
+void writeSAMDDutyCycle(tccConfiguration* info, float dc) {
+	uint8_t tccn = GetTCNumber(info->tcc.chaninfo);
+	uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo);
+	if (tccn<TCC_INST_NUM) {
+		Tcc* tcc = (Tcc*)GetTC(info->tcc.chaninfo);
+		// set via CCBUF
+//		while ( (tcc->SYNCBUSY.vec.CC & (0x1<<chan)) > 0 );
+		tcc->CCBUF[chan].reg = (uint32_t)((info->pwm_res-1) * dc); // TODO pwm frequency!
+	}
+	else { 
+		// we don't support the TC channels on SAMD51, isn't worth it.
+	}
+}
+
+
+#define DPLL_CLOCK_NUM 2  	// use GCLK2
+#define PWM_CLOCK_NUM 3  	// use GCLK3
+
+
+/**
+ * Configure Clock 4 - we want all simplefoc PWMs to use the same clock. This ensures that
+ * any compatible pin combination can be used without having to worry about configuring different
+ * clocks.
+ */
+void configureSAMDClock() {
+	if (!SAMDClockConfigured) {
+		SAMDClockConfigured = true;						// mark clock as configured
+		for (int i=0;i<TCC_INST_NUM;i++)				// mark all the TCCs as not yet configured
+			tccConfigured[i] = false;
+
+		// GCLK->GENCTRL[DPLL_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1)
+		// 								  | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val);
+		// while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<DPLL_CLOCK_NUM));
+
+		// GCLK->PCHCTRL[1].reg = GCLK_PCHCTRL_GEN(DPLL_CLOCK_NUM)|GCLK_PCHCTRL_CHEN;
+		// while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<DPLL_CLOCK_NUM));
+
+		// OSCCTRL->Dpll[0].DPLLCTRLA.bit.ENABLE = 0;
+		// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
+
+		// OSCCTRL->Dpll[0].DPLLCTRLB.bit.REFCLK = OSCCTRL_DPLLCTRLB_REFCLK_GCLK_Val;
+		// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
+		// OSCCTRL->Dpll[0].DPLLRATIO.reg = 3;
+		// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
+
+		// OSCCTRL->Dpll[0].DPLLCTRLA.bit.ENABLE = 1;
+		// while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0);
+
+		GCLK->GENCTRL[PWM_CLOCK_NUM].bit.GENEN = 0;
+		while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<PWM_CLOCK_NUM));
+
+		GCLK->GENCTRL[PWM_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1) | GCLK_GENCTRL_IDC
+										 //| GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val);
+	 	 	 	 	 	 	 	 	 	 | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DPLL0_Val);
+		while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<PWM_CLOCK_NUM));
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_DEBUG("SAMD: Configured clock...");
+#endif
+	}
+}
+
+
+
+
+
+/**
+ * Configure a TCC unit
+ * pwm_frequency is fixed at 24kHz for now. We could go slower, but going
+ * faster won't be possible without sacrificing resolution.
+ */
+void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) {
+	// TODO for the moment we ignore the frequency...
+	if (!tccConfigured[tccConfig.tcc.tccn]) {
+		uint32_t GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_IDs[tccConfig.tcc.tccn];
+		GCLK->PCHCTRL[GCLK_CLKCTRL_ID_ofthistcc].reg = GCLK_PCHCTRL_GEN(PWM_CLOCK_NUM)|GCLK_PCHCTRL_CHEN;
+		while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<<PWM_CLOCK_NUM));
+	}
+
+	if (tccConfig.tcc.tccn<TCC_INST_NUM) {
+		Tcc* tcc = (Tcc*)GetTC(tccConfig.tcc.chaninfo);
+
+		tcc->CTRLA.bit.ENABLE = 0; //switch off tcc
+		while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync
+
+		uint8_t invenMask = ~(1<<tccConfig.tcc.chan);	// negate (invert) the signal if needed
+		uint8_t invenVal = negate?(1<<tccConfig.tcc.chan):0;
+		tcc->DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal;
+		syncTCC(tcc); // wait for sync
+
+		// work out pwm resolution for desired frequency and constrain to max/min values
+		long pwm_resolution = (SIMPLEFOC_SAMD51_DPLL_FREQ/2) / pwm_frequency;
+		if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION) 
+			pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION;
+		if (pwm_resolution<SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION) 
+			pwm_resolution = SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION;
+		// store for later use
+		tccConfig.pwm_res = pwm_resolution;
+
+		if (hw6pwm>0.0) {
+			tcc->WEXCTRL.vec.DTIEN |= (1<<tccConfig.tcc.chan);
+			tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1);
+			tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1);
+			syncTCC(tcc); // wait for sync
+		}
+
+		if (!tccConfigured[tccConfig.tcc.tccn]) {
+			tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVE_WAVEGEN_DSTOP;   // Set wave form configuration - TODO check this... why set like this?
+			while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync
+
+			tcc->PER.reg = pwm_resolution - 1;                 // Set counter Top using the PER register
+			while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync
+
+			// set all channels to 0%
+			for (int i=0;i<TCC_CHANNEL_COUNT[tccConfig.tcc.tccn];i++) {
+				tcc->CC[i].reg = 0;					// start off at 0% duty cycle
+				uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+i);
+				while ( (tcc->SYNCBUSY.reg & chanbit) > 0 );
+			}
+		}
+
+		// Enable TCC
+		tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz
+		while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync
+
+#if defined(SIMPLEFOC_SAMD_DEBUG) && !defined(SIMPLEFOC_DISABLE_DEBUG)
+		SimpleFOCDebug::print("SAMD: (Re-)Initialized TCC ");
+		SimpleFOCDebug::print(tccConfig.tcc.tccn);
+		SimpleFOCDebug::print("-");
+		SimpleFOCDebug::print(tccConfig.tcc.chan);
+		SimpleFOCDebug::print("[");
+		SimpleFOCDebug::print(tccConfig.wo);
+		SimpleFOCDebug::print("]  pwm res ");
+		SimpleFOCDebug::print((int)pwm_resolution);
+		SimpleFOCDebug::println();	
+#endif
+	}
+	else if (tccConfig.tcc.tccn>=TCC_INST_NUM) {
+		//Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo);
+
+		// disable
+		// tc->COUNT8.CTRLA.bit.ENABLE = 0;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+		// // unfortunately we need the 8-bit counter mode to use the PER register...
+		// tc->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM ;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+		// // meaning prescaler of 8, since TC-Unit has no up/down mode, and has resolution of 250 rather than 1000...
+		// tc->COUNT8.CTRLA.bit.PRESCALER = TC_CTRLA_PRESCALER_DIV8_Val ;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+		// // period is 250, period cannot be higher than 256!
+		// tc->COUNT8.PER.reg = SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+		// // initial duty cycle is 0
+		// tc->COUNT8.CC[tccConfig.tcc.chan].reg = 0;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+		// // enable
+		// tc->COUNT8.CTRLA.bit.ENABLE = 1;
+		// while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 );
+
+	#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_DEBUG("SAMD: Not initialized: TC ", tccConfig.tcc.tccn);
+		SIMPLEFOC_DEBUG("SAMD: TC units not supported on SAMD51");
+	#endif
+	}
+
+	// set as configured
+	tccConfigured[tccConfig.tcc.tccn] = true;
+
+
+}
+
+
+
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/samd/samd_mcu.cpp

@@ -0,0 +1,914 @@
+
+
+
+#include "./samd_mcu.h"
+
+#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_)
+
+
+
+/**
+ * Global state
+ */
+tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS];
+uint8_t numTccPinConfigurations = 0;
+bool SAMDClockConfigured = false;
+bool tccConfigured[TCC_INST_NUM+TC_INST_NUM];
+
+
+
+
+
+/**
+ * Attach the TCC to the pin
+ */
+bool attachTCC(tccConfiguration& tccConfig) {
+	if (numTccPinConfigurations>=SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS)
+		return false;
+    pinMode(tccConfig.pin, OUTPUT);
+
+    pinPeripheral(tccConfig.pin, tccConfig.peripheral);
+    tccPinConfigurations[numTccPinConfigurations++] = tccConfig;
+    return true;
+}
+
+
+
+
+
+int getPermutationNumber(int pins) {
+	int num = 1;
+	for (int i=0;i<pins;i++)
+		num *= NUM_PIO_TIMER_PERIPHERALS;
+	return num;
+}
+
+
+
+
+/**
+ * Check if the configuration is in use already.
+ */
+bool inUse(tccConfiguration& tccConfig) {
+	for (int i=0;i<numTccPinConfigurations;i++) {
+		if (tccPinConfigurations[i].tcc.chaninfo==tccConfig.tcc.chaninfo)
+			return true;
+	}
+	return false;
+}
+
+
+tccConfiguration* getTccPinConfiguration(uint8_t pin) {
+	for (int i=0;i<numTccPinConfigurations;i++)
+		if (tccPinConfigurations[i].pin==pin)
+			return &tccPinConfigurations[i];
+	return NULL;
+}
+
+
+
+
+
+/**
+ * Get the TCC channel associated with that pin
+ */
+tccConfiguration getTCCChannelNr(int pin, EPioType peripheral) {
+	tccConfiguration result;
+	result.pin = pin;
+	result.peripheral  = peripheral;
+	result.tcc.tccn = -2;
+	result.tcc.chan = -2;
+	const PinDescription& pinDesc = g_APinDescription[pin];
+	struct wo_association& association = getWOAssociation(pinDesc.ulPort, pinDesc.ulPin);
+	if (association.port==NOT_A_PORT)
+		return result; // could not find the port/pin
+	if (peripheral==PIO_TIMER) {
+		result.tcc.chaninfo = association.tccE;
+		result.wo = association.woE;
+	}
+	else if (peripheral==PIO_TIMER_ALT) {
+		result.tcc.chaninfo = association.tccF;
+		result.wo = association.woF;
+	}
+#if defined(_SAMD51_)||defined(_SAME51_)
+	else if (peripheral==PIO_TCC_PDEC) {
+		result.tcc.chaninfo = association.tccG;
+		result.wo = association.woG;
+	}
+#endif
+	return result;
+}
+
+
+
+
+
+bool checkPeripheralPermutationSameTCC6(tccConfiguration& pinAh, tccConfiguration& pinAl, tccConfiguration& pinBh, tccConfiguration& pinBl, tccConfiguration& pinCh, tccConfiguration& pinCl) {
+	if (inUse(pinAh) || inUse(pinAl) || inUse(pinBh) || inUse(pinBl) || inUse(pinCh) || inUse(pinCl))
+		return false;
+
+	if (pinAh.tcc.tccn<0 || pinAh.tcc.tccn!=pinAl.tcc.tccn || pinAh.tcc.tccn!=pinBh.tcc.tccn || pinAh.tcc.tccn!=pinBl.tcc.tccn
+		|| pinAh.tcc.tccn!=pinCh.tcc.tccn || pinAh.tcc.tccn!=pinCl.tcc.tccn || pinAh.tcc.tccn>=TCC_INST_NUM)
+		return false;
+
+	if (pinAh.tcc.chan==pinBh.tcc.chan || pinAh.tcc.chan==pinBl.tcc.chan || pinAh.tcc.chan==pinCh.tcc.chan || pinAh.tcc.chan==pinCl.tcc.chan)
+		return false;
+	if (pinBh.tcc.chan==pinCh.tcc.chan || pinBh.tcc.chan==pinCl.tcc.chan)
+		return false;
+	if (pinAl.tcc.chan==pinBh.tcc.chan || pinAl.tcc.chan==pinBl.tcc.chan || pinAl.tcc.chan==pinCh.tcc.chan || pinAl.tcc.chan==pinCl.tcc.chan)
+		return false;
+	if (pinBl.tcc.chan==pinCh.tcc.chan || pinBl.tcc.chan==pinCl.tcc.chan)
+		return false;
+
+	if (pinAh.tcc.chan!=pinAl.tcc.chan || pinBh.tcc.chan!=pinBl.tcc.chan || pinCh.tcc.chan!=pinCl.tcc.chan)
+		return false;
+	if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo!=pinCl.wo)
+		return false;
+
+	return true;
+}
+
+
+
+
+bool checkPeripheralPermutationCompatible(tccConfiguration pins[], uint8_t num) {
+	for (int i=0;i<num;i++)
+		if (pins[i].tcc.tccn<0)
+			return false;
+	for (int i=0;i<num-1;i++)
+		for (int j=i+1;j<num;j++)
+			if (pins[i].tcc.chaninfo==pins[j].tcc.chaninfo)
+				return false;
+	for (int i=0;i<num;i++)
+		if (inUse(pins[i]))
+			return false;
+	return true;
+}
+
+
+
+
+
+bool checkPeripheralPermutationCompatible6(tccConfiguration& pinAh, tccConfiguration& pinAl, tccConfiguration& pinBh, tccConfiguration& pinBl, tccConfiguration& pinCh, tccConfiguration& pinCl) {
+	// check we're valid PWM pins
+	if (pinAh.tcc.tccn<0 || pinAl.tcc.tccn<0 || pinBh.tcc.tccn<0 || pinBl.tcc.tccn<0 || pinCh.tcc.tccn<0 || pinCl.tcc.tccn<0)
+		return false;
+	// only TCC units for 6-PWM
+	if (pinAh.tcc.tccn>=TCC_INST_NUM || pinAl.tcc.tccn>=TCC_INST_NUM || pinBh.tcc.tccn>=TCC_INST_NUM
+			|| pinBl.tcc.tccn>=TCC_INST_NUM || pinCh.tcc.tccn>=TCC_INST_NUM || pinCl.tcc.tccn>=TCC_INST_NUM)
+		return false;
+
+	// check we're not in use
+	if (inUse(pinAh) || inUse(pinAl) || inUse(pinBh) || inUse(pinBl) || inUse(pinCh) || inUse(pinCl))
+		return false;
+
+	// check pins are all different tccs/channels
+	if (pinAh.tcc.chaninfo==pinBh.tcc.chaninfo || pinAh.tcc.chaninfo==pinBl.tcc.chaninfo || pinAh.tcc.chaninfo==pinCh.tcc.chaninfo || pinAh.tcc.chaninfo==pinCl.tcc.chaninfo)
+		return false;
+	if (pinAl.tcc.chaninfo==pinBh.tcc.chaninfo || pinAl.tcc.chaninfo==pinBl.tcc.chaninfo || pinAl.tcc.chaninfo==pinCh.tcc.chaninfo || pinAl.tcc.chaninfo==pinCl.tcc.chaninfo)
+		return false;
+	if (pinBh.tcc.chaninfo==pinCh.tcc.chaninfo || pinBh.tcc.chaninfo==pinCl.tcc.chaninfo)
+		return false;
+	if (pinBl.tcc.chaninfo==pinCh.tcc.chaninfo || pinBl.tcc.chaninfo==pinCl.tcc.chaninfo)
+		return false;
+
+	// check H/L pins are on same timer
+	if (pinAh.tcc.tccn!=pinAl.tcc.tccn || pinBh.tcc.tccn!=pinBl.tcc.tccn || pinCh.tcc.tccn!=pinCl.tcc.tccn)
+		return false;
+
+	// check H/L pins aren't on both the same timer and wo
+	if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo==pinCl.wo)
+		return false;
+
+	return true;
+}
+
+
+
+
+
+int checkHardware6PWM(const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
+	for (int i=0;i<64;i++) {
+		tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(i, 0));
+		tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(i, 1));
+		tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(i, 2));
+		tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(i, 3));
+		tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(i, 4));
+		tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(i, 5));
+		if (checkPeripheralPermutationSameTCC6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl))
+			return i;
+	}
+	return -1;
+}
+
+
+
+
+int checkSoftware6PWM(const int pinA_h, const int pinA_l,  const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) {
+	for (int i=0;i<64;i++) {
+		tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(i, 0));
+		tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(i, 1));
+		tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(i, 2));
+		tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(i, 3));
+		tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(i, 4));
+		tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(i, 5));
+		if (checkPeripheralPermutationCompatible6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl))
+			return i;
+	}
+	return -1;
+}
+
+
+
+int scorePermutation(tccConfiguration pins[], uint8_t num) {
+	uint32_t usedtccs = 0;
+	for (int i=0;i<num;i++)
+		usedtccs |= (1<<pins[i].tcc.tccn);
+	int score = 0;
+	for (int i=0;i<TCC_INST_NUM;i++){
+		if (usedtccs&0x1)
+			score+=1;
+		usedtccs = usedtccs>>1;
+	}
+	for (int i=0;i<TC_INST_NUM;i++){
+		if (usedtccs&0x1)
+			score+=(num+1);
+		usedtccs = usedtccs>>1;
+	}
+	return score;
+}
+
+
+
+
+
+
+
+
+int checkPermutations(uint8_t num, int pins[], bool (*checkFunc)(tccConfiguration[], uint8_t) ) {
+	tccConfiguration tccConfs[num];
+	int best = -1;
+	int bestscore = 1000000;
+	for (int i=0;i<(0x1<<num);i++) {
+		for (int j=0;j<num;j++)
+			tccConfs[j] = getTCCChannelNr(pins[j], getPeripheralOfPermutation(i, j));
+		if (checkFunc(tccConfs, num)) {
+			int score = scorePermutation(tccConfs, num);
+			if (score<bestscore) {
+				bestscore = score;
+				best = i;
+			}
+		}
+	}
+	return best;
+}
+
+
+
+
+
+
+
+/**
+ * Configuring PWM frequency, resolution and alignment
+ * - Stepper driver - 2PWM setting
+ * - hardware specific
+ *
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pinA pinA bldc driver
+ * @param pinB pinB bldc driver
+ */
+void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	printAllPinInfos();
+#endif
+	int pins[2] = { pinA, pinB };
+	int compatibility = checkPermutations(2, pins, checkPeripheralPermutationCompatible);
+	if (compatibility<0) {
+		// no result!
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_DEBUG("SAMD: Bad pin combination!");
+#endif
+		return SIMPLEFOC_DRIVER_INIT_FAILED;
+	}
+
+	tccConfiguration tccConfs[2] = { getTCCChannelNr(pinA, getPeripheralOfPermutation(compatibility, 0)),
+								     getTCCChannelNr(pinB, getPeripheralOfPermutation(compatibility, 1)) };
+
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 2));
+	printTCCConfiguration(tccConfs[0]);
+	printTCCConfiguration(tccConfs[1]);
+#endif
+
+	// attach pins to timer peripherals
+	attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it...
+	attachTCC(tccConfs[1]);
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Attached pins...");
+#endif
+
+	// set up clock - Note: if we did this right it should be possible to get all TCC units synchronized?
+	// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
+	configureSAMDClock();
+
+	if (pwm_frequency==NOT_SET) {
+		// use default frequency
+		pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
+	}
+
+	// configure the TCC (waveform, top-value, pre-scaler = frequency)
+	configureTCC(tccConfs[0], pwm_frequency);
+	configureTCC(tccConfs[1], pwm_frequency);
+	getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res;
+	getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res;
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Configured TCCs...");
+#endif
+
+	SAMDHardwareDriverParams* params = new SAMDHardwareDriverParams {
+		.tccPinConfigurations = { getTccPinConfiguration(pinA), getTccPinConfiguration(pinB) },
+		.pwm_frequency = (uint32_t)pwm_frequency
+	}; // Someone with a stepper-setup who can test it?
+	return params;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+/**
+ * Configuring PWM frequency, resolution and alignment
+ * - BLDC driver - 3PWM setting
+ * - hardware specific
+ *
+ * SAMD21 will support up to 2 BLDC motors in 3-PWM:
+ *  one on TCC0 using 3 of the channels 0,1,2 or 3
+ *  one on TCC3 using 3 of the channels 0,1,2 or 3
+ * i.e. 8 different pins can be used, but only 4 different signals (WO[x]) on those 8 pins
+ *  WO[0] and WO[4] are the same
+ *  WO[1] and WO[5] are the same
+ *  WO[2] and WO[6] are the same
+ *  WO[3] and WO[7] are the same
+ *
+ * If you're on the Arduino Nano33 IoT, please see the Nano33 IoT pinout diagram to see which TCC0/WO[x]
+ * signal is on which pin of the Nano. You can drive one motor on TCC0. For other boards, consult their documentation.
+ *
+ * Note:
+ * That's if we want to keep the signals strictly in sync.
+ *
+ * If we can accept out-of-sync PWMs on the different phases, we could drive up to 4 BLDCs in 3-PWM mode,
+ * using all the TCC channels. (TCC0 & TCC3 - 4 channels each, TCC1 & TCC2 - 2 channels each)
+ *
+ * All channels will use the same resolution, prescaler and clock, but they will have different start-times leading
+ * to misaligned signals.
+ *
+ *
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pinA pinA bldc driver
+ * @param pinB pinB bldc driver
+ * @param pinC pinC bldc driver
+ */
+void* _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) {
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	printAllPinInfos();
+#endif
+	int pins[3] = { pinA, pinB, pinC };
+	int compatibility = checkPermutations(3, pins, checkPeripheralPermutationCompatible);
+	if (compatibility<0) {
+		// no result!
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_DEBUG("SAMD: Bad pin combination!");
+#endif
+		return SIMPLEFOC_DRIVER_INIT_FAILED;
+	}
+
+	tccConfiguration tccConfs[3] = { getTCCChannelNr(pinA, getPeripheralOfPermutation(compatibility, 0)),
+									 getTCCChannelNr(pinB, getPeripheralOfPermutation(compatibility, 1)),
+								     getTCCChannelNr(pinC, getPeripheralOfPermutation(compatibility, 2)) };
+
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 3));
+	printTCCConfiguration(tccConfs[0]);
+	printTCCConfiguration(tccConfs[1]);
+	printTCCConfiguration(tccConfs[2]);
+#endif
+
+	// attach pins to timer peripherals
+	attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it...
+	attachTCC(tccConfs[1]);
+	attachTCC(tccConfs[2]);
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Attached pins...");
+#endif
+
+	// set up clock - Note: if we did this right it should be possible to get all TCC units synchronized?
+	// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
+	configureSAMDClock();
+
+	if (pwm_frequency==NOT_SET) {
+		// use default frequency
+		pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
+	}
+
+	// configure the TCC (waveform, top-value, pre-scaler = frequency)
+	configureTCC(tccConfs[0], pwm_frequency);
+	configureTCC(tccConfs[1], pwm_frequency);
+	configureTCC(tccConfs[2], pwm_frequency);
+	getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res;
+	getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res;
+	getTccPinConfiguration(pinC)->pwm_res = tccConfs[2].pwm_res;
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Configured TCCs...");
+#endif
+
+	return new SAMDHardwareDriverParams {
+		.tccPinConfigurations = { getTccPinConfiguration(pinA), getTccPinConfiguration(pinB), getTccPinConfiguration(pinC) },
+		.pwm_frequency = (uint32_t)pwm_frequency
+	};
+
+}
+
+
+
+
+
+
+
+
+/**
+ * Configuring PWM frequency, resolution and alignment
+ * - Stepper driver - 4PWM setting
+ * - hardware specific
+ *
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param pin1A pin1A stepper driver
+ * @param pin1B pin1B stepper driver
+ * @param pin2A pin2A stepper driver
+ * @param pin2B pin2B stepper driver
+ */
+void* _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) {
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	printAllPinInfos();
+#endif
+	int pins[4] = { pin1A, pin1B, pin2A, pin2B };
+	int compatibility = checkPermutations(4, pins, checkPeripheralPermutationCompatible);
+	if (compatibility<0) {
+		// no result!
+#ifdef SIMPLEFOC_SAMD_DEBUG
+		SIMPLEFOC_DEBUG("SAMD: Bad pin combination!");
+#endif
+		return SIMPLEFOC_DRIVER_INIT_FAILED;
+	}
+
+	tccConfiguration tccConfs[4] = { getTCCChannelNr(pin1A, getPeripheralOfPermutation(compatibility, 0)),
+									 getTCCChannelNr(pin1B, getPeripheralOfPermutation(compatibility, 1)),
+									 getTCCChannelNr(pin2A, getPeripheralOfPermutation(compatibility, 2)),
+									 getTCCChannelNr(pin2B, getPeripheralOfPermutation(compatibility, 3)) };
+
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Found configuration: score=", scorePermutation(tccConfs, 4));
+	printTCCConfiguration(tccConfs[0]);
+	printTCCConfiguration(tccConfs[1]);
+	printTCCConfiguration(tccConfs[2]);
+	printTCCConfiguration(tccConfs[3]);
+#endif
+
+	// attach pins to timer peripherals
+	attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it...
+	attachTCC(tccConfs[1]);
+	attachTCC(tccConfs[2]);
+	attachTCC(tccConfs[3]);
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Attached pins...");
+#endif
+
+	// set up clock - Note: if we did this right it should be possible to get all TCC units synchronized?
+	// e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC...
+	configureSAMDClock();
+
+	if (pwm_frequency==NOT_SET) {
+		// use default frequency
+		pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
+	}
+
+	// configure the TCC (waveform, top-value, pre-scaler = frequency)
+	configureTCC(tccConfs[0], pwm_frequency);
+	configureTCC(tccConfs[1], pwm_frequency);
+	configureTCC(tccConfs[2], pwm_frequency);
+	configureTCC(tccConfs[3], pwm_frequency);
+	getTccPinConfiguration(pin1A)->pwm_res = tccConfs[0].pwm_res;
+	getTccPinConfiguration(pin2A)->pwm_res = tccConfs[1].pwm_res;
+	getTccPinConfiguration(pin1B)->pwm_res = tccConfs[2].pwm_res;
+	getTccPinConfiguration(pin2B)->pwm_res = tccConfs[3].pwm_res;
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Configured TCCs...");
+#endif
+
+	return new SAMDHardwareDriverParams {
+		.tccPinConfigurations = { getTccPinConfiguration(pin1A), getTccPinConfiguration(pin1B), getTccPinConfiguration(pin2A), getTccPinConfiguration(pin2B) },
+		.pwm_frequency = (uint32_t)pwm_frequency
+	};
+}
+
+
+
+
+
+
+
+
+
+/**
+ * Configuring PWM frequency, resolution and alignment
+ * - BLDC driver - 6PWM setting
+ * - hardware specific
+ *
+ * SAMD21 will support up to 2 BLDC motors in 6-PWM:
+ *  one on TCC0 using 3 of the channels 0,1,2 or 3
+ *  one on TCC3 using 3 of the channels 0,1,2 or 3
+ * i.e. 6 out of 8 pins must be used, in the following high/low side pairs:
+ *  WO[0] & WO[4]  (high side & low side)
+ *  WO[1] & WO[5]
+ *  WO[2] & WO[6]
+ *  WO[3] & WO[7]
+ *
+ * If you're on the Arduino Nano33 IoT, please see the Nano33 IoT pinout diagram to see which TCC0/WO[x]
+ * signal is on which pin of the Nano. You can drive 1 BLDC on TCC0. For other boards, consult their documentation.
+ *
+ *
+ * @param pwm_frequency - frequency in hertz - if applicable
+ * @param dead_zone  duty cycle protection zone [0, 1] - both low and high side low - if applicable
+ * @param pinA_h pinA high-side bldc driver
+ * @param pinA_l pinA low-side bldc driver
+ * @param pinB_h pinA high-side bldc driver
+ * @param pinB_l pinA low-side bldc driver
+ * @param pinC_h pinA high-side bldc driver
+ * @param pinC_l pinA low-side bldc driver
+ *
+ * @return 0 if config good, -1 if failed
+ */
+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) {
+	// we want to use a TCC channel with 1 non-inverted and 1 inverted output for each phase, with dead-time insertion
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	printAllPinInfos();
+#endif
+	int compatibility = checkHardware6PWM(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
+	if (compatibility<0) {
+		compatibility = checkSoftware6PWM(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l);
+		if (compatibility<0) {
+			// no result!
+#ifdef SIMPLEFOC_SAMD_DEBUG
+			SIMPLEFOC_DEBUG("SAMD: Bad pin combination!");
+#endif
+			return SIMPLEFOC_DRIVER_INIT_FAILED;
+		}
+	}
+
+	tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(compatibility, 0));
+	tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(compatibility, 1));
+	tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(compatibility, 2));
+	tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(compatibility, 3));
+	tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(compatibility, 4));
+	tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(compatibility, 5));
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Found configuration: ");
+	printTCCConfiguration(pinAh);
+	printTCCConfiguration(pinAl);
+	printTCCConfiguration(pinBh);
+	printTCCConfiguration(pinBl);
+	printTCCConfiguration(pinCh);
+	printTCCConfiguration(pinCl);
+#endif
+
+	// attach pins to timer peripherals
+	bool allAttached = true;
+	allAttached |= attachTCC(pinAh); // in theory this can fail, but there is no way to signal it...
+	allAttached |= attachTCC(pinAl);
+	allAttached |= attachTCC(pinBh);
+	allAttached |= attachTCC(pinBl);
+	allAttached |= attachTCC(pinCh);
+	allAttached |= attachTCC(pinCl);
+	if (!allAttached)
+		return SIMPLEFOC_DRIVER_INIT_FAILED;
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Attached pins...");
+#endif
+	// set up clock - if we did this right it should be possible to get all TCC units synchronized?
+	// e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API
+	configureSAMDClock();
+
+	if (pwm_frequency==NOT_SET) {
+		// use default frequency
+		pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ;
+	}
+
+	// configure the TCC(s)
+	configureTCC(pinAh, pwm_frequency, false, (pinAh.tcc.chaninfo==pinAl.tcc.chaninfo)?dead_zone:-1);
+	if ((pinAh.tcc.chaninfo!=pinAl.tcc.chaninfo))
+		configureTCC(pinAl, pwm_frequency, true, -1.0);
+	configureTCC(pinBh, pwm_frequency, false, (pinBh.tcc.chaninfo==pinBl.tcc.chaninfo)?dead_zone:-1);
+	if ((pinBh.tcc.chaninfo!=pinBl.tcc.chaninfo))
+		configureTCC(pinBl, pwm_frequency, true, -1.0);
+	configureTCC(pinCh, pwm_frequency, false, (pinCh.tcc.chaninfo==pinCl.tcc.chaninfo)?dead_zone:-1);
+	if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo))
+		configureTCC(pinCl, pwm_frequency, true, -1.0);
+	getTccPinConfiguration(pinA_h)->pwm_res = pinAh.pwm_res;
+	getTccPinConfiguration(pinA_l)->pwm_res = pinAh.pwm_res; // use the high phase resolution, in case we didn't set it
+	getTccPinConfiguration(pinB_h)->pwm_res = pinBh.pwm_res;
+	getTccPinConfiguration(pinB_l)->pwm_res = pinBh.pwm_res;
+	getTccPinConfiguration(pinC_h)->pwm_res = pinCh.pwm_res;
+	getTccPinConfiguration(pinC_l)->pwm_res = pinCh.pwm_res;
+#ifdef SIMPLEFOC_SAMD_DEBUG
+	SIMPLEFOC_DEBUG("SAMD: Configured TCCs...");
+#endif
+
+	return new SAMDHardwareDriverParams {
+		.tccPinConfigurations = { getTccPinConfiguration(pinA_h), getTccPinConfiguration(pinA_l), getTccPinConfiguration(pinB_h), getTccPinConfiguration(pinB_l), getTccPinConfiguration(pinC_h), getTccPinConfiguration(pinC_l) },
+		.pwm_frequency = (uint32_t)pwm_frequency,
+		.dead_zone = dead_zone,
+	};
+}
+
+
+
+
+/**
+ * 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 pinA  phase A hardware pin number
+ * @param pinB  phase B hardware pin number
+ */
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params) {
+	writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0], dc_a);
+	writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1], dc_b);
+	return;
+}
+
+
+
+
+
+/**
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - BLDC driver - 3PWM setting
+ * - hardware specific
+ *
+ * @param dc_a  duty cycle phase A [0, 1]
+ * @param dc_b  duty cycle phase B [0, 1]
+ * @param dc_c  duty cycle phase C [0, 1]
+ * @param pinA  phase A hardware pin number
+ * @param pinB  phase B hardware pin number
+ * @param pinC  phase C hardware pin number
+ */
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params) {
+	writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0], dc_a);
+	writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1], dc_b);
+	writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[2], dc_c);
+	return;
+}
+
+
+
+
+/**
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - Stepper driver - 4PWM setting
+ * - hardware specific
+ *
+ * @param dc_1a  duty cycle phase 1A [0, 1]
+ * @param dc_1b  duty cycle phase 1B [0, 1]
+ * @param dc_2a  duty cycle phase 2A [0, 1]
+ * @param dc_2b  duty cycle phase 2B [0, 1]
+ * @param pin1A  phase 1A hardware pin number
+ * @param pin1B  phase 1B hardware pin number
+ * @param pin2A  phase 2A hardware pin number
+ * @param pin2B  phase 2B hardware pin number
+ */
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+	writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[0], dc_1a);
+	writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[1], dc_1b);
+	writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[2], dc_2a);
+	writeSAMDDutyCycle(((SAMDHardwareDriverParams*)params)->tccPinConfigurations[3], dc_2b);
+	return;
+}
+
+
+
+
+/**
+ * Function setting the duty cycle to the pwm pin (ex. analogWrite())
+ * - BLDC driver - 6PWM setting
+ * - hardware specific
+ *
+ * Note: dead-time must be setup in advance, so parameter "dead_zone" is ignored
+ *       the low side pins are automatically driven by the SAMD DTI module, so it is enough to set the high-side
+ *       duty cycle.
+ *       No sanity checks are perfomed to ensure the pinA, pinB, pinC are the same pins you used in configure method...
+ *       so use appropriately.
+ *
+ * @param dc_a  duty cycle phase A [0, 1]
+ * @param dc_b  duty cycle phase B [0, 1]
+ * @param dc_c  duty cycle phase C [0, 1]
+ * @param dead_zone  duty cycle protection zone [0, 1] - both low and high side low
+ * @param pinA_h  phase A high-side hardware pin number
+ * @param pinA_l  phase A low-side hardware pin number
+ * @param pinB_h  phase B high-side hardware pin number
+ * @param pinB_l  phase B low-side hardware pin number
+ * @param pinC_h  phase C high-side hardware pin number
+ * @param pinC_l  phase C low-side hardware pin number
+ *
+ */
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params){
+	SAMDHardwareDriverParams* p = (SAMDHardwareDriverParams*)params;
+	tccConfiguration* tcc1 = p->tccPinConfigurations[0];
+	tccConfiguration* tcc2 = p->tccPinConfigurations[1];
+	uint32_t pwm_res =p->tccPinConfigurations[0]->pwm_res;
+	if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) {
+		// low-side on a different pin of same TCC - do dead-time in software...
+		float ls = dc_a+(p->dead_zone * (pwm_res-1)); // TODO resolution!!!
+		if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time
+		writeSAMDDutyCycle(tcc1, dc_a);
+		writeSAMDDutyCycle(tcc2, ls);
+	}
+	else
+		writeSAMDDutyCycle(tcc1, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly
+
+	tcc1 = p->tccPinConfigurations[2];
+	tcc2 = p->tccPinConfigurations[3];
+	if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) {
+		float ls = dc_b+(p->dead_zone * (pwm_res-1));
+		if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time
+		writeSAMDDutyCycle(tcc1, dc_b);
+		writeSAMDDutyCycle(tcc2, ls);
+	}
+	else
+		writeSAMDDutyCycle(tcc1, dc_b);
+
+	tcc1 = p->tccPinConfigurations[4];
+	tcc2 = p->tccPinConfigurations[5];
+	if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) {
+		float ls = dc_c+(p->dead_zone * (pwm_res-1));
+		if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time
+		writeSAMDDutyCycle(tcc1, dc_c);
+		writeSAMDDutyCycle(tcc2, ls);
+	}
+	else
+		writeSAMDDutyCycle(tcc1, dc_c);
+	return;
+
+	_UNUSED(phase_state);
+}
+
+
+
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+
+/**
+ * Prints a table of pin assignments for your SAMD MCU. Very useful since the
+ * board pinout descriptions and variant.cpp are usually quite wrong, and this
+ * saves you hours of cross-referencing with the datasheet.
+ */
+void printAllPinInfos() {
+	SimpleFOCDebug::println();
+	for (uint8_t pin=0;pin<PINS_COUNT;pin++) {
+		const PinDescription& pinDesc = g_APinDescription[pin];
+		wo_association& association = getWOAssociation(pinDesc.ulPort, pinDesc.ulPin);
+		SimpleFOCDebug::print("Pin ");
+		if (pin<10) SimpleFOCDebug::print("0");
+		SimpleFOCDebug::print(pin);
+		switch (pinDesc.ulPort) {
+			case NOT_A_PORT: SimpleFOCDebug::print("    "); break;
+			case PORTA: SimpleFOCDebug::print("  PA"); break;
+			case PORTB: SimpleFOCDebug::print("  PB"); break;
+			case PORTC: SimpleFOCDebug::print("  PC"); break;
+#if defined(_SAMD51_)||defined(_SAME51_)
+			case PORTD: SimpleFOCDebug::print("  PD"); break;
+#endif
+		}
+		if (pinDesc.ulPin <10) SimpleFOCDebug::print("0");
+		SimpleFOCDebug::print((int)pinDesc.ulPin);
+
+		SimpleFOCDebug::print("  E=");
+		if (association.tccE>=0) {
+			int tcn = GetTCNumber(association.tccE);
+			if (tcn>=TCC_INST_NUM){
+				SimpleFOCDebug::print(" TC");
+				SimpleFOCDebug::print(tcn-TCC_INST_NUM);
+			}
+			else {
+				SimpleFOCDebug::print("TCC");
+				SimpleFOCDebug::print(tcn);
+			}
+			SimpleFOCDebug::print("-");
+			SimpleFOCDebug::print(GetTCChannelNumber(association.tccE));
+			SimpleFOCDebug::print("[");
+			SimpleFOCDebug::print(GetTCChannelNumber(association.woE));
+			SimpleFOCDebug::print("]");
+			if (tcn<10)
+				SimpleFOCDebug::print(" ");
+		}
+		else
+			SimpleFOCDebug::print("  None    ");
+
+		SimpleFOCDebug::print(" F=");
+		if (association.tccF>=0) {
+			int tcn = GetTCNumber(association.tccF);
+			if (tcn>=TCC_INST_NUM){
+				SimpleFOCDebug::print(" TC");
+				SimpleFOCDebug::print(tcn-TCC_INST_NUM);
+			}
+			else {
+				SimpleFOCDebug::print("TCC");
+				SimpleFOCDebug::print(tcn);
+			}
+			SimpleFOCDebug::print("-");
+			SimpleFOCDebug::print(GetTCChannelNumber(association.tccF));
+			SimpleFOCDebug::print("[");
+			SimpleFOCDebug::print(GetTCChannelNumber(association.woF));
+			SimpleFOCDebug::print("]");
+			if (tcn<10)
+				SimpleFOCDebug::print(" ");
+		}
+		else
+			SimpleFOCDebug::print("  None    ");
+
+#if defined(_SAMD51_)||defined(_SAME51_)
+		SimpleFOCDebug::print(" G=");
+		if (association.tccG>=0) {
+			int tcn = GetTCNumber(association.tccG);
+			SimpleFOCDebug::print("TCC");
+			if (tcn<10)
+				SimpleFOCDebug::print(" ");
+			SimpleFOCDebug::print(tcn);
+			SimpleFOCDebug::print("-");
+			SimpleFOCDebug::print(GetTCChannelNumber(association.tccG));
+			SimpleFOCDebug::print("[");
+			SimpleFOCDebug::print(GetTCChannelNumber(association.woG));
+			SimpleFOCDebug::println("]");
+		}
+		else
+			SimpleFOCDebug::println("  None ");
+#else
+		SimpleFOCDebug::println();
+#endif
+
+	}
+	SimpleFOCDebug::println();
+}
+
+
+
+
+
+void printTCCConfiguration(tccConfiguration& info) {
+	SimpleFOCDebug::print(info.pin);
+	if (info.tcc.tccn>=TCC_INST_NUM)
+		SimpleFOCDebug::print(":  TC Peripheral");
+	else
+		SimpleFOCDebug::print(": TCC Peripheral");
+	switch (info.peripheral) {
+	case PIO_TIMER:
+		SimpleFOCDebug::print(" E "); break;
+	case PIO_TIMER_ALT:
+		SimpleFOCDebug::print(" F "); break;
+#if defined(_SAMD51_)||defined(_SAME51_)
+	case PIO_TCC_PDEC:
+		SimpleFOCDebug::print(" G "); break;
+#endif
+	default:
+		SimpleFOCDebug::print(" ? "); break;
+	}
+	if (info.tcc.tccn>=0) {
+		SimpleFOCDebug::print(info.tcc.tccn);
+		SimpleFOCDebug::print("-");
+		SimpleFOCDebug::print(info.tcc.chan);
+		SimpleFOCDebug::print("[");
+		SimpleFOCDebug::print(info.wo);
+		SimpleFOCDebug::println("]");
+	}
+	else
+		SimpleFOCDebug::println(" None");
+}
+
+
+
+#endif
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/samd/samd_mcu.h

@@ -0,0 +1,127 @@
+
+#ifndef SAMD_MCU_H
+#define SAMD_MCU_H
+
+
+// uncomment to enable debug output from SAMD driver
+// can set this as build-flag in Arduino IDE or PlatformIO
+// #define SIMPLEFOC_SAMD_DEBUG
+
+#include "../../hardware_api.h"
+
+
+#if defined(__SAME51J19A__) || defined(__ATSAME51J19A__)
+#ifndef _SAME51_
+#define _SAME51_
+#endif
+#endif
+
+
+#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_)
+
+
+#include "Arduino.h"
+#include "variant.h"
+#include "wiring_private.h"
+
+
+#ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION
+#define SIMPLEFOC_SAMD_PWM_RESOLUTION 1000
+#endif
+
+#define SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ 24000
+// arbitrary maximum. On SAMD51 with 120MHz clock this means 2kHz minimum pwm frequency
+#define SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION 30000
+// lets not go too low - 400 with clock speed of 120MHz on SAMD51 means 150kHz maximum PWM frequency...
+//						 400 with 48MHz clock on SAMD21 means 60kHz maximum PWM frequency...
+#define SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION 400 
+// this is the most we can support on the TC units
+#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250
+
+#ifndef SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS
+#define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 24
+#endif
+
+
+
+struct tccConfiguration {
+	uint8_t pin;
+	EPioType peripheral; // 1=true, 0=false
+	uint8_t wo;
+	union tccChanInfo {
+		struct {
+			int8_t chan;
+			int8_t tccn;
+		};
+		uint16_t chaninfo;
+	} tcc;
+	uint16_t pwm_res;
+};
+
+
+
+
+
+
+struct wo_association {
+	EPortType port;
+	uint32_t pin;
+	ETCChannel tccE;
+	uint8_t woE;
+	ETCChannel tccF;
+	uint8_t woF;
+#if defined(_SAMD51_)||defined(_SAME51_)
+	ETCChannel tccG;
+	uint8_t woG;
+#endif
+};
+
+
+
+typedef struct SAMDHardwareDriverParams {
+	tccConfiguration* tccPinConfigurations[6];
+	uint32_t pwm_frequency;
+	float dead_zone;
+} SAMDHardwareDriverParams;
+
+
+
+
+#if defined(_SAMD21_)
+#define NUM_PIO_TIMER_PERIPHERALS 2
+#elif defined(_SAMD51_)||defined(_SAME51_)
+#define NUM_PIO_TIMER_PERIPHERALS 3
+#endif
+
+
+
+/**
+ * Global state
+ */
+extern struct wo_association WO_associations[];
+extern uint8_t TCC_CHANNEL_COUNT[];
+extern tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS];
+extern uint8_t numTccPinConfigurations;
+extern bool SAMDClockConfigured;
+extern bool tccConfigured[TCC_INST_NUM+TC_INST_NUM];
+
+
+
+struct wo_association& getWOAssociation(EPortType port, uint32_t pin);
+void writeSAMDDutyCycle(tccConfiguration* info, float dc);
+void configureSAMDClock();
+void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1);
+__inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused));
+EPioType getPeripheralOfPermutation(int permutation, int pin_position);
+
+#ifdef SIMPLEFOC_SAMD_DEBUG
+void printTCCConfiguration(tccConfiguration& info);
+void printAllPinInfos();
+#endif
+
+
+
+#endif
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/stm32/stm32_mcu.cpp

@@ -0,0 +1,952 @@
+
+#include "../../hardware_api.h"
+#include "stm32_mcu.h"
+
+#if defined(_STM32_DEF_)
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for STM32")
+#pragma message("")
+
+
+//#define SIMPLEFOC_STM32_DEBUG
+
+#ifdef SIMPLEFOC_STM32_DEBUG
+void printTimerCombination(int numPins, PinMap* timers[], int score);
+int getTimerNumber(int timerIndex);
+#endif
+
+
+
+
+#ifndef SIMPLEFOC_STM32_MAX_PINTIMERSUSED
+#define SIMPLEFOC_STM32_MAX_PINTIMERSUSED 12
+#endif
+int numTimerPinsUsed;
+PinMap* timerPinsUsed[SIMPLEFOC_STM32_MAX_PINTIMERSUSED];
+
+
+
+
+
+
+// setting pwm to hardware pin - instead analogWrite()
+void _setPwm(HardwareTimer *HT, uint32_t channel, uint32_t value, int resolution)
+{
+  // TODO - remove commented code
+  // PinName pin = digitalPinToPinName(ulPin);
+  // TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM);
+  // uint32_t index = get_timer_index(Instance);
+  // HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+  
+  HT->setCaptureCompare(channel, value, (TimerCompareFormat_t)resolution);
+}
+
+
+
+
+int getLLChannel(PinMap* timer) {
+#if defined(TIM_CCER_CC1NE)
+  if (STM_PIN_INVERTED(timer->function)) {
+    switch (STM_PIN_CHANNEL(timer->function)) {
+      case 1: return LL_TIM_CHANNEL_CH1N;
+      case 2: return LL_TIM_CHANNEL_CH2N;
+      case 3: return LL_TIM_CHANNEL_CH3N;
+#if defined(LL_TIM_CHANNEL_CH4N)
+      case 4: return LL_TIM_CHANNEL_CH4N;
+#endif
+      default: return -1;
+    }
+  } else
+#endif
+  {
+    switch (STM_PIN_CHANNEL(timer->function)) {
+      case 1: return LL_TIM_CHANNEL_CH1;
+      case 2: return LL_TIM_CHANNEL_CH2;
+      case 3: return LL_TIM_CHANNEL_CH3;
+      case 4: return LL_TIM_CHANNEL_CH4;
+      default: return -1;
+    }
+  }
+  return -1;
+}
+
+
+
+
+
+// init pin pwm
+HardwareTimer* _initPinPWM(uint32_t PWM_freq, PinMap* timer) {
+  // sanity check
+  if (timer==NP)
+    return NP;
+  uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral);
+  bool init = false;
+  if (HardwareTimer_Handle[index] == NULL) {
+    HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral);
+    HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
+    HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1;
+    HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
+    init = true;
+  }
+  HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+  uint32_t channel = STM_PIN_CHANNEL(timer->function);
+  HT->pause();
+  if (init)
+    HT->setOverflow(PWM_freq, HERTZ_FORMAT);
+  HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, timer->pin);
+  #if SIMPLEFOC_PWM_ACTIVE_HIGH==false
+  LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW);
+  #endif
+#ifdef SIMPLEFOC_STM32_DEBUG
+  SIMPLEFOC_DEBUG("STM32-DRV: Configuring high timer ", (int)getTimerNumber(get_timer_index(HardwareTimer_Handle[index]->handle.Instance)));
+  SIMPLEFOC_DEBUG("STM32-DRV: Configuring high channel ", (int)channel);
+#endif
+  return HT;
+}
+
+
+
+
+
+
+
+// init high side pin
+HardwareTimer* _initPinPWMHigh(uint32_t PWM_freq, PinMap* timer) {
+  HardwareTimer* HT = _initPinPWM(PWM_freq, timer);
+  #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false && SIMPLEFOC_PWM_ACTIVE_HIGH==true
+  LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW);
+  #endif
+  return HT;
+}
+
+// init low side pin
+HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, PinMap* timer)
+{
+  uint32_t index = get_timer_index((TIM_TypeDef*)timer->peripheral);
+
+  bool init = false;
+  if (HardwareTimer_Handle[index] == NULL) {
+    HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)timer->peripheral);
+    HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
+    HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1;
+    HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
+    init = true;
+  }
+  HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+  uint32_t channel = STM_PIN_CHANNEL(timer->function);
+
+#ifdef SIMPLEFOC_STM32_DEBUG
+  SIMPLEFOC_DEBUG("STM32-DRV: Configuring low timer ", (int)getTimerNumber(get_timer_index(HardwareTimer_Handle[index]->handle.Instance)));
+  SIMPLEFOC_DEBUG("STM32-DRV: Configuring low channel ", (int)channel);
+#endif
+
+  HT->pause();
+  if (init)
+    HT->setOverflow(PWM_freq, HERTZ_FORMAT);
+  // sets internal fields of HT, but we can't set polarity here
+  HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM2, timer->pin);
+  #if SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH==false
+  LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(timer), LL_TIM_OCPOLARITY_LOW);
+  #endif
+  return HT;
+}
+
+
+
+// align the timers to end the init
+void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3)
+{
+  HT1->pause();
+  HT1->refresh();
+  HT2->pause();
+  HT2->refresh();
+  HT3->pause();
+  HT3->refresh();
+  HT1->resume();
+  HT2->resume();
+  HT3->resume();
+}
+
+
+
+
+// align the timers to end the init
+void _alignPWMTimers(HardwareTimer *HT1, HardwareTimer *HT2, HardwareTimer *HT3, HardwareTimer *HT4)
+{
+  HT1->pause();
+  HT1->refresh();
+  HT2->pause();
+  HT2->refresh();
+  HT3->pause();
+  HT3->refresh();
+  HT4->pause();
+  HT4->refresh();
+  HT1->resume();
+  HT2->resume();
+  HT3->resume();
+  HT4->resume();
+}
+
+
+// align the timers to end the init
+void _stopTimers(HardwareTimer **timers_to_stop, int timer_num)
+{
+  // TODO - stop each timer only once
+  // stop timers
+  for (int i=0; i < timer_num; i++) {
+    if(timers_to_stop[i] == NP) return;
+    timers_to_stop[i]->pause();
+    timers_to_stop[i]->refresh();
+    #ifdef SIMPLEFOC_STM32_DEBUG
+      SIMPLEFOC_DEBUG("STM32-DRV: Stopping timer ", getTimerNumber(get_timer_index(timers_to_stop[i]->getHandle()->Instance)));
+    #endif
+  }
+}
+
+// align the timers to end the init
+void _startTimers(HardwareTimer **timers_to_start, int timer_num)
+{
+  // TODO - sart each timer only once
+  // sart timers
+  for (int i=0; i < timer_num; i++) {
+    if(timers_to_start[i] == NP) return;
+    timers_to_start[i]->resume();
+    #ifdef SIMPLEFOC_STM32_DEBUG
+      SIMPLEFOC_DEBUG("STM32-DRV: Starting timer ", getTimerNumber(get_timer_index(timers_to_start[i]->getHandle()->Instance)));
+    #endif
+  }
+}
+
+void _alignTimersNew() {
+  int numTimers = 0;
+  HardwareTimer *timers[numTimerPinsUsed];
+
+  // reset timer counters
+  for (int i=0; i<numTimerPinsUsed; i++) {
+    uint32_t index = get_timer_index((TIM_TypeDef*)timerPinsUsed[i]->peripheral);
+    HardwareTimer *timer = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+    bool found = false;
+    for (int j=0; j<numTimers; j++) {
+      if (timers[j] == timer) {
+        found = true;
+        break;
+      }
+    }
+    if (!found)
+      timers[numTimers++] = timer;
+  }
+
+  // enable timer clock
+  for (int i=0; i<numTimers; i++) {
+    timers[i]->pause();
+    timers[i]->refresh();
+    #ifdef SIMPLEFOC_STM32_DEBUG
+      SIMPLEFOC_DEBUG("STM32-DRV: Restarting timer ", getTimerNumber(get_timer_index(timers[i]->getHandle()->Instance)));
+    #endif
+  }
+
+  for (int i=0; i<numTimers; i++) {
+    timers[i]->resume();
+  }
+
+}
+
+
+
+
+
+// configure hardware 6pwm for a complementary pair of channels
+STM32DriverParams* _initHardware6PWMPair(long PWM_freq, float dead_zone, PinMap* pinH, PinMap* pinL, STM32DriverParams* params, int paramsPos) {
+  // sanity check
+  if (pinH==NP || pinL==NP)
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+  
+#if defined(STM32L0xx) // L0 boards dont have hardware 6pwm interface 
+  return SIMPLEFOC_DRIVER_INIT_FAILED; // return nothing
+#endif
+
+  uint32_t channel1 = STM_PIN_CHANNEL(pinH->function);
+  uint32_t channel2 = STM_PIN_CHANNEL(pinL->function);
+
+  // more sanity check
+  if (channel1!=channel2 || pinH->peripheral!=pinL->peripheral)
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  uint32_t index = get_timer_index((TIM_TypeDef*)pinH->peripheral);
+
+  if (HardwareTimer_Handle[index] == NULL) {
+    HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef*)pinH->peripheral);
+    HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3;
+    HardwareTimer_Handle[index]->handle.Init.RepetitionCounter = 1;
+    // HardwareTimer_Handle[index]->handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; 
+    HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle));
+    ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow((uint32_t)PWM_freq, HERTZ_FORMAT);
+  }
+  HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this);
+
+  HT->setMode(channel1, TIMER_OUTPUT_COMPARE_PWM1, pinH->pin);
+  HT->setMode(channel2, TIMER_OUTPUT_COMPARE_PWM1, pinL->pin);
+
+  // dead time is set in nanoseconds
+  uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone;
+  uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns);
+  if (dead_time>255) dead_time = 255;
+  if (dead_time==0 && dead_zone>0) {
+    dead_time = 255; // LL_TIM_CALC_DEADTIME returns 0 if dead_time_ns is too large
+    SIMPLEFOC_DEBUG("STM32-DRV: WARN: dead time too large, setting to max");
+  }
+  LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear!
+  #if SIMPLEFOC_PWM_HIGHSIDE_ACTIVE_HIGH==false
+  LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinH), LL_TIM_OCPOLARITY_LOW);
+  #endif
+  #if SIMPLEFOC_PWM_LOWSIDE_ACTIVE_HIGH==false
+  LL_TIM_OC_SetPolarity(HT->getHandle()->Instance, getLLChannel(pinL), LL_TIM_OCPOLARITY_LOW);
+  #endif
+  LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, getLLChannel(pinH) | getLLChannel(pinL));
+  HT->pause();
+
+  // make sure timer output goes to LOW when timer channels are temporarily disabled
+  LL_TIM_SetOffStates(HT->getHandle()->Instance, LL_TIM_OSSI_DISABLE, LL_TIM_OSSR_ENABLE);
+
+  params->timers[paramsPos] = HT;
+  params->timers[paramsPos+1] = HT;
+  params->channels[paramsPos] = channel1;
+  params->channels[paramsPos+1] = channel2;
+  return params;
+}
+
+
+
+
+STM32DriverParams* _initHardware6PWMInterface(long PWM_freq, float dead_zone, PinMap* pinA_h, PinMap* pinA_l, PinMap* pinB_h, PinMap* pinB_l, PinMap* pinC_h, PinMap* pinC_l) {
+  STM32DriverParams* params = new STM32DriverParams {
+    .timers = { NP, NP, NP, NP, NP, NP },
+    .channels = { 0, 0, 0, 0, 0, 0 },
+    .pwm_frequency = PWM_freq,
+    .dead_zone = dead_zone,
+    .interface_type = _HARDWARE_6PWM
+  };
+
+  if (_initHardware6PWMPair(PWM_freq, dead_zone, pinA_h, pinA_l, params, 0) == SIMPLEFOC_DRIVER_INIT_FAILED)
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+  if(_initHardware6PWMPair(PWM_freq, dead_zone, pinB_h, pinB_l, params, 2) == SIMPLEFOC_DRIVER_INIT_FAILED)
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+  if (_initHardware6PWMPair(PWM_freq, dead_zone, pinC_h, pinC_l, params, 4) == SIMPLEFOC_DRIVER_INIT_FAILED)
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  return params;
+}
+
+
+
+
+
+
+/*
+  timer combination scoring function
+  assigns a score, and also checks the combination is valid
+  returns <0 if combination is invalid, >=0 if combination is valid. lower (but positive) score is better
+  for 6 pwm, hardware 6-pwm is preferred over software 6-pwm
+  hardware 6-pwm is possible if each low channel is the inverted counterpart of its high channel
+  inverted channels are not allowed except when using hardware 6-pwm (in theory they could be but lets not complicate things)
+*/
+int scoreCombination(int numPins, PinMap* pinTimers[]) {
+  // check already used - TODO move this to outer loop also...
+  for (int i=0; i<numTimerPinsUsed; i++) {
+    if (pinTimers[i]->peripheral == timerPinsUsed[i]->peripheral
+        && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(timerPinsUsed[i]->function))
+      return -2; // bad combination - timer channel already used
+  }
+  
+  // TODO LPTIM and HRTIM should be ignored for now
+  
+  // check for inverted channels
+  if (numPins < 6) {
+    for (int i=0; i<numPins; i++) {
+      if (STM_PIN_INVERTED(pinTimers[i]->function))
+        return -3; // bad combination - inverted channel used in non-hardware 6pwm
+    }
+  }
+  // check for duplicated channels
+  for (int i=0; i<numPins-1; i++) {
+    for (int j=i+1; j<numPins; j++) {
+      if (pinTimers[i]->peripheral == pinTimers[j]->peripheral
+          && STM_PIN_CHANNEL(pinTimers[i]->function) == STM_PIN_CHANNEL(pinTimers[j]->function)
+          && STM_PIN_INVERTED(pinTimers[i]->function) == STM_PIN_INVERTED(pinTimers[j]->function))
+        return -4; // bad combination - duplicated channel
+    }
+  }
+  int score = 0;
+  for (int i=0; i<numPins; i++) {
+    // count distinct timers
+    bool found = false;
+    for (int j=i+1; j<numPins; j++) {
+      if (pinTimers[i]->peripheral == pinTimers[j]->peripheral)
+        found = true;
+    }
+    if (!found) score++;
+  }
+  if (numPins==6) {
+    // check for inverted channels - best: 1 timer, 3 channels, 3 matching inverted channels
+    //                                     >1 timer, 3 channels, 3 matching inverted channels
+    //                                     1 timer, 6 channels (no inverted channels)
+    //                                     >1 timer, 6 channels (no inverted channels)
+    // check for inverted high-side channels - TODO is this a configuration we should allow? what if all 3 high side channels are inverted and the low-side non-inverted?
+    if (STM_PIN_INVERTED(pinTimers[0]->function) || STM_PIN_INVERTED(pinTimers[2]->function) || STM_PIN_INVERTED(pinTimers[4]->function))
+      return -5; // bad combination - inverted channel used on high-side channel
+    if (pinTimers[0]->peripheral == pinTimers[1]->peripheral
+        && pinTimers[2]->peripheral == pinTimers[3]->peripheral
+        && pinTimers[4]->peripheral == pinTimers[5]->peripheral
+        && STM_PIN_CHANNEL(pinTimers[0]->function) == STM_PIN_CHANNEL(pinTimers[1]->function)
+        && STM_PIN_CHANNEL(pinTimers[2]->function) == STM_PIN_CHANNEL(pinTimers[3]->function)
+        && STM_PIN_CHANNEL(pinTimers[4]->function) == STM_PIN_CHANNEL(pinTimers[5]->function)
+        && STM_PIN_INVERTED(pinTimers[1]->function) && STM_PIN_INVERTED(pinTimers[3]->function) && STM_PIN_INVERTED(pinTimers[5]->function)) {
+          // hardware 6pwm, score <10
+
+          // TODO F37xxx doesn't support dead-time insertion, it has no TIM1/TIM8
+          // F301, F302 --> 6 channels, but only 1-3 have dead-time insertion
+          // TIM2/TIM3/TIM4/TIM5 don't do dead-time insertion
+          // TIM15/TIM16/TIM17 do dead-time insertion only on channel 1
+
+          // TODO check these defines
+          #if defined(STM32F4xx_HAL_TIM_H) || defined(STM32F3xx_HAL_TIM_H) || defined(STM32F2xx_HAL_TIM_H) || defined(STM32F1xx_HAL_TIM_H) || defined(STM32F100_HAL_TIM_H) || defined(STM32FG0x1_HAL_TIM_H)  || defined(STM32G0x0_HAL_TIM_H) 
+          if (STM_PIN_CHANNEL(pinTimers[0]->function)>3 || STM_PIN_CHANNEL(pinTimers[2]->function)>3 || STM_PIN_CHANNEL(pinTimers[4]->function)>3 )
+            return -8; // channel 4 does not have dead-time insertion
+          #endif
+          #ifdef STM32G4xx_HAL_TIM_H
+          if (STM_PIN_CHANNEL(pinTimers[0]->function)>4 || STM_PIN_CHANNEL(pinTimers[2]->function)>4 || STM_PIN_CHANNEL(pinTimers[4]->function)>4 )
+            return -8; // channels 5 & 6 do not have dead-time insertion
+          #endif
+        }
+    else {
+      // check for inverted low-side channels
+      if (STM_PIN_INVERTED(pinTimers[1]->function) || STM_PIN_INVERTED(pinTimers[3]->function) || STM_PIN_INVERTED(pinTimers[5]->function))
+        return -6; // bad combination - inverted channel used on low-side channel in software 6-pwm
+      if (pinTimers[0]->peripheral != pinTimers[1]->peripheral
+        || pinTimers[2]->peripheral != pinTimers[3]->peripheral
+        || pinTimers[4]->peripheral != pinTimers[5]->peripheral)
+        return -7; // bad combination - non-matching timers for H/L side in software 6-pwm
+      score += 10; // software 6pwm, score >10
+    }
+  }
+  return score;
+}
+
+
+
+
+int findIndexOfFirstPinMapEntry(int pin) {
+  PinName pinName = digitalPinToPinName(pin);
+  int i = 0;
+  while (PinMap_TIM[i].pin!=NC) {
+    if (pinName == PinMap_TIM[i].pin)
+      return i;
+    i++;
+  }
+  return -1;
+}
+
+
+int findIndexOfLastPinMapEntry(int pin) {
+  PinName pinName = digitalPinToPinName(pin);
+  int i = 0;
+  while (PinMap_TIM[i].pin!=NC) {
+    if (   pinName == (PinMap_TIM[i].pin & ~ALTX_MASK) 
+        && pinName != (PinMap_TIM[i+1].pin & ~ALTX_MASK))
+      return i;
+    i++;
+  }
+  return -1;
+}
+
+
+
+
+
+
+#define NOT_FOUND 1000
+
+int findBestTimerCombination(int numPins, int index, int pins[], PinMap* pinTimers[]) {
+  PinMap* searchArray[numPins];
+  for (int j=0;j<numPins;j++)
+        searchArray[j] = pinTimers[j];
+  int bestScore = NOT_FOUND;
+  int startIndex = findIndexOfFirstPinMapEntry(pins[index]);
+  int endIndex = findIndexOfLastPinMapEntry(pins[index]);
+  if (startIndex == -1 || endIndex == -1) {
+    SIMPLEFOC_DEBUG("STM32-DRV: ERR: no timer on pin ", pins[index]);
+    return -1; // pin is not connected to any timer
+  }
+  for (int i=startIndex;i<=endIndex;i++) {
+    searchArray[index] = (PinMap*)&PinMap_TIM[i];
+    int score = NOT_FOUND;
+    if (index<numPins-1)
+      score = findBestTimerCombination(numPins, index+1, pins, searchArray);
+    else {
+      score = scoreCombination(numPins, searchArray);
+      #ifdef SIMPLEFOC_STM32_DEBUG
+      printTimerCombination(numPins, searchArray, score);
+      #endif
+    }
+    if (score==-1)
+      return -1; // pin not connected to any timer, propagate driectly
+    if (score>=0 && score<bestScore) {
+      bestScore = score;
+      for (int j=index;j<numPins;j++)
+        pinTimers[j] = searchArray[j];
+    }
+  }
+  return bestScore;
+}
+
+
+
+
+
+int findBestTimerCombination(int numPins, int pins[], PinMap* pinTimers[]) {
+  int bestScore = findBestTimerCombination(numPins, 0, pins, pinTimers);
+  if (bestScore == NOT_FOUND) {
+    #ifdef SIMPLEFOC_STM32_DEBUG
+    SimpleFOCDebug::println("STM32-DRV: no workable combination found on these pins");
+    #endif
+    return -10; // no workable combination found
+  }
+  else if (bestScore >= 0) {
+    #ifdef SIMPLEFOC_STM32_DEBUG
+    SimpleFOCDebug::print("STM32-DRV: best: ");
+    printTimerCombination(numPins, pinTimers, bestScore);
+    #endif
+  }
+  return bestScore;
+};
+
+
+
+void* _configure1PWM(long pwm_frequency, const int pinA) {
+  if (numTimerPinsUsed+1 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) {
+    SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used");
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+  }
+
+  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
+  // center-aligned frequency is uses two periods
+  pwm_frequency *=2;
+
+  int pins[1] = { pinA };
+  PinMap* pinTimers[1] = { NP };
+  if (findBestTimerCombination(1, pins, pinTimers)<0)
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);\
+  // allign the timers
+  _alignTimersNew();
+  
+  uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function);
+
+  STM32DriverParams* params = new STM32DriverParams {
+    .timers = { HT1 },
+    .channels = { channel1 },
+    .pwm_frequency = pwm_frequency
+  };
+  timerPinsUsed[numTimerPinsUsed++] = pinTimers[0];
+  return params;
+}
+
+
+
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 2PWM setting
+// - hardware speciffic
+void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) {
+  if (numTimerPinsUsed+2 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) {
+    SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used");
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+  }
+
+  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
+  // center-aligned frequency is uses two periods
+  pwm_frequency *=2;
+
+  int pins[2] = { pinA, pinB };
+  PinMap* pinTimers[2] = { NP, NP };
+  if (findBestTimerCombination(2, pins, pinTimers)<0)
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);
+  HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]);
+  // allign the timers
+  _alignPWMTimers(HT1, HT2, HT2);
+  
+  uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function);
+  uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function);
+
+  STM32DriverParams* params = new STM32DriverParams {
+    .timers = { HT1, HT2 },
+    .channels = { channel1, channel2 },
+    .pwm_frequency = pwm_frequency
+  };
+  timerPinsUsed[numTimerPinsUsed++] = pinTimers[0];
+  timerPinsUsed[numTimerPinsUsed++] = pinTimers[1];
+  return params;
+}
+
+
+
+
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  if (numTimerPinsUsed+3 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) {
+    SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used");
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+  }
+  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
+  // center-aligned frequency is uses two periods
+  pwm_frequency *=2;
+
+  int pins[3] = { pinA, pinB, pinC };
+  PinMap* pinTimers[3] = { NP, NP, NP };
+  if (findBestTimerCombination(3, pins, pinTimers)<0)
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);
+  HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]);
+  HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]);
+  
+  uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function);
+  uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function);
+  uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function);
+
+  STM32DriverParams* params = new STM32DriverParams {
+    .timers = { HT1, HT2, HT3 },
+    .channels = { channel1, channel2, channel3 },
+    .pwm_frequency = pwm_frequency
+  };
+  timerPinsUsed[numTimerPinsUsed++] = pinTimers[0];
+  timerPinsUsed[numTimerPinsUsed++] = pinTimers[1];
+  timerPinsUsed[numTimerPinsUsed++] = pinTimers[2];
+
+  _alignTimersNew();
+
+  return params;
+}
+
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void* _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) {
+  if (numTimerPinsUsed+4 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) {
+    SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used");
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+  }
+  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
+  // center-aligned frequency is uses two periods
+  pwm_frequency *=2;
+
+  int pins[4] = { pinA, pinB, pinC, pinD };
+  PinMap* pinTimers[4] = { NP, NP, NP, NP };
+  if (findBestTimerCombination(4, pins, pinTimers)<0)
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinTimers[0]);
+  HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinTimers[1]);
+  HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinTimers[2]);
+  HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinTimers[3]);
+  // allign the timers
+  _alignPWMTimers(HT1, HT2, HT3, HT4);
+
+  uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function);
+  uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function);
+  uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function);
+  uint32_t channel4 = STM_PIN_CHANNEL(pinTimers[3]->function);
+
+  STM32DriverParams* params = new STM32DriverParams {
+    .timers = { HT1, HT2, HT3, HT4 },
+    .channels = { channel1, channel2, channel3, channel4 },
+    .pwm_frequency = pwm_frequency
+  };
+  timerPinsUsed[numTimerPinsUsed++] = pinTimers[0];
+  timerPinsUsed[numTimerPinsUsed++] = pinTimers[1];
+  timerPinsUsed[numTimerPinsUsed++] = pinTimers[2];
+  timerPinsUsed[numTimerPinsUsed++] = pinTimers[3];
+  return params;
+}
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - DC motor - 1PWM setting
+// - hardware speciffic
+void _writeDutyCycle1PWM(float dc_a, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION);
+}
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 2PWM setting
+//- hardware speciffic
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params){
+  // transform duty cycle from [0,1] to [0,4095]
+  _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION);
+  _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - BLDC motor - 3PWM setting
+//- hardware speciffic
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params){
+  // transform duty cycle from [0,1] to [0,4095]
+  _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION);
+  _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_b, _PWM_RESOLUTION);
+  _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_c, _PWM_RESOLUTION);
+}
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+//- hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+  // transform duty cycle from [0,1] to [0,4095]
+  _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_1a, _PWM_RESOLUTION);
+  _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _PWM_RANGE*dc_1b, _PWM_RESOLUTION);
+  _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_2a, _PWM_RESOLUTION);
+  _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _PWM_RANGE*dc_2b, _PWM_RESOLUTION);
+}
+
+
+
+
+// Configuring PWM frequency, resolution and alignment
+// - BLDC driver - 6PWM setting
+// - hardware specific
+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){
+  if (numTimerPinsUsed+6 > SIMPLEFOC_STM32_MAX_PINTIMERSUSED) {
+    SIMPLEFOC_DEBUG("STM32-DRV: ERR: too many pins used");
+    return (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+  }
+  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 |%0kHz max
+  // center-aligned frequency is uses two periods
+  pwm_frequency *=2;
+
+  // find configuration
+  int pins[6] = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l };
+  PinMap* pinTimers[6] = { NP, NP, NP, NP, NP, NP };
+  int score = findBestTimerCombination(6, pins, pinTimers);
+
+  STM32DriverParams* params;
+  // configure accordingly
+  if (score<0)
+    params = (STM32DriverParams*)SIMPLEFOC_DRIVER_INIT_FAILED;
+  else if (score<10)  // hardware pwm
+    params = _initHardware6PWMInterface(pwm_frequency, dead_zone, pinTimers[0], pinTimers[1], pinTimers[2], pinTimers[3], pinTimers[4], pinTimers[5]);
+  else {  // software pwm
+    HardwareTimer* HT1 = _initPinPWMHigh(pwm_frequency, pinTimers[0]);
+    HardwareTimer* HT2 = _initPinPWMLow(pwm_frequency, pinTimers[1]);
+    HardwareTimer* HT3 = _initPinPWMHigh(pwm_frequency, pinTimers[2]);
+    HardwareTimer* HT4 = _initPinPWMLow(pwm_frequency, pinTimers[3]);
+    HardwareTimer* HT5 = _initPinPWMHigh(pwm_frequency, pinTimers[4]);
+    HardwareTimer* HT6 = _initPinPWMLow(pwm_frequency, pinTimers[5]);
+    uint32_t channel1 = STM_PIN_CHANNEL(pinTimers[0]->function);
+    uint32_t channel2 = STM_PIN_CHANNEL(pinTimers[1]->function);
+    uint32_t channel3 = STM_PIN_CHANNEL(pinTimers[2]->function);
+    uint32_t channel4 = STM_PIN_CHANNEL(pinTimers[3]->function);
+    uint32_t channel5 = STM_PIN_CHANNEL(pinTimers[4]->function);
+    uint32_t channel6 = STM_PIN_CHANNEL(pinTimers[5]->function);
+    params = new STM32DriverParams {
+      .timers = { HT1, HT2, HT3, HT4, HT5, HT6 },
+      .channels = { channel1, channel2, channel3, channel4, channel5, channel6 },
+      .pwm_frequency = pwm_frequency,
+      .dead_zone = dead_zone,
+      .interface_type = _SOFTWARE_6PWM
+    };
+  }
+  if (score>=0) {
+    for (int i=0; i<6; i++)
+      timerPinsUsed[numTimerPinsUsed++] = pinTimers[i];
+    _alignTimersNew();
+  }
+  return params; // success
+}
+
+
+
+void _setSinglePhaseState(PhaseState state, HardwareTimer *HT, int channel1,int channel2) {
+  _UNUSED(channel2);
+  switch (state) {
+    case PhaseState::PHASE_OFF:
+      // Due to a weird quirk in the arduino timer API, pauseChannel only disables the complementary channel (e.g. CC1NE).
+      // To actually make the phase floating, we must also set pwm to 0.
+      HT->pauseChannel(channel1);
+      break;
+    default:
+      HT->resumeChannel(channel1);
+      break;
+  }
+}
+
+
+// Function setting the duty cycle to the pwm pin (ex. analogWrite())
+// - BLDC driver - 6PWM setting
+// - hardware specific
+void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState* phase_state, void* params){
+  switch(((STM32DriverParams*)params)->interface_type){
+    case _HARDWARE_6PWM:
+      // phase a
+      _setSinglePhaseState(phase_state[0], ((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], ((STM32DriverParams*)params)->channels[1]);
+      if(phase_state[0] == PhaseState::PHASE_OFF) dc_a = 0.0f;
+      _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _PWM_RANGE*dc_a, _PWM_RESOLUTION);
+      // phase b
+      _setSinglePhaseState(phase_state[1], ((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], ((STM32DriverParams*)params)->channels[3]);
+      if(phase_state[1] == PhaseState::PHASE_OFF) dc_b = 0.0f;
+      _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _PWM_RANGE*dc_b, _PWM_RESOLUTION);
+      // phase c
+      _setSinglePhaseState(phase_state[2], ((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], ((STM32DriverParams*)params)->channels[5]);
+      if(phase_state[2] == PhaseState::PHASE_OFF) dc_c = 0.0f;
+      _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _PWM_RANGE*dc_c, _PWM_RESOLUTION);
+      break;
+    case _SOFTWARE_6PWM:
+      float dead_zone = ((STM32DriverParams*)params)->dead_zone  / 2.0f;
+      if (phase_state[0] == PhaseState::PHASE_ON || phase_state[0] == PhaseState::PHASE_HI)
+        _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], _constrain(dc_a - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
+      else
+        _setPwm(((STM32DriverParams*)params)->timers[0], ((STM32DriverParams*)params)->channels[0], 0.0f, _PWM_RESOLUTION);
+      if (phase_state[0] == PhaseState::PHASE_ON || phase_state[0] == PhaseState::PHASE_LO)
+        _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], _constrain(dc_a + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
+      else
+        _setPwm(((STM32DriverParams*)params)->timers[1], ((STM32DriverParams*)params)->channels[1], 0.0f, _PWM_RESOLUTION);
+
+      if (phase_state[1] == PhaseState::PHASE_ON || phase_state[1] == PhaseState::PHASE_HI)
+        _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], _constrain(dc_b - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
+      else
+        _setPwm(((STM32DriverParams*)params)->timers[2], ((STM32DriverParams*)params)->channels[2], 0.0f, _PWM_RESOLUTION);
+      if (phase_state[1] == PhaseState::PHASE_ON || phase_state[1] == PhaseState::PHASE_LO)
+        _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], _constrain(dc_b + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
+      else
+        _setPwm(((STM32DriverParams*)params)->timers[3], ((STM32DriverParams*)params)->channels[3], 0.0f, _PWM_RESOLUTION);
+
+      if (phase_state[2] == PhaseState::PHASE_ON || phase_state[2] == PhaseState::PHASE_HI)
+        _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], _constrain(dc_c - dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
+      else
+        _setPwm(((STM32DriverParams*)params)->timers[4], ((STM32DriverParams*)params)->channels[4], 0.0f, _PWM_RESOLUTION);
+      if (phase_state[2] == PhaseState::PHASE_ON || phase_state[2] == PhaseState::PHASE_LO)
+        _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], _constrain(dc_c + dead_zone, 0.0f, 1.0f)*_PWM_RANGE, _PWM_RESOLUTION);
+      else
+        _setPwm(((STM32DriverParams*)params)->timers[5], ((STM32DriverParams*)params)->channels[5], 0.0f, _PWM_RESOLUTION);
+      break;
+  }
+  _UNUSED(phase_state);
+}
+
+
+
+#ifdef SIMPLEFOC_STM32_DEBUG
+
+int getTimerNumber(int timerIndex) {
+  #if defined(TIM1_BASE)
+    if (timerIndex==TIMER1_INDEX) return 1;
+  #endif
+  #if defined(TIM2_BASE)
+    if (timerIndex==TIMER2_INDEX) return 2;
+  #endif
+  #if defined(TIM3_BASE)
+    if (timerIndex==TIMER3_INDEX) return 3;
+  #endif
+  #if defined(TIM4_BASE)
+    if (timerIndex==TIMER4_INDEX) return 4;
+  #endif
+  #if defined(TIM5_BASE)
+    if (timerIndex==TIMER5_INDEX) return 5;
+  #endif
+  #if defined(TIM6_BASE)
+    if (timerIndex==TIMER6_INDEX) return 6;
+  #endif
+  #if defined(TIM7_BASE)
+    if (timerIndex==TIMER7_INDEX) return 7;
+  #endif
+  #if defined(TIM8_BASE)
+    if (timerIndex==TIMER8_INDEX) return 8;
+  #endif
+  #if defined(TIM9_BASE)
+    if (timerIndex==TIMER9_INDEX) return 9;
+  #endif
+  #if defined(TIM10_BASE)
+    if (timerIndex==TIMER10_INDEX) return 10;
+  #endif
+  #if defined(TIM11_BASE)
+    if (timerIndex==TIMER11_INDEX) return 11;
+  #endif
+  #if defined(TIM12_BASE)
+    if (timerIndex==TIMER12_INDEX) return 12;
+  #endif
+  #if defined(TIM13_BASE)
+    if (timerIndex==TIMER13_INDEX) return 13;
+  #endif
+  #if defined(TIM14_BASE)
+    if (timerIndex==TIMER14_INDEX) return 14;
+  #endif
+  #if defined(TIM15_BASE)
+    if (timerIndex==TIMER15_INDEX) return 15;
+  #endif
+  #if defined(TIM16_BASE)
+    if (timerIndex==TIMER16_INDEX) return 16;
+  #endif
+  #if defined(TIM17_BASE)
+    if (timerIndex==TIMER17_INDEX) return 17;
+  #endif
+  #if defined(TIM18_BASE)
+    if (timerIndex==TIMER18_INDEX) return 18;
+  #endif
+  #if defined(TIM19_BASE)
+    if (timerIndex==TIMER19_INDEX) return 19;
+  #endif
+  #if defined(TIM20_BASE)
+    if (timerIndex==TIMER20_INDEX) return 20;
+  #endif
+  #if defined(TIM21_BASE)
+    if (timerIndex==TIMER21_INDEX) return 21;
+  #endif
+  #if defined(TIM22_BASE)
+    if (timerIndex==TIMER22_INDEX) return 22;
+  #endif
+  return -1;
+}
+
+
+void printTimerCombination(int numPins, PinMap* timers[], int score) {
+  for (int i=0; i<numPins; i++) {
+    if (timers[i] == NP)
+      SimpleFOCDebug::print("NP");
+    else {
+      SimpleFOCDebug::print("TIM");
+      SimpleFOCDebug::print(getTimerNumber(get_timer_index((TIM_TypeDef*)timers[i]->peripheral)));
+      SimpleFOCDebug::print("-CH");
+      SimpleFOCDebug::print(STM_PIN_CHANNEL(timers[i]->function));
+      if (STM_PIN_INVERTED(timers[i]->function))
+        SimpleFOCDebug::print("N");
+    }
+    SimpleFOCDebug::print(" ");
+  }
+  SimpleFOCDebug::println("score: ", score);
+}
+
+#endif
+
+
+
+
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/stm32/stm32_mcu.h

@@ -0,0 +1,32 @@
+#ifndef STM32_DRIVER_MCU_DEF
+#define STM32_DRIVER_MCU_DEF
+#include "../../hardware_api.h"
+
+#if defined(_STM32_DEF_)
+
+// default pwm parameters
+#define _PWM_RESOLUTION 12 // 12bit
+#define _PWM_RANGE 4095.0f // 2^12 -1 = 4095
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
+// 6pwm parameters
+#define _HARDWARE_6PWM 1
+#define _SOFTWARE_6PWM 0
+#define _ERROR_6PWM -1
+
+
+typedef struct STM32DriverParams {
+  HardwareTimer* timers[6] = {NULL};
+  uint32_t channels[6];
+  long pwm_frequency;
+  float dead_zone;
+  uint8_t interface_type;
+} STM32DriverParams;
+
+// timer synchornisation functions
+void _stopTimers(HardwareTimer **timers_to_stop, int timer_num=6);
+void _startTimers(HardwareTimer **timers_to_start, int timer_num=6);
+
+#endif
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/teensy/teensy3_mcu.cpp

@@ -0,0 +1,228 @@
+#include "teensy_mcu.h"
+
+// if defined 
+// - Teensy 3.0 MK20DX128
+// - Teensy 3.1/3.2 MK20DX256
+// - Teensy 3.5 MK20DX128
+// - Teensy LC MKL26Z64
+// - Teensy 3.5 MK64FX512
+// - Teensy 3.6 MK66FX1M0
+#if defined(__arm__) && defined(CORE_TEENSY) &&  (defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MKL26Z64__) || defined(__MK64FX512__) || defined(__MK66FX1M0__))
+
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for Teensy 3.x")
+#pragma message("")
+
+// pin definition from https://github.com/PaulStoffregen/cores/blob/286511f3ec849a6c9e0ec8b73ad6a2fada52e44c/teensy3/pins_teensy.c
+#if defined(__MK20DX128__)
+#define FTM0_CH0_PIN 22
+#define FTM0_CH1_PIN 23
+#define FTM0_CH2_PIN  9
+#define FTM0_CH3_PIN 10
+#define FTM0_CH4_PIN  6
+#define FTM0_CH5_PIN 20
+#define FTM0_CH6_PIN 21
+#define FTM0_CH7_PIN  5
+#define FTM1_CH0_PIN  3
+#define FTM1_CH1_PIN  4
+#elif defined(__MK20DX256__)
+#define FTM0_CH0_PIN 22
+#define FTM0_CH1_PIN 23
+#define FTM0_CH2_PIN  9
+#define FTM0_CH3_PIN 10
+#define FTM0_CH4_PIN  6
+#define FTM0_CH5_PIN 20
+#define FTM0_CH6_PIN 21
+#define FTM0_CH7_PIN  5
+#define FTM1_CH0_PIN  3
+#define FTM1_CH1_PIN  4
+#define FTM2_CH0_PIN 32
+#define FTM2_CH1_PIN 25
+#elif defined(__MKL26Z64__)
+#define FTM0_CH0_PIN 22
+#define FTM0_CH1_PIN 23
+#define FTM0_CH2_PIN  9
+#define FTM0_CH3_PIN 10
+#define FTM0_CH4_PIN  6
+#define FTM0_CH5_PIN 20
+#define FTM1_CH0_PIN 16
+#define FTM1_CH1_PIN 17
+#define FTM2_CH0_PIN  3
+#define FTM2_CH1_PIN  4
+#elif defined(__MK64FX512__)
+#define FTM0_CH0_PIN 22
+#define FTM0_CH1_PIN 23
+#define FTM0_CH2_PIN  9
+#define FTM0_CH3_PIN 10
+#define FTM0_CH4_PIN  6
+#define FTM0_CH5_PIN 20
+#define FTM0_CH6_PIN 21
+#define FTM0_CH7_PIN  5
+#define FTM1_CH0_PIN  3
+#define FTM1_CH1_PIN  4
+#define FTM2_CH0_PIN 29
+#define FTM2_CH1_PIN 30
+#define FTM3_CH0_PIN  2
+#define FTM3_CH1_PIN 14
+#define FTM3_CH2_PIN  7
+#define FTM3_CH3_PIN  8
+#define FTM3_CH4_PIN 35
+#define FTM3_CH5_PIN 36
+#define FTM3_CH6_PIN 37
+#define FTM3_CH7_PIN 38
+#elif defined(__MK66FX1M0__)
+#define FTM0_CH0_PIN 22
+#define FTM0_CH1_PIN 23
+#define FTM0_CH2_PIN  9
+#define FTM0_CH3_PIN 10
+#define FTM0_CH4_PIN  6
+#define FTM0_CH5_PIN 20
+#define FTM0_CH6_PIN 21
+#define FTM0_CH7_PIN  5
+#define FTM1_CH0_PIN  3
+#define FTM1_CH1_PIN  4
+#define FTM2_CH0_PIN 29
+#define FTM2_CH1_PIN 30
+#define FTM3_CH0_PIN  2
+#define FTM3_CH1_PIN 14
+#define FTM3_CH2_PIN  7
+#define FTM3_CH3_PIN  8
+#define FTM3_CH4_PIN 35
+#define FTM3_CH5_PIN 36
+#define FTM3_CH6_PIN 37
+#define FTM3_CH7_PIN 38
+#define TPM1_CH0_PIN 16
+#define TPM1_CH1_PIN 17
+#endif
+
+int _findTimer( const int Ah, const int Al,  const int Bh, const int Bl, const int Ch, const int Cl){
+
+  if((Ah == FTM0_CH0_PIN && Al == FTM0_CH1_PIN) || 
+    (Ah == FTM0_CH2_PIN && Al == FTM0_CH3_PIN) ||
+    (Ah == FTM0_CH4_PIN && Al == FTM0_CH5_PIN) ){
+      if((Bh == FTM0_CH0_PIN && Bl == FTM0_CH1_PIN) || 
+        (Bh == FTM0_CH2_PIN && Bl == FTM0_CH3_PIN) ||
+        (Bh == FTM0_CH4_PIN && Bl == FTM0_CH5_PIN) ){
+         if((Ch == FTM0_CH0_PIN && Cl == FTM0_CH1_PIN) || 
+          (Ch == FTM0_CH2_PIN && Cl == FTM0_CH3_PIN) ||
+          (Ch == FTM0_CH4_PIN && Cl == FTM0_CH5_PIN) ){
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+                SIMPLEFOC_DEBUG("TEENSY-DRV: Using timer FTM0.");
+#endif
+            // timer FTM0 
+            return 0;
+        }
+      }
+  }
+
+  #ifdef FTM3_SC // if the board has FTM3 timer
+    if((Ah == FTM3_CH0_PIN && Al == FTM3_CH1_PIN) || 
+      (Ah == FTM3_CH2_PIN && Al == FTM3_CH3_PIN) ||
+      (Ah == FTM3_CH4_PIN && Al == FTM3_CH5_PIN) ){
+        if((Bh == FTM3_CH0_PIN && Bl == FTM3_CH1_PIN) || 
+          (Bh == FTM3_CH2_PIN && Bl == FTM3_CH3_PIN) ||
+          (Bh == FTM3_CH4_PIN && Bl == FTM3_CH5_PIN) ){
+          if((Ch == FTM3_CH0_PIN && Cl == FTM3_CH1_PIN) || 
+            (Ch == FTM3_CH2_PIN && Cl == FTM3_CH3_PIN) ||
+            (Ch == FTM3_CH4_PIN && Cl == FTM3_CH5_PIN) ){
+              // timer FTM3 
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+                SIMPLEFOC_DEBUG("TEENSY-DRV: Using timer FTM3.");
+#endif
+              return 3;
+          }
+        }
+    }
+  #endif
+  
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+  SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Pins not on timers FTM0 or FTM3!");
+#endif
+  return -1;
+
+}
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 6PWM setting
+// - hardware specific
+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) {
+  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
+  unsigned long pwm_freq = 2*pwm_frequency; // center-aligned pwm has 4 times lower freq
+  _setHighFrequency(pwm_freq, pinA_h);
+  _setHighFrequency(pwm_freq, pinA_l);
+  _setHighFrequency(pwm_freq, pinB_h);
+  _setHighFrequency(pwm_freq, pinB_l);
+  _setHighFrequency(pwm_freq, pinC_h);
+  _setHighFrequency(pwm_freq, pinC_l);
+
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA_h,pinA_l, pinB_h,pinB_l, pinC_h, pinC_l },
+    .pwm_frequency = pwm_frequency
+  };
+
+
+  int timer = _findTimer(pinA_h,pinA_l,pinB_h,pinB_l,pinC_h,pinC_l);
+  if(timer<0) return SIMPLEFOC_DRIVER_INIT_FAILED;
+
+  // find the best combination of prescalers and counter value
+  double dead_time = dead_zone/pwm_freq;
+  int prescaler = 1; // initial prescaler (1,4 or 16)
+  double count = 1; // inital count (1 - 63)
+  for (; prescaler<=16; prescaler*=4){
+    count = dead_time*((double)F_CPU)/((double)prescaler);
+    if(count < 64) break; // found the solution
+  }
+  count = _constrain(count, 1, 63);
+
+  // configure the timer 
+  if(timer==0){
+    // Configure FTM0
+    // // inverting and deadtime insertion for FTM1
+    FTM0_COMBINE = 0x00121212; // 0x2 - complemetary mode, 0x1 - dead timer insertion enabled
+
+    // Deadtime config
+    FTM0_DEADTIME = (int)count; // set counter - 1-63
+    FTM0_DEADTIME |= ((prescaler>1) << 7) | ((prescaler>4) << 6); // set prescaler (0b01 - 1, 0b10 - 4, 0b11 - 16)
+
+    // configure center aligned PWM
+    FTM0_SC = 0x00000028; // 0x2 - center-alignment, 0x8 - fixed clock freq
+  }else if(timer==3){
+    // Configure FTM3
+    // inverting and deadtime insertion for FTM1
+    FTM3_COMBINE = 0x00121212; // 0x2 - complemetary mode, 0x1 - dead timer insertion enabled
+
+    // Deadtime config
+    FTM3_DEADTIME = (int)count; // set counter - 1-63
+    FTM3_DEADTIME |= ((prescaler>1) << 7) | ((prescaler>4) << 6); // set prescaler (0b01 - 1, 0b10 - 4, 0b11 - 16)
+
+    // configure center aligned PWM
+    FTM3_SC = 0x00000028; // 0x2 - center-alignment, 0x8 - fixed clock freq
+  }
+  
+  return params;
+}
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 6PWM setting
+// - hardware specific
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params){
+  _UNUSED(phase_state);
+  // transform duty cycle from [0,1] to [0,255]
+  // phase A
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_a);
+
+  // phase B
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_b);
+  analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_b);
+
+  // phase C
+  analogWrite(((GenericDriverParams*)params)->pins[4], 255.0f*dc_c);
+  analogWrite(((GenericDriverParams*)params)->pins[5], 255.0f*dc_c);
+}
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp

@@ -0,0 +1,306 @@
+#include "teensy_mcu.h"
+#include "teensy4_mcu.h"
+#include "../../../communication/SimpleFOCDebug.h"
+
+// if defined 
+// - Teensy 4.0 
+// - Teensy 4.1 
+#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) )
+
+
+#pragma message("")
+#pragma message("SimpleFOC: compiling for Teensy 4.x")
+#pragma message("")
+
+
+// half_cycle of the PWM variable
+int half_cycle = 0;
+
+
+// function which finds the flexpwm instance for a pair of pins
+// if they do not belong to the same timer it returns a nullpointer
+IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){
+ 
+  const struct pwm_pin_info_struct *info;
+  if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL) {
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+    char s[60];
+    sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1);
+    SIMPLEFOC_DEBUG(s);
+#endif
+    return nullptr;
+  }
+  info = pwm_pin_info + pin;
+  // FlexPWM pin
+  IMXRT_FLEXPWM_t *flexpwm1,*flexpwm2;
+  switch ((info->module >> 4) & 3) {
+    case 0: flexpwm1 = &IMXRT_FLEXPWM1; break;
+    case 1: flexpwm1 = &IMXRT_FLEXPWM2; break;
+    case 2: flexpwm1 = &IMXRT_FLEXPWM3; break;
+    default: flexpwm1 = &IMXRT_FLEXPWM4;
+  }
+  
+  info = pwm_pin_info + pin1;
+  switch ((info->module >> 4) & 3) {
+    case 0: flexpwm2 = &IMXRT_FLEXPWM1; break;
+    case 1: flexpwm2 = &IMXRT_FLEXPWM2; break;
+    case 2: flexpwm2 = &IMXRT_FLEXPWM3; break;
+    default: flexpwm2 = &IMXRT_FLEXPWM4;
+  }
+  if(flexpwm1 == flexpwm2){
+    char s[60];
+    sprintf (s, "TEENSY-DRV: Pins: %d, %d on Flextimer %d.", pin, pin1, ((info->module >> 4) & 3) + 1);
+    SIMPLEFOC_DEBUG(s);
+    return flexpwm1;
+  } else {
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+    char s[60];
+    sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same Flextimer!", pin, pin1);
+    SIMPLEFOC_DEBUG(s);
+#endif
+    return nullptr; 
+  }
+}
+
+
+// function which finds the timer submodule for a pair of pins
+// if they do not belong to the same submodule it returns a -1
+int get_submodule(uint8_t pin, uint8_t pin1){
+ 
+  const struct pwm_pin_info_struct *info;
+  if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+    char s[60];
+    sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1);
+    SIMPLEFOC_DEBUG(s);
+#endif
+    return -1;
+  } 
+  
+  info = pwm_pin_info + pin;
+  int sm1 = info->module&0x3;
+  info = pwm_pin_info + pin1;
+  int sm2 = info->module&0x3;
+
+  if (sm1 == sm2) {
+    char s[60];
+    sprintf (s, "TEENSY-DRV: Pins: %d, %d on submodule %d.", pin, pin1, sm1);
+    SIMPLEFOC_DEBUG(s);
+    return sm1;
+  }  else  {
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+    char s[50];
+    sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same submodule!", pin, pin1);
+    SIMPLEFOC_DEBUG(s);
+#endif
+    return -1; 
+  }
+}
+
+
+// function which finds the timer submodule for a pair of pins
+// if they do not belong to the same submodule it returns a -1
+int get_inverted_channel(uint8_t pin, uint8_t pin1){
+ 
+  const struct pwm_pin_info_struct *info;
+  if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+    char s[60];
+    sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1);
+    SIMPLEFOC_DEBUG(s);
+#endif
+    return -1;
+  } 
+  
+  info = pwm_pin_info + pin;
+  int ch1 = info->channel;
+  info = pwm_pin_info + pin1;
+  int ch2 = info->channel;
+
+  if (ch1 != 1) {
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+    char s[60];
+    sprintf (s, "TEENSY-DRV: ERR: Pin: %d on channel %s - only A supported", pin1, ch1==2 ? "B" : "X");
+    SIMPLEFOC_DEBUG(s);
+#endif
+    return -1;
+  }  else  if (ch2 != 2) {
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+    char s[60];
+    sprintf (s, "TEENSY-DRV: ERR: Inverted pin: %d on channel %s - only B supported", pin1, ch2==1 ? "A" : "X");
+    SIMPLEFOC_DEBUG(s);
+#endif
+    return -1;
+  } else {
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+    char s[60];
+    sprintf (s, "TEENSY-DRV: Pin: %d on channel B inverted.", pin1);
+    SIMPLEFOC_DEBUG(s);
+#endif 
+return ch2;
+  } 
+}
+
+// Helper to set up A/B pair on a FlexPWM submodule.
+// can configure sync, prescale and B inversion.
+// sets the desired frequency of the PWM 
+// sets the center-aligned pwm
+void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long frequency, float dead_zone )
+{
+  int submodule_mask = 1 << submodule ;
+  flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ;  // stop it if its already running
+  flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ;  // clear load OK
+  
+
+  // calculate the counter and prescaler for the desired pwm frequency
+  uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f);
+	uint32_t prescale = 0;
+	//printf(" div=%lu\n", newdiv);
+	while (newdiv > 65535 && prescale < 7) {
+		newdiv = newdiv >> 1;
+		prescale = prescale + 1;
+	}
+	if (newdiv > 65535) {
+		newdiv = 65535;
+	} else if (newdiv < 2) {
+		newdiv = 2;
+	}
+
+  // the halfcycle of the PWM
+  half_cycle = int(newdiv/2.0f);
+  int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% 
+  int mid_pwm = int((half_cycle)/2.0f);
+
+  // setup the timer
+  // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h
+  flexpwm->SM[submodule].CTRL2 =  FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN |
+                                 FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(0) | FLEXPWM_SMCTRL2_FORCE_SEL(6);
+  flexpwm->SM[submodule].CTRL  = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ;
+  // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948
+  flexpwm->SM[submodule].OCTRL = 0;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ;
+  flexpwm->SM[submodule].DTCNT0 = dead_time ;  // should try this out (deadtime control)
+  flexpwm->SM[submodule].DTCNT1 = dead_time ;  // should try this out (deadtime control)
+  // flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010)  ; // sync trig out on VAL1 match.
+  flexpwm->SM[submodule].INIT = -half_cycle;      // count from -HALFCYCLE to +HALFCYCLE
+  flexpwm->SM[submodule].VAL0 = 0 ;
+  flexpwm->SM[submodule].VAL1 = half_cycle ;
+  flexpwm->SM[submodule].VAL2 = -mid_pwm  ;
+  flexpwm->SM[submodule].VAL3 = +mid_pwm  ;
+  // flexpwm->SM[submodule].VAL4 = -mid_pwm ;
+  // flexpwm->SM[submodule].VAL5 = +mid_pwm  ;
+  
+  flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ;  // loading reenabled
+  flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ;   // start it running
+}
+
+
+// staring the PWM on A and B channels of the submodule
+void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule)
+{
+  int submodule_mask = 1 << submodule ;
+
+  flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output
+  flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output
+}
+
+
+
+// PWM setting on the high and low pair of the PWM channels
+void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){
+  
+  int mid_pwm = int((half_cycle)/2.0f);
+  int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm;
+
+  flexpwm->SM[submodule].VAL2 =  count_pwm; // A on
+  flexpwm->SM[submodule].VAL3 =  -count_pwm  ; // A off
+  // flexpwm->SM[submodule].VAL4 = - count_pwm  ; // B off  (assuming B inverted)
+  // flexpwm->SM[submodule].VAL5 = +  count_pwm ; // B on
+  flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<<submodule) ;  // signal new values
+}
+
+
+
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 6PWM setting
+// - hardware specific
+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) {
+  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
+  
+  IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC;
+  int submoduleA,submoduleB,submoduleC;
+  int inverted_channelA,inverted_channelB,inverted_channelC;
+  flexpwmA = get_flexpwm(pinA_h,pinA_l);
+  submoduleA = get_submodule(pinA_h,pinA_l);
+  inverted_channelA = get_inverted_channel(pinA_h,pinA_l);
+  flexpwmB = get_flexpwm(pinB_h,pinB_l);
+  submoduleB = get_submodule(pinB_h,pinB_l);
+  inverted_channelB = get_inverted_channel(pinB_h,pinB_l);
+  flexpwmC = get_flexpwm(pinC_h,pinC_l);
+  submoduleC = get_submodule(pinC_h,pinC_l);
+  inverted_channelC = get_inverted_channel(pinC_h,pinC_l);
+
+  if((flexpwmA == nullptr) || (flexpwmB == nullptr) || (flexpwmC == nullptr) ){
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+      SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer problem - failed driver config!");
+#endif
+    return SIMPLEFOC_DRIVER_INIT_FAILED;
+  }
+  if((submoduleA < 0) || (submoduleB < 0) || (submoduleC < 0) ){
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+      SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer submodule problem - failed driver config!");
+#endif
+    return SIMPLEFOC_DRIVER_INIT_FAILED;
+  }
+  if((inverted_channelA < 0) || (inverted_channelB < 0) || (inverted_channelC < 0) ){
+#ifdef SIMPLEFOC_TEENSY_DEBUG
+      SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer channel problem - failed driver config!");
+#endif
+    return SIMPLEFOC_DRIVER_INIT_FAILED;
+  }
+
+
+  Teensy4DriverParams* params = new Teensy4DriverParams {
+    .flextimers = { flexpwmA,flexpwmB, flexpwmC},
+    .submodules = { submoduleA, submoduleB, submoduleC},
+    .pwm_frequency = pwm_frequency,
+    .dead_zone = dead_zone
+  };
+
+  // Configure FlexPWM units, each driving A/B pair, B inverted.
+  // full speed about 80kHz, prescale 2 (div by 4) gives 20kHz
+  setup_pwm_pair (flexpwmA, submoduleA, pwm_frequency, dead_zone) ;  // this is the master, internally synced
+  setup_pwm_pair (flexpwmB, submoduleB, pwm_frequency, dead_zone) ;   // others externally synced
+  setup_pwm_pair (flexpwmC, submoduleC, pwm_frequency, dead_zone) ;
+  delayMicroseconds (100) ;
+
+  startup_pwm_pair (flexpwmA, submoduleA) ;
+  startup_pwm_pair (flexpwmB, submoduleB) ;
+  startup_pwm_pair (flexpwmC, submoduleC) ;
+
+  delayMicroseconds(50) ;
+  // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates.
+  *portConfigRegister(pinA_h) = pwm_pin_info[pinA_h].muxval ;
+  *portConfigRegister(pinA_l) = pwm_pin_info[pinA_l].muxval ;
+  *portConfigRegister(pinB_h) = pwm_pin_info[pinB_h].muxval ;
+  *portConfigRegister(pinB_l) = pwm_pin_info[pinB_l].muxval ;
+  *portConfigRegister(pinC_h) = pwm_pin_info[pinC_h].muxval ;
+  *portConfigRegister(pinC_l) = pwm_pin_info[pinC_l].muxval ;
+  
+  return params;
+}
+
+
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 6PWM setting
+// - hardware specific
+void _writeDutyCycle6PWM(float dc_a,  float dc_b, float dc_c, PhaseState *phase_state, void* params){
+  _UNUSED(phase_state);
+  write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a);
+  write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b);
+  write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c);
+}
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/teensy/teensy4_mcu.h

@@ -0,0 +1,110 @@
+#include "teensy_mcu.h"
+
+// if defined 
+// - Teensy 4.0 
+// - Teensy 4.1 
+#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) )
+
+// teensy 4 driver configuration parameters  
+typedef struct Teensy4DriverParams {
+  IMXRT_FLEXPWM_t* flextimers[3] = {NULL};
+  int submodules[3];
+  long pwm_frequency;
+  float dead_zone;
+} Teensy4DriverParams;
+
+
+// pin definition from https://github.com/PaulStoffregen/cores/blob/master/teensy4/pwm.c
+struct pwm_pin_info_struct {
+  uint8_t type;    // 0=no pwm, 1=flexpwm, 2=quad
+  uint8_t module;  // 0-3, 0-3
+  uint8_t channel; // 0=X, 1=A, 2=B
+  uint8_t muxval;  //
+};
+#define M(a, b) ((((a) - 1) << 4) | (b))
+#if defined(__IMXRT1062__)
+const struct pwm_pin_info_struct pwm_pin_info[] = {
+  {1, M(1, 1), 0, 4},  // FlexPWM1_1_X   0  // AD_B0_03
+  {1, M(1, 0), 0, 4},  // FlexPWM1_0_X   1  // AD_B0_02
+  {1, M(4, 2), 1, 1},  // FlexPWM4_2_A   2  // EMC_04
+  {1, M(4, 2), 2, 1},  // FlexPWM4_2_B   3  // EMC_05
+  {1, M(2, 0), 1, 1},  // FlexPWM2_0_A   4  // EMC_06
+  {1, M(2, 1), 1, 1},  // FlexPWM2_1_A   5  // EMC_08
+  {1, M(2, 2), 1, 2},  // FlexPWM2_2_A   6  // B0_10
+  {1, M(1, 3), 2, 6},  // FlexPWM1_3_B   7  // B1_01
+  {1, M(1, 3), 1, 6},  // FlexPWM1_3_A   8  // B1_00
+  {1, M(2, 2), 2, 2},  // FlexPWM2_2_B   9  // B0_11
+  {2, M(1, 0), 0, 1},  // QuadTimer1_0  10  // B0_00
+  {2, M(1, 2), 0, 1},  // QuadTimer1_2  11  // B0_02
+  {2, M(1, 1), 0, 1},  // QuadTimer1_1  12  // B0_01
+  {2, M(2, 0), 0, 1},  // QuadTimer2_0  13  // B0_03
+  {2, M(3, 2), 0, 1},  // QuadTimer3_2  14  // AD_B1_02
+  {2, M(3, 3), 0, 1},  // QuadTimer3_3  15  // AD_B1_03
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {2, M(3, 1), 0, 1},  // QuadTimer3_1  18  // AD_B1_01
+  {2, M(3, 0), 0, 1},  // QuadTimer3_0  19  // AD_B1_00
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {1, M(4, 0), 1, 1},  // FlexPWM4_0_A  22  // AD_B1_08
+  {1, M(4, 1), 1, 1},  // FlexPWM4_1_A  23  // AD_B1_09
+  {1, M(1, 2), 0, 4},  // FlexPWM1_2_X  24  // AD_B0_12
+  {1, M(1, 3), 0, 4},  // FlexPWM1_3_X  25  // AD_B0_13
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {1, M(3, 1), 2, 1},  // FlexPWM3_1_B  28  // EMC_32
+  {1, M(3, 1), 1, 1},  // FlexPWM3_1_A  29  // EMC_31
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {1, M(2, 0), 2, 1},  // FlexPWM2_0_B  33  // EMC_07
+#ifdef ARDUINO_TEENSY40
+  {1, M(1, 1), 2, 1},  // FlexPWM1_1_B  34  // SD_B0_03
+  {1, M(1, 1), 1, 1},  // FlexPWM1_1_A  35  // SD_B0_02
+  {1, M(1, 0), 2, 1},  // FlexPWM1_0_B  36  // SD_B0_01
+  {1, M(1, 0), 1, 1},  // FlexPWM1_0_A  37  // SD_B0_00
+  {1, M(1, 2), 2, 1},  // FlexPWM1_2_B  38  // SD_B0_05
+  {1, M(1, 2), 1, 1},  // FlexPWM1_2_A  39  // SD_B0_04
+#endif
+#ifdef ARDUINO_TEENSY41
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {1, M(2, 3), 1, 6},  // FlexPWM2_3_A  36  // B1_00
+  {1, M(2, 3), 2, 6},  // FlexPWM2_3_B  37  // B1_01
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {0, M(1, 0), 0, 0},
+  {1, M(1, 1), 2, 1},  // FlexPWM1_1_B  42  // SD_B0_03
+  {1, M(1, 1), 1, 1},  // FlexPWM1_1_A  43  // SD_B0_02
+  {1, M(1, 0), 2, 1},  // FlexPWM1_0_B  44  // SD_B0_01
+  {1, M(1, 0), 1, 1},  // FlexPWM1_0_A  45  // SD_B0_00
+  {1, M(1, 2), 2, 1},  // FlexPWM1_2_B  46  // SD_B0_05
+  {1, M(1, 2), 1, 1},  // FlexPWM1_2_A  47  // SD_B0_04
+  {0, M(1, 0), 0, 0},  // duplicate FlexPWM1_0_B
+  {0, M(1, 0), 0, 0},  // duplicate FlexPWM1_2_A
+  {0, M(1, 0), 0, 0},  // duplicate FlexPWM1_2_B
+  {1, M(3, 3), 2, 1},  // FlexPWM3_3_B  51  // EMC_22
+  {0, M(1, 0), 0, 0},  // duplicate FlexPWM1_1_B
+  {0, M(1, 0), 0, 0},  // duplicate FlexPWM1_1_A
+  {1, M(3, 0), 1, 1},  // FlexPWM3_0_A  54  // EMC_29
+#endif
+#ifdef ARDUINO_TEENSY_MICROMOD
+  {1, M(1, 1), 2, 1},  // FlexPWM1_1_B  34  // SD_B0_03
+  {1, M(1, 1), 1, 1},  // FlexPWM1_1_A  35  // SD_B0_02
+  {1, M(1, 0), 2, 1},  // FlexPWM1_0_B  36  // SD_B0_01
+  {1, M(1, 0), 1, 1},  // FlexPWM1_0_A  37  // SD_B0_00
+  {1, M(1, 2), 1, 1},  // FlexPWM1_2_A  38  // SD_B0_04
+  {1, M(1, 2), 2, 1},  // FlexPWM1_2_B  39  // SD_B0_05
+  {2, M(2, 1), 0, 1},  // QuadTimer2_1  40  // B0_04
+  {2, M(2, 2), 0, 1},  // QuadTimer2_2  41  // B0_05
+  {0, M(1, 0), 0, 0},  // duplicate QuadTimer3_0
+  {0, M(1, 0), 0, 0},  // duplicate QuadTimer3_1
+  {0, M(1, 0), 0, 0},  // duplicate QuadTimer3_2
+  {2, M(4, 0), 0, 1},  // QuadTimer4_0  45  // B0_09
+#endif
+};
+
+#endif
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/teensy/teensy_mcu.cpp

@@ -0,0 +1,116 @@
+#include "teensy_mcu.h"
+
+#if defined(__arm__) && defined(CORE_TEENSY)
+
+//  configure High PWM frequency
+void _setHighFrequency(const long freq, const int pin){
+  analogWrite(pin, 0);
+  analogWriteFrequency(pin, freq);
+}
+
+
+// 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
+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
+  _setHighFrequency(pwm_frequency, pinA);
+  _setHighFrequency(pwm_frequency, pinB);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - BLDC motor - 3PWM setting
+// - hardware speciffic
+void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) {
+  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);
+  _setHighFrequency(pwm_frequency, pinB);
+  _setHighFrequency(pwm_frequency, pinC);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB, pinC },
+    .pwm_frequency = pwm_frequency
+  };
+  return params;
+}
+
+// function setting the high pwm frequency to the supplied pins
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD) {
+  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);
+  _setHighFrequency(pwm_frequency, pinB);
+  _setHighFrequency(pwm_frequency, pinC);
+  _setHighFrequency(pwm_frequency, pinD);
+  GenericDriverParams* params = new GenericDriverParams {
+    .pins = { pinA, pinB, pinC, pinD },
+    .pwm_frequency = pwm_frequency
+  };
+  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
+void _writeDutyCycle2PWM(float dc_a,  float dc_b, void* params) {
+  // transform duty cycle from [0,1] to [0,255]
+  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
+void _writeDutyCycle3PWM(float dc_a,  float dc_b, float dc_c, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c);
+}
+
+// function setting the pwm duty cycle to the hardware
+// - Stepper motor - 4PWM setting
+// - hardware speciffic
+void _writeDutyCycle4PWM(float dc_1a,  float dc_1b, float dc_2a, float dc_2b, void* params){
+  // transform duty cycle from [0,1] to [0,255]
+  analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a);
+  analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b);
+  analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a);
+  analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b);
+}
+
+#endif

firmware/lib/Arduino-FOC/src/drivers/hardware_specific/teensy/teensy_mcu.h

@@ -0,0 +1,14 @@
+#include "../../hardware_api.h"
+
+#if defined(__arm__) && defined(CORE_TEENSY)
+
+#define _PWM_FREQUENCY 25000 // 25khz
+#define _PWM_FREQUENCY_MAX 50000 // 50khz
+
+// debugging output 
+#define SIMPLEFOC_TEENSY_DEBUG 
+
+//  configure High PWM frequency
+void _setHighFrequency(const long freq, const int pin);
+
+#endif