try to fix submodule

firmware/lib/Arduino-FOC/src/BLDCMotor.cpp

@@ -0,0 +1,736 @@
+#include "BLDCMotor.h"
+#include "./communication/SimpleFOCDebug.h"
+
+
+// see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5
+// each is 60 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
+int trap_120_map[6][3] = {
+    {_HIGH_IMPEDANCE,1,-1},
+    {-1,1,_HIGH_IMPEDANCE},
+    {-1,_HIGH_IMPEDANCE,1},
+    {_HIGH_IMPEDANCE,-1,1},
+    {1,-1,_HIGH_IMPEDANCE},
+    {1,_HIGH_IMPEDANCE,-1} 
+};
+
+// see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
+// each is 30 degrees with values for 3 phases of 1=positive -1=negative 0=high-z
+int trap_150_map[12][3] = {
+    {_HIGH_IMPEDANCE,1,-1},
+    {-1,1,-1},
+    {-1,1,_HIGH_IMPEDANCE},
+    {-1,1,1},
+    {-1,_HIGH_IMPEDANCE,1},
+    {-1,-1,1},
+    {_HIGH_IMPEDANCE,-1,1},
+    {1,-1,1},
+    {1,-1,_HIGH_IMPEDANCE},
+    {1,-1,-1},
+    {1,_HIGH_IMPEDANCE,-1},
+    {1,1,-1} 
+};
+
+// BLDCMotor( int pp , float R)
+// - pp            - pole pair number
+// - R             - motor phase resistance
+// - KV            - motor kv rating (rmp/v)
+// - L             - motor phase inductance
+BLDCMotor::BLDCMotor(int pp, float _R, float _KV, float _inductance)
+: FOCMotor()
+{
+  // save pole pairs number
+  pole_pairs = pp;
+  // save phase resistance number
+  phase_resistance = _R;
+  // save back emf constant KV = 1/KV
+  // 1/sqrt(2) - rms value
+  KV_rating = NOT_SET;
+  if (_isset(_KV))
+    KV_rating = _KV*_SQRT2;
+  // save phase inductance
+  phase_inductance = _inductance;
+
+  // torque control type is voltage by default
+  torque_controller = TorqueControlType::voltage;
+}
+
+
+/**
+	Link the driver which controls the motor
+*/
+void BLDCMotor::linkDriver(BLDCDriver* _driver) {
+  driver = _driver;
+}
+
+// init hardware pins
+void BLDCMotor::init() {
+  if (!driver || !driver->initialized) {
+    motor_status = FOCMotorStatus::motor_init_failed;
+    SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized");
+    return;
+  }
+  motor_status = FOCMotorStatus::motor_initializing;
+  SIMPLEFOC_DEBUG("MOT: Init");
+
+  // sanity check for the voltage limit configuration
+  if(voltage_limit > driver->voltage_limit) voltage_limit =  driver->voltage_limit;
+  // constrain voltage for sensor alignment
+  if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit;
+
+  // update the controller limits
+  if(current_sense){
+    // current control loop controls voltage
+    PID_current_q.limit = voltage_limit;
+    PID_current_d.limit = voltage_limit;
+  }
+  if(_isset(phase_resistance) || torque_controller != TorqueControlType::voltage){
+    // velocity control loop controls current
+    PID_velocity.limit = current_limit;
+  }else{
+    // velocity control loop controls the voltage
+    PID_velocity.limit = voltage_limit;
+  }
+  P_angle.limit = velocity_limit;
+
+  // if using open loop control, set a CW as the default direction if not already set
+  if ((controller==MotionControlType::angle_openloop
+     ||controller==MotionControlType::velocity_openloop)
+     && (sensor_direction == Direction::UNKNOWN)) {
+      sensor_direction = Direction::CW;
+  }
+
+  _delay(500);
+  // enable motor
+  SIMPLEFOC_DEBUG("MOT: Enable driver.");
+  enable();
+  _delay(500);
+  motor_status = FOCMotorStatus::motor_uncalibrated;
+}
+
+
+// disable motor driver
+void BLDCMotor::disable()
+{
+  // set zero to PWM
+  driver->setPwm(0, 0, 0);
+  // disable the driver
+  driver->disable();
+  // motor status update
+  enabled = 0;
+}
+// enable motor driver
+void BLDCMotor::enable()
+{
+  // enable the driver
+  driver->enable();
+  // set zero to PWM
+  driver->setPwm(0, 0, 0);
+  // motor status update
+  enabled = 1;
+}
+
+/**
+  FOC functions
+*/
+// FOC initialization function
+int  BLDCMotor::initFOC() {
+  int exit_flag = 1;
+
+  motor_status = FOCMotorStatus::motor_calibrating;
+
+  // align motor if necessary
+  // alignment necessary for encoders!
+  // sensor and motor alignment - can be skipped
+  // by setting motor.sensor_direction and motor.zero_electric_angle
+  _delay(500);
+  if(sensor){
+    exit_flag *= alignSensor();
+    // added the shaft_angle update
+    sensor->update();
+    shaft_angle = shaftAngle();
+  }else {
+    exit_flag = 0; // no FOC without sensor
+    SIMPLEFOC_DEBUG("MOT: No sensor.");
+  }
+
+  // aligning the current sensor - can be skipped
+  // checks if driver phases are the same as current sense phases
+  // and checks the direction of measuremnt.
+  _delay(500);
+  if(exit_flag){
+    if(current_sense){ 
+      if (!current_sense->initialized) {
+        motor_status = FOCMotorStatus::motor_calib_failed;
+        SIMPLEFOC_DEBUG("MOT: Init FOC error, current sense not initialized");
+        exit_flag = 0;
+      }else{
+        exit_flag *= alignCurrentSense();
+      }
+    }
+    else { SIMPLEFOC_DEBUG("MOT: No current sense."); }
+  }
+
+  if(exit_flag){
+    SIMPLEFOC_DEBUG("MOT: Ready.");
+    motor_status = FOCMotorStatus::motor_ready;
+  }else{
+    SIMPLEFOC_DEBUG("MOT: Init FOC failed.");
+    motor_status = FOCMotorStatus::motor_calib_failed;
+    disable();
+  }
+
+  return exit_flag;
+}
+
+// Calibarthe the motor and current sense phases
+int BLDCMotor::alignCurrentSense() {
+  int exit_flag = 1; // success
+
+  SIMPLEFOC_DEBUG("MOT: Align current sense.");
+
+  // align current sense and the driver
+  exit_flag = current_sense->driverAlign(voltage_sensor_align);
+  if(!exit_flag){
+    // error in current sense - phase either not measured or bad connection
+    SIMPLEFOC_DEBUG("MOT: Align error!");
+    exit_flag = 0;
+  }else{
+    // output the alignment status flag
+    SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag);
+  }
+
+  return exit_flag > 0;
+}
+
+// Encoder alignment to electrical 0 angle
+int BLDCMotor::alignSensor() {
+  int exit_flag = 1; //success
+  SIMPLEFOC_DEBUG("MOT: Align sensor.");
+
+  // check if sensor needs zero search
+  if(sensor->needsSearch()) exit_flag = absoluteZeroSearch();
+  // stop init if not found index
+  if(!exit_flag) return exit_flag;
+
+  // if unknown natural direction
+  if(sensor_direction==Direction::UNKNOWN){
+
+    // find natural direction
+    // move one electrical revolution forward
+    for (int i = 0; i <=500; i++ ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f;
+      setPhaseVoltage(voltage_sensor_align, 0,  angle);
+	    sensor->update();
+      _delay(2);
+    }
+    // take and angle in the middle
+    sensor->update();
+    float mid_angle = sensor->getAngle();
+    // move one electrical revolution backwards
+    for (int i = 500; i >=0; i-- ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f ;
+      setPhaseVoltage(voltage_sensor_align, 0,  angle);
+	    sensor->update();
+      _delay(2);
+    }
+    sensor->update();
+    float end_angle = sensor->getAngle();
+    setPhaseVoltage(0, 0, 0);
+    _delay(200);
+    // determine the direction the sensor moved
+    float moved =  fabs(mid_angle - end_angle);
+    if (moved<MIN_ANGLE_DETECT_MOVEMENT) { // minimum angle to detect movement
+      SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
+      return 0; // failed calibration
+    } else if (mid_angle < end_angle) {
+      SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW");
+      sensor_direction = Direction::CCW;
+    } else{
+      SIMPLEFOC_DEBUG("MOT: sensor_direction==CW");
+      sensor_direction = Direction::CW;
+    }
+    // check pole pair number
+    if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
+      SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
+    } else {
+      SIMPLEFOC_DEBUG("MOT: PP check: OK!");
+    }
+
+  } else { SIMPLEFOC_DEBUG("MOT: Skip dir calib."); }
+
+  // zero electric angle not known
+  if(!_isset(zero_electric_angle)){
+    // align the electrical phases of the motor and sensor
+    // set angle -90(270 = 3PI/2) degrees
+    setPhaseVoltage(voltage_sensor_align, 0,  _3PI_2);
+    _delay(700);
+    // read the sensor
+    sensor->update();
+    // get the current zero electric angle
+    zero_electric_angle = 0;
+    zero_electric_angle = electricalAngle();
+    //zero_electric_angle =  _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs));
+    _delay(20);
+    if(monitor_port){
+      SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
+    }
+    // stop everything
+    setPhaseVoltage(0, 0, 0);
+    _delay(200);
+  } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); }
+  return exit_flag;
+}
+
+// Encoder alignment the absolute zero angle
+// - to the index
+int BLDCMotor::absoluteZeroSearch() {
+  // sensor precision: this is all ok, as the search happens near the 0-angle, where the precision
+  //                    of float is sufficient.
+  SIMPLEFOC_DEBUG("MOT: Index search...");
+  // search the absolute zero with small velocity
+  float limit_vel = velocity_limit;
+  float limit_volt = voltage_limit;
+  velocity_limit = velocity_index_search;
+  voltage_limit = voltage_sensor_align;
+  shaft_angle = 0;
+  while(sensor->needsSearch() && shaft_angle < _2PI){
+    angleOpenloop(1.5f*_2PI);
+    // call important for some sensors not to loose count
+    // not needed for the search
+    sensor->update();
+  }
+  // disable motor
+  setPhaseVoltage(0, 0, 0);
+  // reinit the limits
+  velocity_limit = limit_vel;
+  voltage_limit = limit_volt;
+  // check if the zero found
+  if(monitor_port){
+    if(sensor->needsSearch()) { SIMPLEFOC_DEBUG("MOT: Error: Not found!"); }
+    else { SIMPLEFOC_DEBUG("MOT: Success!"); }
+  }
+  return !sensor->needsSearch();
+}
+
+// Iterative function looping FOC algorithm, setting Uq on the Motor
+// The faster it can be run the better
+void BLDCMotor::loopFOC() {
+  // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
+  //                 of full rotations otherwise.
+  if (sensor) sensor->update();
+
+  // if open-loop do nothing
+  if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
+  
+  // if disabled do nothing
+  if(!enabled) return;
+
+  // Needs the update() to be called first
+  // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() 
+  // which is in range 0-2PI
+  electrical_angle = electricalAngle();
+  switch (torque_controller) {
+    case TorqueControlType::voltage:
+      // no need to do anything really
+      break;
+    case TorqueControlType::dc_current:
+      if(!current_sense) return;
+      // read overall current magnitude
+      current.q = current_sense->getDCCurrent(electrical_angle);
+      // filter the value values
+      current.q = LPF_current_q(current.q);
+      // calculate the phase voltage
+      voltage.q = PID_current_q(current_sp - current.q);
+      // d voltage  - lag compensation
+      if(_isset(phase_inductance)) voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      else voltage.d = 0;
+      break;
+    case TorqueControlType::foc_current:
+      if(!current_sense) return;
+      // read dq currents
+      current = current_sense->getFOCCurrents(electrical_angle);
+      // filter values
+      current.q = LPF_current_q(current.q);
+      current.d = LPF_current_d(current.d);
+      // calculate the phase voltages
+      voltage.q = PID_current_q(current_sp - current.q);
+      voltage.d = PID_current_d(-current.d);
+      // d voltage - lag compensation - TODO verify
+      // if(_isset(phase_inductance)) voltage.d = _constrain( voltage.d - current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      break;
+    default:
+      // no torque control selected
+      SIMPLEFOC_DEBUG("MOT: no torque control selected!");
+      break;
+  }
+
+  // set the phase voltage - FOC heart function :)
+  setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
+}
+
+// Iterative function running outer loop of the FOC algorithm
+// Behavior of this function is determined by the motor.controller variable
+// It runs either angle, velocity or torque loop
+// - needs to be called iteratively it is asynchronous function
+// - if target is not set it uses motor.target value
+void BLDCMotor::move(float new_target) {
+
+  // downsampling (optional)
+  if(motion_cnt++ < motion_downsample) return;
+  motion_cnt = 0;
+
+  // shaft angle/velocity need the update() to be called first
+  // get shaft angle
+  // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float
+  //                        For this reason it is NOT precise when the angles become large.
+  //                        Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem
+  //                        when switching to a 2-component representation.
+  if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) 
+    shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode
+  // get angular velocity  TODO the velocity reading probably also shouldn't happen in open loop modes?
+  shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated
+
+  // if disabled do nothing
+  if(!enabled) return;
+  // set internal target variable
+  if(_isset(new_target)) target = new_target;
+  
+  // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV)
+  if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS;
+  // estimate the motor current if phase reistance available and current_sense not available
+  if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance;
+
+  // upgrade the current based voltage limit
+  switch (controller) {
+    case MotionControlType::torque:
+      if(torque_controller == TorqueControlType::voltage){ // if voltage torque control
+        if(!_isset(phase_resistance))  voltage.q = target;
+        else  voltage.q =  target*phase_resistance + voltage_bemf;
+        voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
+        // set d-component (lag compensation if known inductance)
+        if(!_isset(phase_inductance)) voltage.d = 0;
+        else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      }else{
+        current_sp = target; // if current/foc_current torque control
+      }
+      break;
+    case MotionControlType::angle:
+      // TODO sensor precision: this calculation is not numerically precise. The target value cannot express precise positions when
+      //                        the angles are large. This results in not being able to command small changes at high position values.
+      //                        to solve this, the delta-angle has to be calculated in a numerically precise way.
+      // angle set point
+      shaft_angle_sp = target;
+      // calculate velocity set point
+      shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
+      shaft_angle_sp = _constrain(shaft_angle_sp,-velocity_limit, velocity_limit);
+      // calculate the torque command - sensor precision: this calculation is ok, but based on bad value from previous calculation
+      current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
+      // if torque controlled through voltage
+      if(torque_controller == TorqueControlType::voltage){
+        // use voltage if phase-resistance not provided
+        if(!_isset(phase_resistance))  voltage.q = current_sp;
+        else  voltage.q =  _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+        // set d-component (lag compensation if known inductance)
+        if(!_isset(phase_inductance)) voltage.d = 0;
+        else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      }
+      break;
+    case MotionControlType::velocity:
+      // velocity set point - sensor precision: this calculation is numerically precise.
+      shaft_velocity_sp = target;
+      // calculate the torque command
+      current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
+      // if torque controlled through voltage control
+      if(torque_controller == TorqueControlType::voltage){
+        // use voltage if phase-resistance not provided
+        if(!_isset(phase_resistance))  voltage.q = current_sp;
+        else  voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+        // set d-component (lag compensation if known inductance)
+        if(!_isset(phase_inductance)) voltage.d = 0;
+        else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      }
+      break;
+    case MotionControlType::velocity_openloop:
+      // velocity control in open loop - sensor precision: this calculation is numerically precise.
+      shaft_velocity_sp = target;
+      voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
+      voltage.d = 0;
+      break;
+    case MotionControlType::angle_openloop:
+      // angle control in open loop - 
+      // TODO sensor precision: this calculation NOT numerically precise, and subject
+      //                        to the same problems in small set-point changes at high angles 
+      //                        as the closed loop version.
+      shaft_angle_sp = target;
+      voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
+      voltage.d = 0;
+      break;
+  }
+}
+
+
+// Method using FOC to set Uq and Ud to the motor at the optimal angle
+// Function implementing Space Vector PWM and Sine PWM algorithms
+//
+// Function using sine approximation
+// regular sin + cos ~300us    (no memory usage)
+// approx  _sin + _cos ~110us  (400Byte ~ 20% of memory)
+void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
+
+  float center;
+  int sector;
+  float _ca,_sa;
+
+  switch (foc_modulation)
+  {
+    case FOCModulationType::Trapezoid_120 :
+      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 5
+      // determine the sector
+      sector = 6 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes
+      // centering the voltages around either
+      // modulation_centered == true > driver.voltage_limit/2
+      // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
+      center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
+
+      if(trap_120_map[sector][0]  == _HIGH_IMPEDANCE){
+        Ua= center;
+        Ub = trap_120_map[sector][1] * Uq + center;
+        Uc = trap_120_map[sector][2] * Uq + center;
+        driver->setPhaseState(PhaseState::PHASE_OFF, PhaseState::PHASE_ON, PhaseState::PHASE_ON); // disable phase if possible
+      }else if(trap_120_map[sector][1]  == _HIGH_IMPEDANCE){
+        Ua = trap_120_map[sector][0] * Uq + center;
+        Ub = center;
+        Uc = trap_120_map[sector][2] * Uq + center;
+        driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_OFF, PhaseState::PHASE_ON);// disable phase if possible
+      }else{
+        Ua = trap_120_map[sector][0] * Uq + center;
+        Ub = trap_120_map[sector][1] * Uq + center;
+        Uc = center;
+        driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_OFF);// disable phase if possible
+      }
+
+    break;
+
+    case FOCModulationType::Trapezoid_150 :
+      // see https://www.youtube.com/watch?v=InzXA7mWBWE Slide 8
+      // determine the sector
+      sector = 12 * (_normalizeAngle(angle_el + _PI_6 ) / _2PI); // adding PI/6 to align with other modes
+      // centering the voltages around either
+      // modulation_centered == true > driver.voltage_limit/2
+      // modulation_centered == false > or Adaptable centering, all phases drawn to 0 when Uq=0
+      center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
+
+      if(trap_150_map[sector][0]  == _HIGH_IMPEDANCE){
+        Ua= center;
+        Ub = trap_150_map[sector][1] * Uq + center;
+        Uc = trap_150_map[sector][2] * Uq + center;
+        driver->setPhaseState(PhaseState::PHASE_OFF, PhaseState::PHASE_ON, PhaseState::PHASE_ON); // disable phase if possible
+      }else if(trap_150_map[sector][1]  == _HIGH_IMPEDANCE){
+        Ua = trap_150_map[sector][0] * Uq + center;
+        Ub = center;
+        Uc = trap_150_map[sector][2] * Uq + center;
+        driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_OFF, PhaseState::PHASE_ON); // disable phase if possible
+      }else if(trap_150_map[sector][2]  == _HIGH_IMPEDANCE){
+        Ua = trap_150_map[sector][0] * Uq + center;
+        Ub = trap_150_map[sector][1] * Uq + center;
+        Uc = center;
+        driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_OFF); // disable phase if possible
+      }else{
+        Ua = trap_150_map[sector][0] * Uq + center;
+        Ub = trap_150_map[sector][1] * Uq + center;
+        Uc = trap_150_map[sector][2] * Uq + center;
+        driver->setPhaseState(PhaseState::PHASE_ON, PhaseState::PHASE_ON, PhaseState::PHASE_ON); // enable all phases
+      }
+
+    break;
+
+    case FOCModulationType::SinePWM :
+      // Sinusoidal PWM modulation
+      // Inverse Park + Clarke transformation
+      _sincos(angle_el, &_sa, &_ca);
+
+      // Inverse park transform
+      Ualpha =  _ca * Ud - _sa * Uq;  // -sin(angle) * Uq;
+      Ubeta =  _sa * Ud + _ca * Uq;    //  cos(angle) * Uq;
+
+      // center = modulation_centered ? (driver->voltage_limit)/2 : Uq;
+      center = driver->voltage_limit/2;
+      // Clarke transform
+      Ua = Ualpha + center;
+      Ub = -0.5f * Ualpha  + _SQRT3_2 * Ubeta + center;
+      Uc = -0.5f * Ualpha - _SQRT3_2 * Ubeta + center;
+
+      if (!modulation_centered) {
+        float Umin = min(Ua, min(Ub, Uc));
+        Ua -= Umin;
+        Ub -= Umin;
+        Uc -= Umin;
+      }
+
+      break;
+
+    case FOCModulationType::SpaceVectorPWM :
+      // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
+      // https://www.youtube.com/watch?v=QMSWUMEAejg
+
+      // the algorithm goes
+      // 1) Ualpha, Ubeta
+      // 2) Uout = sqrt(Ualpha^2 + Ubeta^2)
+      // 3) angle_el = atan2(Ubeta, Ualpha)
+      //
+      // equivalent to 2)  because the magnitude does not change is:
+      // Uout = sqrt(Ud^2 + Uq^2)
+      // equivalent to 3) is
+      // angle_el = angle_el + atan2(Uq,Ud)
+
+      float Uout;
+      // a bit of optitmisation
+      if(Ud){ // only if Ud and Uq set
+        // _sqrt is an approx of sqrt (3-4% error)
+        Uout = _sqrt(Ud*Ud + Uq*Uq) / driver->voltage_limit;
+        // angle normalisation in between 0 and 2pi
+        // only necessary if using _sin and _cos - approximation functions
+        angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
+      }else{// only Uq available - no need for atan2 and sqrt
+        Uout = Uq / driver->voltage_limit;
+        // angle normalisation in between 0 and 2pi
+        // only necessary if using _sin and _cos - approximation functions
+        angle_el = _normalizeAngle(angle_el + _PI_2);
+      }
+      // find the sector we are in currently
+      sector = floor(angle_el / _PI_3) + 1;
+      // calculate the duty cycles
+      float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uout;
+      float T2 = _SQRT3*_sin(angle_el - (sector-1.0f)*_PI_3) * Uout;
+      // two versions possible
+      float T0 = 0; // pulled to 0 - better for low power supply voltage
+      if (modulation_centered) {
+        T0 = 1 - T1 - T2; // modulation_centered around driver->voltage_limit/2
+      }
+
+      // calculate the duty cycles(times)
+      float Ta,Tb,Tc;
+      switch(sector){
+        case 1:
+          Ta = T1 + T2 + T0/2;
+          Tb = T2 + T0/2;
+          Tc = T0/2;
+          break;
+        case 2:
+          Ta = T1 +  T0/2;
+          Tb = T1 + T2 + T0/2;
+          Tc = T0/2;
+          break;
+        case 3:
+          Ta = T0/2;
+          Tb = T1 + T2 + T0/2;
+          Tc = T2 + T0/2;
+          break;
+        case 4:
+          Ta = T0/2;
+          Tb = T1+ T0/2;
+          Tc = T1 + T2 + T0/2;
+          break;
+        case 5:
+          Ta = T2 + T0/2;
+          Tb = T0/2;
+          Tc = T1 + T2 + T0/2;
+          break;
+        case 6:
+          Ta = T1 + T2 + T0/2;
+          Tb = T0/2;
+          Tc = T1 + T0/2;
+          break;
+        default:
+         // possible error state
+          Ta = 0;
+          Tb = 0;
+          Tc = 0;
+      }
+
+      // calculate the phase voltages and center
+      Ua = Ta*driver->voltage_limit;
+      Ub = Tb*driver->voltage_limit;
+      Uc = Tc*driver->voltage_limit;
+      break;
+
+  }
+
+  // set the voltages in driver
+  driver->setPwm(Ua, Ub, Uc);
+}
+
+
+
+// Function (iterative) generating open loop movement for target velocity
+// - target_velocity - rad/s
+// it uses voltage_limit variable
+float BLDCMotor::velocityOpenloop(float target_velocity){
+  // get current timestamp
+  unsigned long now_us = _micros();
+  // calculate the sample time from last call
+  float Ts = (now_us - open_loop_timestamp) * 1e-6f;
+  // quick fix for strange cases (micros overflow + timestamp not defined)
+  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+  // calculate the necessary angle to achieve target velocity
+  shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);
+  // for display purposes
+  shaft_velocity = target_velocity;
+
+  // use voltage limit or current limit
+  float Uq = voltage_limit;
+  if(_isset(phase_resistance)){
+    Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit);
+    // recalculate the current  
+    current.q = (Uq - fabs(voltage_bemf))/phase_resistance;
+  }
+  // set the maximal allowed voltage (voltage_limit) with the necessary angle
+  setPhaseVoltage(Uq,  0, _electricalAngle(shaft_angle, pole_pairs));
+
+  // save timestamp for next call
+  open_loop_timestamp = now_us;
+
+  return Uq;
+}
+
+// Function (iterative) generating open loop movement towards the target angle
+// - target_angle - rad
+// it uses voltage_limit and velocity_limit variables
+float BLDCMotor::angleOpenloop(float target_angle){
+  // get current timestamp
+  unsigned long now_us = _micros();
+  // calculate the sample time from last call
+  float Ts = (now_us - open_loop_timestamp) * 1e-6f;
+  // quick fix for strange cases (micros overflow + timestamp not defined)
+  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+  // calculate the necessary angle to move from current position towards target angle
+  // with maximal velocity (velocity_limit)
+  // TODO sensor precision: this calculation is not numerically precise. The angle can grow to the point
+  //                        where small position changes are no longer captured by the precision of floats
+  //                        when the total position is large.
+  if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){
+    shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts;
+    shaft_velocity = velocity_limit;
+  }else{
+    shaft_angle = target_angle;
+    shaft_velocity = 0;
+  }
+
+  // use voltage limit or current limit
+  float Uq = voltage_limit;
+  if(_isset(phase_resistance)){
+    Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit);
+    // recalculate the current  
+    current.q = (Uq - fabs(voltage_bemf))/phase_resistance;
+  }
+  // set the maximal allowed voltage (voltage_limit) with the necessary angle
+  // sensor precision: this calculation is OK due to the normalisation
+  setPhaseVoltage(Uq,  0, _electricalAngle(_normalizeAngle(shaft_angle), pole_pairs));
+
+  // save timestamp for next call
+  open_loop_timestamp = now_us;
+
+  return Uq;
+}

firmware/lib/Arduino-FOC/src/BLDCMotor.h

@@ -0,0 +1,115 @@
+#ifndef BLDCMotor_h
+#define BLDCMotor_h
+
+#include "Arduino.h"
+#include "common/base_classes/FOCMotor.h"
+#include "common/base_classes/Sensor.h"
+#include "common/base_classes/BLDCDriver.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+#include "common/defaults.h"
+
+/**
+ BLDC motor class
+*/
+class BLDCMotor: public FOCMotor
+{
+  public:
+    /**
+     BLDCMotor class constructor
+     @param pp pole pairs number
+     @param R  motor phase resistance - [Ohm]
+     @param KV  motor KV rating (1/K_bemf) - rpm/V
+     @param L  motor phase inductance - [H]
+     */ 
+    BLDCMotor(int pp,  float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET);
+    
+    /**
+     * Function linking a motor and a foc driver 
+     * 
+     * @param driver BLDCDriver class implementing all the hardware specific functions necessary PWM setting
+     */
+    virtual void linkDriver(BLDCDriver* driver);
+
+    /** 
+      * BLDCDriver link:
+      * - 3PWM 
+      * - 6PWM 
+    */
+    BLDCDriver* driver; 
+    
+    /**  Motor hardware init function */
+  	void init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    /**
+     * Function initializing FOC algorithm
+     * and aligning sensor's and motors' zero position 
+     */  
+    int initFOC() override;
+    /**
+     * Function running FOC algorithm in real-time
+     * it calculates the gets motor angle and sets the appropriate voltages 
+     * to the phase pwm signals
+     * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us
+     */ 
+    void loopFOC() override;
+
+    /**
+     * Function executing the control loops set by the controller parameter of the BLDCMotor.
+     * 
+     * @param target  Either voltage, angle or velocity based on the motor.controller
+     *                If it is not set the motor will use the target set in its variable motor.target
+     * 
+     * This function doesn't need to be run upon each loop execution - depends of the use case
+     */
+    void move(float target = NOT_SET) override;
+    
+    float Ua, Ub, Uc;//!< Current phase voltages Ua,Ub and Uc set to motor
+    float	Ualpha, Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform
+
+  /**
+    * Method using FOC to set Uq to the motor at the optimal angle
+    * Heart of the FOC algorithm
+    * 
+    * @param Uq Current voltage in q axis to set to the motor
+    * @param Ud Current voltage in d axis to set to the motor
+    * @param angle_el current electrical angle of the motor
+    */
+    void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
+
+  private:
+    // FOC methods 
+
+    /** Sensor alignment to electrical 0 angle of the motor */
+    int alignSensor();
+    /** Current sense and motor phase alignment */
+    int alignCurrentSense();
+    /** Motor and sensor alignment to the sensors absolute 0 angle  */
+    int absoluteZeroSearch();
+
+        
+    // Open loop motion control    
+    /**
+     * Function (iterative) generating open loop movement for target velocity
+     * it uses voltage_limit variable
+     * 
+     * @param target_velocity - rad/s
+     */
+    float velocityOpenloop(float target_velocity);
+    /**
+     * Function (iterative) generating open loop movement towards the target angle
+     * it uses voltage_limit and velocity_limit variables
+     * 
+     * @param target_angle - rad
+     */
+    float angleOpenloop(float target_angle);
+    // open loop variables
+    long open_loop_timestamp;
+};
+
+
+#endif

firmware/lib/Arduino-FOC/src/SimpleFOC.h

@@ -0,0 +1,119 @@
+/*!
+ * @file SimpleFOC.h
+ *
+ * @mainpage Simple Field Oriented Control BLDC motor control library
+ *
+ * @section intro_sec Introduction
+ *
+ * Proper low-cost and low-power FOC supporting boards are very hard to find these days and even may not exist.<br> Even harder to find is a stable and simple FOC algorithm code capable of running on Arduino devices. Therefore this is an attempt to:
+ * - Demystify FOC algorithm and make a robust but simple Arduino library: Arduino SimpleFOC library
+ * - Develop a modular BLDC driver board: Arduino SimpleFOC shield.
+ *
+ * @section features Features
+ *  - Arduino compatible: Arduino library code
+ *  - Easy to setup and configure:
+ *     - Easy hardware configuration
+ *     - Easy tuning the control loops
+ *  - Modular:
+ *     - Supports as many sensors , BLDC motors and driver boards as possible
+ *     - Supports as many application requirements as possible
+ *  - Plug & play: Arduino SimpleFOC shield
+ * 
+ * @section dependencies Supported Hardware
+ *  - Motors 
+ *    - BLDC motors
+ *    - Stepper motors
+ * - Drivers 
+ *    - BLDC drivers
+ *    - Gimbal drivers
+ *    - Stepper drivers
+ * - Position sensors 
+ *    - Encoders
+ *    - Magnetic sensors
+ *    - Hall sensors
+ *    - Open-loop control
+ * - Microcontrollers 
+ *    - Arduino
+ *    - STM32
+ *    - ESP32
+ *    - Teensy
+ * 
+ * @section example_code Example code
+ * @code
+#include <SimpleFOC.h>
+
+//  BLDCMotor( pole_pairs )
+BLDCMotor motor = BLDCMotor(11);
+//  BLDCDriver( pin_pwmA, pin_pwmB, pin_pwmC, enable (optional) )
+BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
+//  Encoder(pin_A, pin_B, CPR)
+Encoder encoder = Encoder(2, 3, 2048);
+// channel A and B callbacks
+void doA(){encoder.handleA();}
+void doB(){encoder.handleB();}
+
+
+void setup() {  
+  // initialize encoder hardware
+  encoder.init();
+  // hardware interrupt enable
+  encoder.enableInterrupts(doA, doB);
+  // link the motor to the sensor
+  motor.linkSensor(&encoder);
+  
+  // power supply voltage [V]
+  driver.voltage_power_supply = 12;
+  // initialise driver hardware
+  driver.init();
+  // link driver
+  motor.linkDriver(&driver);
+
+  // set control loop type to be used
+  motor.controller = MotionControlType::velocity;
+  // initialize motor
+  motor.init();
+  
+  // align encoder and start FOC
+  motor.initFOC();
+}
+
+void loop() {
+  // FOC algorithm function
+  motor.loopFOC();
+
+  // velocity control loop function
+  // setting the target velocity or 2rad/s
+  motor.move(2);
+}
+ * @endcode 
+ *
+ * @section license License
+ *
+ * MIT license, all text here must be included in any redistribution.
+ *
+ */
+
+#ifndef SIMPLEFOC_H
+#define SIMPLEFOC_H
+
+#include "BLDCMotor.h"
+#include "StepperMotor.h"
+#include "sensors/Encoder.h"
+#include "sensors/MagneticSensorSPI.h"
+#include "sensors/MagneticSensorI2C.h"
+#include "sensors/MagneticSensorAnalog.h"
+#include "sensors/MagneticSensorPWM.h"
+#include "sensors/HallSensor.h"
+#include "sensors/GenericSensor.h"
+#include "drivers/BLDCDriver3PWM.h"
+#include "drivers/BLDCDriver6PWM.h"
+#include "drivers/StepperDriver4PWM.h"
+#include "drivers/StepperDriver2PWM.h"
+#include "current_sense/InlineCurrentSense.h"
+#include "current_sense/LowsideCurrentSense.h"
+#include "current_sense/GenericCurrentSense.h"
+#include "communication/Commander.h"
+#include "communication/StepDirListener.h"
+#include "communication/SimpleFOCDebug.h"
+
+#endif

firmware/lib/Arduino-FOC/src/StepperMotor.cpp

@@ -0,0 +1,463 @@
+#include "StepperMotor.h"
+#include "./communication/SimpleFOCDebug.h"
+
+
+// StepperMotor(int pp)
+// - pp            - pole pair number
+// - R             - motor phase resistance
+// - KV            - motor kv rating (rmp/v)
+// - L             - motor phase inductance [H]
+StepperMotor::StepperMotor(int pp, float _R, float _KV, float _inductance)
+: FOCMotor()
+{
+  // number od pole pairs
+  pole_pairs = pp;
+  // save phase resistance number
+  phase_resistance = _R;
+  // save back emf constant KV = 1/K_bemf
+  // usually used rms
+  KV_rating = _KV*_SQRT2;
+  // save phase inductance
+  phase_inductance = _inductance;
+
+  // torque control type is voltage by default
+  // current and foc_current not supported yet
+  torque_controller = TorqueControlType::voltage;
+}
+
+/**
+	Link the driver which controls the motor
+*/
+void StepperMotor::linkDriver(StepperDriver* _driver) {
+  driver = _driver;
+}
+
+// init hardware pins
+void StepperMotor::init() {
+  if (!driver || !driver->initialized) {
+    motor_status = FOCMotorStatus::motor_init_failed;
+    SIMPLEFOC_DEBUG("MOT: Init not possible, driver not initialized");
+    return;
+  }
+  motor_status = FOCMotorStatus::motor_initializing;
+  SIMPLEFOC_DEBUG("MOT: Init");
+
+  // sanity check for the voltage limit configuration
+  if(voltage_limit > driver->voltage_limit) voltage_limit =  driver->voltage_limit;
+  // constrain voltage for sensor alignment
+  if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit;
+
+  // update the controller limits
+  if(_isset(phase_resistance)){
+    // velocity control loop controls current
+    PID_velocity.limit = current_limit;
+  }else{
+    PID_velocity.limit = voltage_limit;
+  }
+  P_angle.limit = velocity_limit;
+
+  // if using open loop control, set a CW as the default direction if not already set
+  if ((controller==MotionControlType::angle_openloop
+     ||controller==MotionControlType::velocity_openloop)
+     && (sensor_direction == Direction::UNKNOWN)) {
+      sensor_direction = Direction::CW;
+  }
+
+  _delay(500);
+  // enable motor
+  SIMPLEFOC_DEBUG("MOT: Enable driver.");
+  enable();
+  _delay(500);
+
+  motor_status = FOCMotorStatus::motor_uncalibrated;
+}
+
+
+// disable motor driver
+void StepperMotor::disable()
+{
+  // set zero to PWM
+  driver->setPwm(0, 0);
+  // disable driver
+  driver->disable();
+  // motor status update
+  enabled = 0;
+}
+// enable motor driver
+void StepperMotor::enable()
+{
+  // disable enable
+  driver->enable();
+  // set zero to PWM
+  driver->setPwm(0, 0);
+  // motor status update
+  enabled = 1;
+}
+
+
+/**
+  FOC functions
+*/
+// FOC initialization function
+int  StepperMotor::initFOC() {
+  int exit_flag = 1;
+  
+  motor_status = FOCMotorStatus::motor_calibrating;
+
+  // align motor if necessary
+  // alignment necessary for encoders!
+  // sensor and motor alignment - can be skipped
+  // by setting motor.sensor_direction and motor.zero_electric_angle
+  _delay(500);
+  if(sensor){
+    exit_flag *= alignSensor();
+    // added the shaft_angle update
+    sensor->update();
+    shaft_angle = sensor->getAngle();
+  } else { SIMPLEFOC_DEBUG("MOT: No sensor."); }
+
+  if(exit_flag){
+    SIMPLEFOC_DEBUG("MOT: Ready.");
+    motor_status = FOCMotorStatus::motor_ready;
+  }else{
+    SIMPLEFOC_DEBUG("MOT: Init FOC failed.");
+    motor_status = FOCMotorStatus::motor_calib_failed;
+    disable();
+  }
+
+  return exit_flag;
+}
+
+// Encoder alignment to electrical 0 angle
+int StepperMotor::alignSensor() {
+  int exit_flag = 1; //success
+  SIMPLEFOC_DEBUG("MOT: Align sensor.");
+
+  // if unknown natural direction
+  if(sensor_direction == Direction::UNKNOWN){
+    // check if sensor needs zero search
+    if(sensor->needsSearch()) exit_flag = absoluteZeroSearch();
+    // stop init if not found index
+    if(!exit_flag) return exit_flag;
+
+    // find natural direction
+    // move one electrical revolution forward
+    for (int i = 0; i <=500; i++ ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f;
+      setPhaseVoltage(voltage_sensor_align, 0,  angle);
+	    sensor->update();
+      _delay(2);
+    }
+    // take and angle in the middle
+    sensor->update();
+    float mid_angle = sensor->getAngle();
+    // move one electrical revolution backwards
+    for (int i = 500; i >=0; i-- ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f ;
+      setPhaseVoltage(voltage_sensor_align, 0,  angle);
+	    sensor->update();
+      _delay(2);
+    }
+    sensor->update();
+    float end_angle = sensor->getAngle();
+    setPhaseVoltage(0, 0, 0);
+    _delay(200);
+    // determine the direction the sensor moved
+    if (mid_angle == end_angle) {
+      SIMPLEFOC_DEBUG("MOT: Failed to notice movement");
+      return 0; // failed calibration
+    } else if (mid_angle < end_angle) {
+      SIMPLEFOC_DEBUG("MOT: sensor_direction==CCW");
+      sensor_direction = Direction::CCW;
+    } else{
+      SIMPLEFOC_DEBUG("MOT: sensor_direction==CW");
+      sensor_direction = Direction::CW;
+    }
+    // check pole pair number
+    float moved =  fabs(mid_angle - end_angle);
+    if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher!
+      SIMPLEFOC_DEBUG("MOT: PP check: fail - estimated pp: ", _2PI/moved);
+    } else {
+      SIMPLEFOC_DEBUG("MOT: PP check: OK!");
+    }
+
+  } else { 
+    SIMPLEFOC_DEBUG("MOT: Skip dir calib.");
+  }
+
+  // zero electric angle not known
+  if(zero_electric_angle == NOT_SET){
+    // align the electrical phases of the motor and sensor
+    // set angle -90(270 = 3PI/2) degrees
+    setPhaseVoltage(voltage_sensor_align, 0,  _3PI_2);
+    _delay(700);
+    // read the sensor
+    sensor->update();
+    // get the current zero electric angle
+    zero_electric_angle = 0;
+    zero_electric_angle = electricalAngle();
+    _delay(20);
+    if(monitor_port){
+      SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
+    }
+    // stop everything
+    setPhaseVoltage(0, 0, 0);
+    _delay(200);
+  } else { SIMPLEFOC_DEBUG("MOT: Skip offset calib."); }
+  return exit_flag;
+}
+
+// Calibrate the motor and current sense phases
+int StepperMotor::alignCurrentSense() {
+  int exit_flag = 1; // success
+
+  SIMPLEFOC_DEBUG("MOT: Align current sense.");
+
+  // align current sense and the driver
+  exit_flag = current_sense->driverAlign(voltage_sensor_align);
+  if(!exit_flag){
+    // error in current sense - phase either not measured or bad connection
+    SIMPLEFOC_DEBUG("MOT: Align error!");
+    exit_flag = 0;
+  }else{
+    // output the alignment status flag
+    SIMPLEFOC_DEBUG("MOT: Success: ", exit_flag);
+  }
+
+  return exit_flag > 0;
+}
+
+// Encoder alignment the absolute zero angle
+// - to the index
+int StepperMotor::absoluteZeroSearch() {
+
+  SIMPLEFOC_DEBUG("MOT: Index search...");
+  // search the absolute zero with small velocity
+  float limit_vel = velocity_limit;
+  float limit_volt = voltage_limit;
+  velocity_limit = velocity_index_search;
+  voltage_limit = voltage_sensor_align;
+  shaft_angle = 0;
+  while(sensor->needsSearch() && shaft_angle < _2PI){
+    angleOpenloop(1.5f*_2PI);
+    // call important for some sensors not to loose count
+    // not needed for the search
+    sensor->update();
+  }
+  // disable motor
+  setPhaseVoltage(0, 0, 0);
+  // reinit the limits
+  velocity_limit = limit_vel;
+  voltage_limit = limit_volt;
+  // check if the zero found
+  if(monitor_port){
+    if(sensor->needsSearch()) SIMPLEFOC_DEBUG("MOT: Error: Not found!");
+    else { SIMPLEFOC_DEBUG("MOT: Success!"); }
+  }
+  return !sensor->needsSearch();
+}
+
+
+// Iterative function looping FOC algorithm, setting Uq on the Motor
+// The faster it can be run the better
+void StepperMotor::loopFOC() {
+  
+  // update sensor - do this even in open-loop mode, as user may be switching between modes and we could lose track
+  //                 of full rotations otherwise.
+  if (sensor) sensor->update();
+
+  // if open-loop do nothing
+  if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return;
+  // shaft angle
+  shaft_angle = shaftAngle();
+
+  // if disabled do nothing
+  if(!enabled) return;
+
+  // Needs the update() to be called first
+  // This function will not have numerical issues because it uses Sensor::getMechanicalAngle() 
+  // which is in range 0-2PI
+  electrical_angle = electricalAngle();
+
+  // set the phase voltage - FOC heart function :)
+  setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
+}
+
+// Iterative function running outer loop of the FOC algorithm
+// Behavior of this function is determined by the motor.controller variable
+// It runs either angle, velocity or voltage loop
+// - needs to be called iteratively it is asynchronous function
+// - if target is not set it uses motor.target value
+void StepperMotor::move(float new_target) {
+
+  // downsampling (optional)
+  if(motion_cnt++ < motion_downsample) return;
+  motion_cnt = 0;
+
+  // shaft angle/velocity need the update() to be called first
+  // get shaft angle
+  // TODO sensor precision: the shaft_angle actually stores the complete position, including full rotations, as a float
+  //                        For this reason it is NOT precise when the angles become large.
+  //                        Additionally, the way LPF works on angle is a precision issue, and the angle-LPF is a problem
+  //                        when switching to a 2-component representation.
+  if( controller!=MotionControlType::angle_openloop && controller!=MotionControlType::velocity_openloop ) 
+    shaft_angle = shaftAngle(); // read value even if motor is disabled to keep the monitoring updated but not in openloop mode
+  // get angular velocity 
+  shaft_velocity = shaftVelocity(); // read value even if motor is disabled to keep the monitoring updated
+
+  // if disabled do nothing
+  if(!enabled) return;
+
+  // set internal target variable
+  if(_isset(new_target) ) target = new_target;
+
+  // calculate the back-emf voltage if KV_rating available U_bemf = vel*(1/KV)
+  if (_isset(KV_rating)) voltage_bemf = shaft_velocity/KV_rating/_RPM_TO_RADS;
+  // estimate the motor current if phase reistance available and current_sense not available
+  if(!current_sense && _isset(phase_resistance)) current.q = (voltage.q - voltage_bemf)/phase_resistance;
+
+  // choose control loop
+  switch (controller) {
+    case MotionControlType::torque:
+      if(!_isset(phase_resistance))  voltage.q = target; // if voltage torque control
+      else  voltage.q =  target*phase_resistance + voltage_bemf;
+      voltage.q = _constrain(voltage.q, -voltage_limit, voltage_limit);
+      // set d-component (lag compensation if known inductance)
+      if(!_isset(phase_inductance)) voltage.d = 0;
+      else voltage.d = _constrain( -target*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      break;
+    case MotionControlType::angle:
+      // angle set point
+      shaft_angle_sp = target;
+      // calculate velocity set point
+      shaft_velocity_sp = feed_forward_velocity + P_angle( shaft_angle_sp - shaft_angle );
+      shaft_angle_sp = _constrain(shaft_angle_sp, -velocity_limit, velocity_limit);
+      // calculate the torque command
+      current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control
+      // if torque controlled through voltage
+      // use voltage if phase-resistance not provided
+      if(!_isset(phase_resistance))  voltage.q = current_sp;
+      else  voltage.q =  _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+      // set d-component (lag compensation if known inductance)
+      if(!_isset(phase_inductance)) voltage.d = 0;
+      else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      break;
+    case MotionControlType::velocity:
+      // velocity set point
+      shaft_velocity_sp = target;
+      // calculate the torque command
+      current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
+      // if torque controlled through voltage control
+      // use voltage if phase-resistance not provided
+      if(!_isset(phase_resistance))  voltage.q = current_sp;
+      else  voltage.q = _constrain( current_sp*phase_resistance + voltage_bemf , -voltage_limit, voltage_limit);
+      // set d-component (lag compensation if known inductance)
+      if(!_isset(phase_inductance)) voltage.d = 0;
+      else voltage.d = _constrain( -current_sp*shaft_velocity*pole_pairs*phase_inductance, -voltage_limit, voltage_limit);
+      break;
+    case MotionControlType::velocity_openloop:
+      // velocity control in open loop
+      shaft_velocity_sp = target;
+      voltage.q = velocityOpenloop(shaft_velocity_sp); // returns the voltage that is set to the motor
+      voltage.d = 0; // TODO d-component lag-compensation 
+      break;
+    case MotionControlType::angle_openloop:
+      // angle control in open loop
+      shaft_angle_sp = target;
+      voltage.q = angleOpenloop(shaft_angle_sp); // returns the voltage that is set to the motor
+      voltage.d = 0; // TODO d-component lag-compensation 
+      break;
+  }
+}
+
+
+// Method using FOC to set Uq and Ud to the motor at the optimal angle
+// Function implementing Sine PWM algorithms
+// - space vector not implemented yet
+//
+// Function using sine approximation
+// regular sin + cos ~300us    (no memory usaage)
+// approx  _sin + _cos ~110us  (400Byte ~ 20% of memory)
+void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
+  // Sinusoidal PWM modulation
+  // Inverse Park transformation
+  float _sa, _ca;
+  _sincos(angle_el, &_sa, &_ca);
+
+  // Inverse park transform
+  Ualpha =  _ca * Ud - _sa * Uq;  // -sin(angle) * Uq;
+  Ubeta =  _sa * Ud + _ca * Uq;    //  cos(angle) * Uq;
+
+  // set the voltages in hardware
+  driver->setPwm(Ualpha, Ubeta);
+}
+
+// Function (iterative) generating open loop movement for target velocity
+// - target_velocity - rad/s
+// it uses voltage_limit variable
+float StepperMotor::velocityOpenloop(float target_velocity){
+  // get current timestamp
+  unsigned long now_us = _micros();
+  // calculate the sample time from last call
+  float Ts = (now_us - open_loop_timestamp) * 1e-6f;
+  // quick fix for strange cases (micros overflow + timestamp not defined)
+  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+  // calculate the necessary angle to achieve target velocity
+  shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);
+  // for display purposes
+  shaft_velocity = target_velocity;
+
+  // use voltage limit or current limit
+  float Uq = voltage_limit;
+  if(_isset(phase_resistance)){
+    Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit);
+    // recalculate the current  
+    current.q = (Uq - fabs(voltage_bemf))/phase_resistance;
+  }
+
+  // set the maximal allowed voltage (voltage_limit) with the necessary angle
+  setPhaseVoltage(Uq,  0, _electricalAngle(shaft_angle, pole_pairs));
+
+  // save timestamp for next call
+  open_loop_timestamp = now_us;
+
+  return Uq;
+}
+
+// Function (iterative) generating open loop movement towards the target angle
+// - target_angle - rad
+// it uses voltage_limit and velocity_limit variables
+float StepperMotor::angleOpenloop(float target_angle){
+  // get current timestamp
+  unsigned long now_us = _micros();
+  // calculate the sample time from last call
+  float Ts = (now_us - open_loop_timestamp) * 1e-6f;
+  // quick fix for strange cases (micros overflow + timestamp not defined)
+  if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+  // calculate the necessary angle to move from current position towards target angle
+  // with maximal velocity (velocity_limit)
+  if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts)){
+    shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts;
+    shaft_velocity = velocity_limit;
+  }else{
+    shaft_angle = target_angle;
+    shaft_velocity = 0;
+  }
+
+  // use voltage limit or current limit
+  float Uq = voltage_limit;
+  if(_isset(phase_resistance)){
+    Uq = _constrain(current_limit*phase_resistance + fabs(voltage_bemf),-voltage_limit, voltage_limit);
+    // recalculate the current  
+    current.q = (Uq - fabs(voltage_bemf))/phase_resistance;
+  }
+  // set the maximal allowed voltage (voltage_limit) with the necessary angle
+  setPhaseVoltage(Uq,  0, _electricalAngle((shaft_angle), pole_pairs));
+
+  // save timestamp for next call
+  open_loop_timestamp = now_us;
+
+  return Uq;
+}

firmware/lib/Arduino-FOC/src/StepperMotor.h

@@ -0,0 +1,117 @@
+/**
+ *  @file StepperMotor.h
+ * 
+ */
+
+#ifndef StepperMotor_h
+#define StepperMotor_h
+
+#include "Arduino.h"
+#include "common/base_classes/FOCMotor.h"
+#include "common/base_classes/StepperDriver.h"
+#include "common/base_classes/Sensor.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+#include "common/defaults.h"
+
+/**
+ Stepper Motor class
+*/
+class StepperMotor: public FOCMotor
+{
+  public:
+    /**
+      StepperMotor class constructor
+      @param pp  pole pair number 
+     @param R  motor phase resistance - [Ohm]
+     @param KV  motor KV rating (1/K_bemf) - rpm/V
+     @param L  motor phase inductance - [H]
+    */
+    StepperMotor(int pp,  float R = NOT_SET, float KV = NOT_SET, float L = NOT_SET);
+
+    /**
+     * Function linking a motor and a foc driver 
+     * 
+     * @param driver StepperDriver class implementing all the hardware specific functions necessary PWM setting
+     */
+    void linkDriver(StepperDriver* driver);
+
+    /** 
+      * StepperDriver link:
+      * - 4PWM  - L298N for example
+    */
+    StepperDriver* driver; 
+
+    /**  Motor hardware init function */
+  	void init() override;
+    /** Motor disable function */
+  	void disable() override;
+    /** Motor enable function */
+    void enable() override;
+
+    /**
+     * Function initializing FOC algorithm
+     * and aligning sensor's and motors' zero position 
+     * 
+     * - If zero_electric_offset parameter is set the alignment procedure is skipped
+     */  
+    int initFOC() override;
+    /**
+     * Function running FOC algorithm in real-time
+     * it calculates the gets motor angle and sets the appropriate voltages 
+     * to the phase pwm signals
+     * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us
+     */ 
+    void loopFOC() override;
+    /**
+     * Function executing the control loops set by the controller parameter of the StepperMotor.
+     * 
+     * @param target  Either voltage, angle or velocity based on the motor.controller
+     *                If it is not set the motor will use the target set in its variable motor.target
+     * 
+     * This function doesn't need to be run upon each loop execution - depends of the use case
+     */
+    void move(float target = NOT_SET) override;
+    
+    float	Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform
+
+  /**
+    * Method using FOC to set Uq to the motor at the optimal angle
+    * Heart of the FOC algorithm
+    * 
+    * @param Uq Current voltage in q axis to set to the motor
+    * @param Ud Current voltage in d axis to set to the motor
+    * @param angle_el current electrical angle of the motor
+    */
+    void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
+
+  private:
+  
+    /** Sensor alignment to electrical 0 angle of the motor */
+    int alignSensor();
+    /** Current sense and motor phase alignment */
+    int alignCurrentSense();
+    /** Motor and sensor alignment to the sensors absolute 0 angle  */
+    int absoluteZeroSearch();
+        
+    // Open loop motion control    
+    /**
+     * Function (iterative) generating open loop movement for target velocity
+     * it uses voltage_limit variable
+     * 
+     * @param target_velocity - rad/s
+     */
+    float velocityOpenloop(float target_velocity);
+    /**
+     * Function (iterative) generating open loop movement towards the target angle
+     * it uses voltage_limit and velocity_limit variables
+     * 
+     * @param target_angle - rad
+     */
+    float angleOpenloop(float target_angle);
+    // open loop variables
+    long open_loop_timestamp;
+};
+
+
+#endif

firmware/lib/Arduino-FOC/src/common/base_classes/BLDCDriver.h

@@ -0,0 +1,42 @@
+#ifndef BLDCDRIVER_H
+#define BLDCDRIVER_H
+
+#include "Arduino.h"
+#include "FOCDriver.h"
+
+
+enum PhaseState : uint8_t {
+  PHASE_OFF = 0, // both sides of the phase are off
+  PHASE_ON = 1,  // both sides of the phase are driven with PWM, dead time is applied in 6-PWM mode
+  PHASE_HI = 2,  // only the high side of the phase is driven with PWM (6-PWM mode only)
+  PHASE_LO = 3,  // only the low side of the phase is driven with PWM (6-PWM mode only)
+};
+
+
+class BLDCDriver: public FOCDriver{
+    public:
+
+        float dc_a; //!< currently set duty cycle on phaseA
+        float dc_b; //!< currently set duty cycle on phaseB
+        float dc_c; //!< currently set duty cycle on phaseC
+
+        /**
+         * Set phase voltages to the harware
+         *
+         * @param Ua - phase A voltage
+         * @param Ub - phase B voltage
+         * @param Uc - phase C voltage
+        */
+        virtual void setPwm(float Ua, float Ub, float Uc) = 0;
+
+        /**
+         * Set phase state, enable/disable
+         *
+         * @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) = 0;
+};
+
+#endif

firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.cpp

@@ -0,0 +1,121 @@
+#include "CurrentSense.h"
+
+// get current magnitude
+//   - absolute  - if no electrical_angle provided
+//   - signed    - if angle provided
+float CurrentSense::getDCCurrent(float motor_electrical_angle)
+{
+    // read current phase currents
+    PhaseCurrent_s current = getPhaseCurrents();
+    // currnet sign - if motor angle not provided the magnitude is always positive
+    float sign = 1;
+
+    // calculate clarke transform
+    float i_alpha, i_beta;
+    if (!current.c)
+    {
+        // if only two measured currents
+        i_alpha = current.a;
+        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
+    }
+    if (!current.a)
+    {
+        // if only two measured currents
+        float a = -current.c - current.b;
+        i_alpha = a;
+        i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
+    }
+    if (!current.b)
+    {
+        // if only two measured currents
+        float b = -current.a - current.c;
+        i_alpha = current.a;
+        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b;
+    }
+    else
+    {
+        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
+        float mid = (1.f / 3) * (current.a + current.b + current.c);
+        float a = current.a - mid;
+        float b = current.b - mid;
+        i_alpha = a;
+        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
+    }
+
+    // if motor angle provided function returns signed value of the current
+    // determine the sign of the current
+    // sign(atan2(current.q, current.d)) is the same as c.q > 0 ? 1 : -1
+    if (motor_electrical_angle)
+    {
+        float ct;
+        float st;
+        _sincos(motor_electrical_angle, &st, &ct);
+        sign = (i_beta * ct - i_alpha * st) > 0 ? 1 : -1;
+    }
+    // return current magnitude
+    return sign * _sqrt(i_alpha * i_alpha + i_beta * i_beta);
+}
+
+// function used with the foc algorihtm
+//   calculating DQ currents from phase currents
+//   - function calculating park and clarke transform of the phase currents
+//   - using getPhaseCurrents internally
+DQCurrent_s CurrentSense::getFOCCurrents(float angle_el)
+{
+    // read current phase currents
+    PhaseCurrent_s current = getPhaseCurrents();
+
+    // calculate clarke transform
+    float i_alpha, i_beta;
+    if (!current.c)
+    {
+        // if only two measured currents
+        i_alpha = current.a;
+        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * current.b;
+    }
+    if (!current.a)
+    {
+        // if only two measured currents
+        float a = -current.c - current.b;
+        i_alpha = a;
+        i_beta = _1_SQRT3 * a + _2_SQRT3 * current.b;
+    }
+    if (!current.b)
+    {
+        // if only two measured currents
+        float b = -current.a - current.c;
+        i_alpha = current.a;
+        i_beta = _1_SQRT3 * current.a + _2_SQRT3 * b;
+    }
+    else
+    {
+        // signal filtering using identity a + b + c = 0. Assumes measurement error is normally distributed.
+        float mid = (1.f / 3) * (current.a + current.b + current.c);
+        float a = current.a - mid;
+        float b = current.b - mid;
+        i_alpha = a;
+        i_beta = _1_SQRT3 * a + _2_SQRT3 * b;
+    }
+
+    // calculate park transform
+    float ct;
+    float st;
+    _sincos(angle_el, &st, &ct);
+    DQCurrent_s return_current;
+    return_current.d = i_alpha * ct + i_beta * st;
+    return_current.q = i_beta * ct - i_alpha * st;
+    return return_current;
+}
+
+/**
+    Driver linking to the current sense
+*/
+void CurrentSense::linkDriver(BLDCDriver *_driver)
+{
+    driver = _driver;
+}
+
+void CurrentSense::linkDriver(StepperDriver *_driver)
+{
+    driver = _driver;
+}

firmware/lib/Arduino-FOC/src/common/base_classes/CurrentSense.h

@@ -0,0 +1,75 @@
+#ifndef CURRENTSENSE_H
+#define CURRENTSENSE_H
+
+#include "BLDCDriver.h"
+#include "StepperDriver.h"
+#include "../foc_utils.h"
+
+/**
+ *  Current sensing abstract class defintion
+ * Each current sensoring implementation needs to extend this interface
+ */
+class CurrentSense{
+    public:
+
+    /**
+     *  Function intialising the CurrentSense class
+     *   - All the necessary intialisations of adc and sync should be implemented here
+     *   
+     * @returns -  0 - for failure &  1 - for success 
+     */
+    virtual int init() = 0;
+    
+    /**
+     * Linking the current sense with the motor driver
+     * Only necessary if synchronisation in between the two is required
+     */
+    void linkDriver(BLDCDriver *driver);
+    void linkDriver(StepperDriver *driver);
+
+    // variables
+    bool skip_align = false; //!< variable signaling that the phase current direction should be verified during initFOC()
+    
+    FOCDriver* driver = nullptr; //!< driver link
+    bool initialized = false; // true if current sense was successfully initialized   
+    void* params = 0; //!< pointer to hardware specific parameters of current sensing
+    
+    /**
+     * Function intended to verify if:
+     *   - phase current are oriented properly 
+     *   - if their order is the same as driver phases
+     * 
+     * This function corrects the alignment errors if possible ans if no such thing is needed it can be left empty (return 1)
+     * @returns -  0 - for failure &  positive number (with status) - for success 
+     */
+    virtual int driverAlign(float align_voltage) = 0;
+
+    /**
+     *  Function rading the phase currents a, b and c
+     *   This function will be used with the foc control throught the function 
+     *   CurrentSense::getFOCCurrents(electrical_angle)
+     *   - it returns current c equal to 0 if only two phase measurements available
+     * 
+     *  @return PhaseCurrent_s current values
+     */
+    virtual PhaseCurrent_s getPhaseCurrents() = 0;
+    /**
+     * Function reading the magnitude of the current set to the motor
+     *  It returns the abosolute or signed magnitude if possible
+     *  It can receive the motor electrical angle to help with calculation
+     *  This function is used with the current control  (not foc)
+     *  
+     * @param angle_el - electrical angle of the motor (optional) 
+     */
+    virtual float getDCCurrent(float angle_el = 0);
+
+    /**
+     * Function used for FOC contorl, it reads the DQ currents of the motor 
+     *   It uses the function getPhaseCurrents internally
+     * 
+     * @param angle_el - motor electrical angle
+     */
+    DQCurrent_s getFOCCurrents(float angle_el);
+};
+
+#endif

firmware/lib/Arduino-FOC/src/common/base_classes/FOCDriver.h

@@ -0,0 +1,21 @@
+#ifndef FOCMOTOR_H
+#define FOTMOTOR_H
+
+#include "Arduino.h"
+
+class FOCDriver{
+    public: 
+        virtual int init() = 0;
+
+        virtual void enable() = 0;
+
+        virtual void disable() = 0;
+
+        long pwm_frequency;
+        float voltage_power_supply;
+        float voltage_limit;
+        bool initialized = false;
+        void* params = 0;
+};
+
+#endif

firmware/lib/Arduino-FOC/src/common/base_classes/FOCMotor.cpp

@@ -0,0 +1,155 @@
+#include "FOCMotor.h"
+#include "../../communication/SimpleFOCDebug.h"
+
+/**
+ * Default constructor - setting all variabels to default values
+ */
+FOCMotor::FOCMotor()
+{
+  // maximum angular velocity to be used for positioning 
+  velocity_limit = DEF_VEL_LIM;
+  // maximum voltage to be set to the motor
+  voltage_limit = DEF_POWER_SUPPLY;
+  // not set on the begining
+  current_limit = DEF_CURRENT_LIM;
+
+  // index search velocity
+  velocity_index_search = DEF_INDEX_SEARCH_TARGET_VELOCITY;
+  // sensor and motor align voltage
+  voltage_sensor_align = DEF_VOLTAGE_SENSOR_ALIGN;
+
+  // default modulation is SinePWM
+  foc_modulation = FOCModulationType::SinePWM;
+
+  // default target value
+  target = 0;
+  voltage.d = 0;
+  voltage.q = 0;
+  // current target values
+  current_sp = 0;
+  current.q = 0;
+  current.d = 0;
+
+  // voltage bemf 
+  voltage_bemf = 0;
+  
+  //monitor_port 
+  monitor_port = nullptr;
+  //sensor 
+  sensor_offset = 0.0f;
+  sensor = nullptr;
+  //current sensor 
+  current_sense = nullptr;
+}
+
+
+/**
+	Sensor linking method
+*/
+void FOCMotor::linkSensor(Sensor* _sensor) {
+  sensor = _sensor;
+}
+
+/**
+	CurrentSense linking method
+*/
+void FOCMotor::linkCurrentSense(CurrentSense* _current_sense) {
+  current_sense = _current_sense;
+}
+
+// shaft angle calculation
+float FOCMotor::shaftAngle() {
+  // if no sensor linked return previous value ( for open loop )
+  if(!sensor) return shaft_angle;
+  return sensor_direction*LPF_angle(sensor->getAngle()) - sensor_offset;
+}
+// shaft velocity calculation
+float FOCMotor::shaftVelocity() {
+  // if no sensor linked return previous value ( for open loop )
+  if(!sensor) return shaft_velocity;
+  return sensor_direction*LPF_velocity(sensor->getVelocity());
+}
+
+float FOCMotor::electricalAngle(){
+  // if no sensor linked return previous value ( for open loop )
+  if(!sensor) return electrical_angle;
+  return  _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle()  - zero_electric_angle );
+}
+
+/**
+ *  Monitoring functions
+ */
+// function implementing the monitor_port setter
+void FOCMotor::useMonitoring(Print &print){
+  monitor_port = &print; //operate on the address of print
+  #ifndef SIMPLEFOC_DISABLE_DEBUG
+  SimpleFOCDebug::enable(&print);
+  SIMPLEFOC_DEBUG("MOT: Monitor enabled!");
+  #endif
+}
+
+// utility function intended to be used with serial plotter to monitor motor variables
+// significantly slowing the execution down!!!!
+void FOCMotor::monitor() {
+  if( !monitor_downsample || monitor_cnt++ < monitor_downsample ) return;
+  monitor_cnt = 0;
+  if(!monitor_port) return;
+  bool printed = 0;
+
+  if(monitor_variables & _MON_TARGET){
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    monitor_port->print(target,monitor_decimals);    
+    printed= true;
+  }
+  if(monitor_variables & _MON_VOLT_Q) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(voltage.q,monitor_decimals);
+    printed= true;
+  }
+  if(monitor_variables & _MON_VOLT_D) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(voltage.d,monitor_decimals);
+    printed= true;
+  }
+  // read currents if possible - even in voltage mode (if current_sense available)
+  if(monitor_variables & _MON_CURR_Q || monitor_variables & _MON_CURR_D) {
+    DQCurrent_s c = current;
+    if( current_sense && torque_controller != TorqueControlType::foc_current ){
+      c = current_sense->getFOCCurrents(electrical_angle);
+      c.q = LPF_current_q(c.q);
+      c.d = LPF_current_d(c.d);
+    }
+    if(monitor_variables & _MON_CURR_Q) {
+      if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+      else if(printed) monitor_port->print(monitor_separator);
+      monitor_port->print(c.q*1000, monitor_decimals); // mAmps
+      printed= true;
+    }
+    if(monitor_variables & _MON_CURR_D) {
+      if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+      else if(printed) monitor_port->print(monitor_separator);
+      monitor_port->print(c.d*1000, monitor_decimals); // mAmps
+      printed= true;
+    }
+  }
+ 
+  if(monitor_variables & _MON_VEL) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(shaft_velocity,monitor_decimals);
+    printed= true;
+  }
+  if(monitor_variables & _MON_ANGLE) {
+    if(!printed && monitor_start_char) monitor_port->print(monitor_start_char);
+    else if(printed) monitor_port->print(monitor_separator);
+    monitor_port->print(shaft_angle,monitor_decimals);
+    printed= true;
+  }
+  if(printed){
+    if(monitor_end_char) monitor_port->println(monitor_end_char);
+    else monitor_port->println("");
+  }
+}   
+

firmware/lib/Arduino-FOC/src/common/base_classes/FOCMotor.h

@@ -0,0 +1,251 @@
+#ifndef FOCMOTOR_H
+#define FOCMOTOR_H
+
+#include "Arduino.h"
+#include "Sensor.h"
+#include "CurrentSense.h"
+
+#include "../time_utils.h"
+#include "../foc_utils.h"
+#include "../defaults.h"
+#include "../pid.h"
+#include "../lowpass_filter.h"
+
+
+// monitoring bitmap
+#define _MON_TARGET 0b1000000 // monitor target value
+#define _MON_VOLT_Q 0b0100000 // monitor voltage q value
+#define _MON_VOLT_D 0b0010000 // monitor voltage d value
+#define _MON_CURR_Q 0b0001000 // monitor current q value - if measured
+#define _MON_CURR_D 0b0000100 // monitor current d value - if measured
+#define _MON_VEL    0b0000010 // monitor velocity value
+#define _MON_ANGLE  0b0000001 // monitor angle value
+
+/**
+ *  Motiron control type
+ */
+enum MotionControlType : uint8_t {
+  torque            = 0x00,     //!< Torque control
+  velocity          = 0x01,     //!< Velocity motion control
+  angle             = 0x02,     //!< Position/angle motion control
+  velocity_openloop = 0x03,
+  angle_openloop    = 0x04
+};
+
+/**
+ *  Motiron control type
+ */
+enum TorqueControlType : uint8_t { 
+  voltage            = 0x00,     //!< Torque control using voltage
+  dc_current         = 0x01,     //!< Torque control using DC current (one current magnitude)
+  foc_current        = 0x02,     //!< torque control using dq currents
+};
+
+/**
+ *  FOC modulation type
+ */
+enum FOCModulationType : uint8_t {
+  SinePWM            = 0x00,     //!< Sinusoidal PWM modulation
+  SpaceVectorPWM     = 0x01,     //!< Space vector modulation method
+  Trapezoid_120      = 0x02,     
+  Trapezoid_150      = 0x03,     
+};
+
+
+
+enum FOCMotorStatus : uint8_t {
+  motor_uninitialized = 0x00,     //!< Motor is not yet initialized
+  motor_initializing  = 0x01,     //!< Motor intiialization is in progress
+  motor_uncalibrated  = 0x02,     //!< Motor is initialized, but not calibrated (open loop possible)
+  motor_calibrating   = 0x03,     //!< Motor calibration in progress
+  motor_ready         = 0x04,     //!< Motor is initialized and calibrated (closed loop possible)
+  motor_error         = 0x08,     //!< Motor is in error state (recoverable, e.g. overcurrent protection active)
+  motor_calib_failed  = 0x0E,     //!< Motor calibration failed (possibly recoverable)
+  motor_init_failed   = 0x0F,     //!< Motor initialization failed (not recoverable)
+};
+
+
+
+/**
+ Generic motor class
+*/
+class FOCMotor
+{
+  public:
+    /**
+     * Default constructor - setting all variabels to default values
+     */
+    FOCMotor();
+
+    /**  Motor hardware init function */
+  	virtual void init()=0;
+    /** Motor disable function */
+  	virtual void disable()=0;
+    /** Motor enable function */
+    virtual void enable()=0;
+
+    /**
+     * Function linking a motor and a sensor 
+     * 
+     * @param sensor Sensor class  wrapper for the FOC algorihtm to read the motor angle and velocity
+     */
+    void linkSensor(Sensor* sensor);
+
+    /**
+     * Function linking a motor and current sensing 
+     * 
+     * @param current_sense CurrentSense class wrapper for the FOC algorihtm to read the motor current measurements
+     */
+    void linkCurrentSense(CurrentSense* current_sense);
+
+
+    /**
+     * Function initializing FOC algorithm
+     * and aligning sensor's and motors' zero position 
+     * 
+     * - If zero_electric_offset parameter is set the alignment procedure is skipped
+     */  
+    virtual int initFOC()=0;
+    /**
+     * Function running FOC algorithm in real-time
+     * it calculates the gets motor angle and sets the appropriate voltages 
+     * to the phase pwm signals
+     * - the faster you can run it the better Arduino UNO ~1ms, Bluepill ~ 100us
+     */ 
+    virtual void loopFOC()=0;
+    /**
+     * Function executing the control loops set by the controller parameter of the BLDCMotor.
+     * 
+     * @param target  Either voltage, angle or velocity based on the motor.controller
+     *                If it is not set the motor will use the target set in its variable motor.target
+     * 
+     * This function doesn't need to be run upon each loop execution - depends of the use case
+     */
+    virtual void move(float target = NOT_SET)=0;
+
+    /**
+    * Method using FOC to set Uq to the motor at the optimal angle
+    * Heart of the FOC algorithm
+    * 
+    * @param Uq Current voltage in q axis to set to the motor
+    * @param Ud Current voltage in d axis to set to the motor
+    * @param angle_el current electrical angle of the motor
+    */
+    virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=0;
+    
+    // State calculation methods 
+    /** Shaft angle calculation in radians [rad] */
+    float shaftAngle();
+    /** 
+     * Shaft angle calculation function in radian per second [rad/s]
+     * It implements low pass filtering
+     */
+    float shaftVelocity();
+
+
+
+    /** 
+     * Electrical angle calculation  
+     */
+    float electricalAngle();
+
+    // state variables
+    float target; //!< current target value - depends of the controller
+    float feed_forward_velocity = 0.0f; //!< current feed forward velocity
+  	float shaft_angle;//!< current motor angle
+  	float electrical_angle;//!< current electrical angle
+  	float shaft_velocity;//!< current motor velocity 
+    float current_sp;//!< target current ( q current )
+    float shaft_velocity_sp;//!< current target velocity
+    float shaft_angle_sp;//!< current target angle
+    DQVoltage_s voltage;//!< current d and q voltage set to the motor
+    DQCurrent_s current;//!< current d and q current measured
+    float voltage_bemf; //!< estimated backemf voltage (if provided KV constant)
+
+    // motor configuration parameters
+    float voltage_sensor_align;//!< sensor and motor align voltage parameter
+    float velocity_index_search;//!< target velocity for index search 
+    
+    // motor physical parameters
+    float	phase_resistance; //!< motor phase resistance
+    int pole_pairs;//!< motor pole pairs number
+    float KV_rating; //!< motor KV rating
+    float	phase_inductance; //!< motor phase inductance
+
+    // limiting variables
+    float voltage_limit; //!< Voltage limiting variable - global limit
+    float current_limit; //!< Current limiting variable - global limit
+    float velocity_limit; //!< Velocity limiting variable - global limit
+
+    // motor status vairables
+    int8_t enabled = 0;//!< enabled or disabled motor flag
+    FOCMotorStatus motor_status = FOCMotorStatus::motor_uninitialized; //!< motor status
+    
+    // pwm modulation related variables
+    FOCModulationType foc_modulation;//!<  parameter determining modulation algorithm
+    int8_t modulation_centered = 1;//!< flag (1) centered modulation around driver limit /2  or  (0) pulled to 0
+
+
+    // configuration structures
+    TorqueControlType torque_controller; //!< parameter determining the torque control type
+    MotionControlType controller; //!< parameter determining the control loop to be used
+
+    // controllers and low pass filters
+    PIDController PID_current_q{DEF_PID_CURR_P,DEF_PID_CURR_I,DEF_PID_CURR_D,DEF_PID_CURR_RAMP, DEF_POWER_SUPPLY};//!< parameter determining the q current PID config
+    PIDController PID_current_d{DEF_PID_CURR_P,DEF_PID_CURR_I,DEF_PID_CURR_D,DEF_PID_CURR_RAMP, DEF_POWER_SUPPLY};//!< parameter determining the d current PID config
+    LowPassFilter LPF_current_q{DEF_CURR_FILTER_Tf};//!<  parameter determining the current Low pass filter configuration 
+    LowPassFilter LPF_current_d{DEF_CURR_FILTER_Tf};//!<  parameter determining the current Low pass filter configuration 
+    PIDController PID_velocity{DEF_PID_VEL_P,DEF_PID_VEL_I,DEF_PID_VEL_D,DEF_PID_VEL_RAMP,DEF_PID_VEL_LIMIT};//!< parameter determining the velocity PID configuration
+    PIDController P_angle{DEF_P_ANGLE_P,0,0,0,DEF_VEL_LIM};	//!< parameter determining the position PID configuration 
+    LowPassFilter LPF_velocity{DEF_VEL_FILTER_Tf};//!<  parameter determining the velocity Low pass filter configuration 
+    LowPassFilter LPF_angle{0.0};//!<  parameter determining the angle low pass filter configuration 
+    unsigned int motion_downsample = DEF_MOTION_DOWNSMAPLE; //!< parameter defining the ratio of downsampling for move commad
+    unsigned int motion_cnt = 0; //!< counting variable for downsampling for move commad
+
+    // sensor related variabels
+    float sensor_offset; //!< user defined sensor zero offset
+    float zero_electric_angle = NOT_SET;//!< absolute zero electric angle - if available
+    Direction sensor_direction = Direction::UNKNOWN; //!< default is CW. if sensor_direction == Direction::CCW then direction will be flipped compared to CW. Set to UNKNOWN to set by calibration
+
+    /**
+     * Function providing BLDCMotor class with the 
+     * Serial interface and enabling monitoring mode
+     * 
+     * @param serial Monitoring Serial class reference
+     */
+    void useMonitoring(Print &serial);
+
+    /**
+     * Utility function intended to be used with serial plotter to monitor motor variables
+     * significantly slowing the execution down!!!!
+     */
+    void monitor();
+    unsigned int monitor_downsample = DEF_MON_DOWNSMAPLE; //!< show monitor outputs each monitor_downsample calls 
+    char monitor_start_char = '\0'; //!< monitor starting character 
+    char monitor_end_char = '\0'; //!< monitor outputs ending character 
+    char monitor_separator = '\t'; //!< monitor outputs separation character
+    unsigned int  monitor_decimals = 4; //!< monitor outputs decimal places
+    // initial monitoring will display target, voltage, velocity and angle
+    uint8_t monitor_variables = _MON_TARGET | _MON_VOLT_Q | _MON_VEL | _MON_ANGLE; //!< Bit array holding the map of variables the user wants to monitor
+   
+    /** 
+      * Sensor link:
+      * - Encoder 
+      * - MagneticSensor*
+      * - HallSensor
+    */
+    Sensor* sensor; 
+    /** 
+      * CurrentSense link
+    */
+    CurrentSense* current_sense; 
+
+    // monitoring functions
+    Print* monitor_port; //!< Serial terminal variable if provided
+  private:
+    // monitor counting variable
+    unsigned int monitor_cnt = 0 ; //!< counting variable
+};
+
+
+#endif

firmware/lib/Arduino-FOC/src/common/base_classes/Sensor.cpp

@@ -0,0 +1,78 @@
+#include "Sensor.h"
+#include "../foc_utils.h"
+#include "../time_utils.h"
+
+
+
+void Sensor::update() {
+    float val = getSensorAngle();
+    angle_prev_ts = _micros();
+    float d_angle = val - angle_prev;
+    // if overflow happened track it as full rotation
+    if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 
+    angle_prev = val;
+}
+
+
+ /** get current angular velocity (rad/s) */
+float Sensor::getVelocity() {
+    // calculate sample time
+    float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;
+    if (Ts < 0.0f) {    // handle micros() overflow - we need to reset vel_angle_prev_ts
+        vel_angle_prev = angle_prev;
+        vel_full_rotations = full_rotations;
+        vel_angle_prev_ts = angle_prev_ts;
+        return velocity;
+    }
+    if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small
+
+    velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;
+    vel_angle_prev = angle_prev;
+    vel_full_rotations = full_rotations;
+    vel_angle_prev_ts = angle_prev_ts;
+    return velocity;
+}
+
+
+
+void Sensor::init() {
+    // initialize all the internal variables of Sensor to ensure a "smooth" startup (without a 'jump' from zero)
+    getSensorAngle(); // call once
+    delayMicroseconds(1);
+    vel_angle_prev = getSensorAngle(); // call again
+    vel_angle_prev_ts = _micros();
+    delay(1);
+    getSensorAngle(); // call once
+    delayMicroseconds(1);
+    angle_prev = getSensorAngle(); // call again
+    angle_prev_ts = _micros();
+}
+
+
+float Sensor::getMechanicalAngle() {
+    return angle_prev;
+}
+
+
+
+float Sensor::getAngle(){
+    return (float)full_rotations * _2PI + angle_prev;
+}
+
+
+
+double Sensor::getPreciseAngle() {
+    return (double)full_rotations * (double)_2PI + (double)angle_prev;
+}
+
+
+
+int32_t Sensor::getFullRotations() {
+    return full_rotations;
+}
+
+
+
+int Sensor::needsSearch() {
+    return 0; // default false
+}

firmware/lib/Arduino-FOC/src/common/base_classes/Sensor.h

@@ -0,0 +1,139 @@
+#ifndef SENSOR_H
+#define SENSOR_H
+
+#include <inttypes.h>
+
+/**
+ *  Direction structure
+ */
+enum Direction : int8_t {
+    CW      = 1,  // clockwise
+    CCW     = -1, // counter clockwise
+    UNKNOWN = 0   // not yet known or invalid state
+};
+
+
+/**
+ *  Pullup configuration structure
+ */
+enum Pullup : uint8_t {
+    USE_INTERN = 0x00, //!< Use internal pullups
+    USE_EXTERN = 0x01  //!< Use external pullups
+};
+
+/**
+ * Sensor abstract class defintion
+ * 
+ * This class is purposefully kept simple, as a base for all kinds of sensors. Currently we have
+ * Encoders, Magnetic Encoders and Hall Sensor implementations. This base class extracts the
+ * most basic common features so that a FOC driver can obtain the data it needs for operation.
+ * 
+ * To implement your own sensors, create a sub-class of this class, and implement the getSensorAngle()
+ * method. getSensorAngle() returns a float value, in radians, representing the current shaft angle in the
+ * range 0 to 2*PI (one full turn). 
+ * 
+ * To function correctly, the sensor class update() method has to be called sufficiently quickly. Normally,
+ * the BLDCMotor's loopFOC() function calls it once per iteration, so you must ensure to call loopFOC() quickly
+ * enough, both for correct motor and sensor operation.
+ * 
+ * The Sensor base class provides an implementation of getVelocity(), and takes care of counting full
+ * revolutions in a precise way, but if you wish you can additionally override these methods to provide more
+ * optimal implementations for your hardware.
+ * 
+ */
+class Sensor{
+	friend class SmoothingSensor;
+    public:
+        /**
+         * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with
+         * the hardware. Base implementation uses the values returned by update() so that 
+         * the same values are returned until update() is called again.
+         */
+        virtual float getMechanicalAngle();
+
+        /**
+         * Get current position (in rad) including full rotations and shaft angle.
+         * Base implementation uses the values returned by update() so that the same
+         * values are returned until update() is called again.
+         * Note that this value has limited precision as the number of rotations increases,
+         * because the limited precision of float can't capture the large angle of the full 
+         * rotations and the small angle of the shaft angle at the same time.
+         */
+        virtual float getAngle();
+        
+        /** 
+         * On architectures supporting it, this will return a double precision position value,
+         * which should have improved precision for large position values.
+         * Base implementation uses the values returned by update() so that the same
+         * values are returned until update() is called again.
+         */
+        virtual double getPreciseAngle();
+
+        /** 
+         * Get current angular velocity (rad/s)
+         * Can be overridden in subclasses. Base implementation uses the values 
+         * returned by update() so that it only makes sense to call this if update()
+         * has been called in the meantime.
+         */
+        virtual float getVelocity();
+
+        /**
+         * Get the number of full rotations
+         * Base implementation uses the values returned by update() so that the same
+         * values are returned until update() is called again. 
+         */
+        virtual int32_t getFullRotations();
+
+        /**
+         * Updates the sensor values by reading the hardware sensor.
+         * Some implementations may work with interrupts, and not need this.
+         * The base implementation calls getSensorAngle(), and updates internal
+         * fields for angle, timestamp and full rotations.
+         * This method must be called frequently enough to guarantee that full
+         * rotations are not "missed" due to infrequent polling.
+         * Override in subclasses if alternative behaviours are required for your
+         * sensor hardware.
+         */
+        virtual void update();
+
+        /** 
+         * returns 0 if it does need search for absolute zero
+         * 0 - magnetic sensor (& encoder with index which is found)
+         * 1 - ecoder with index (with index not found yet)
+         */
+        virtual int needsSearch();
+
+        /**
+         * Minimum time between updates to velocity. If time elapsed is lower than this, the velocity is not updated.
+         */
+        float min_elapsed_time = 0.000100; // default is 100 microseconds, or 10kHz
+
+    protected:
+        /** 
+         * Get current shaft angle from the sensor hardware, and 
+         * return it as a float in radians, in the range 0 to 2PI.
+         * 
+         * This method is pure virtual and must be implemented in subclasses.
+         * Calling this method directly does not update the base-class internal fields.
+         * Use update() when calling from outside code.
+         */
+        virtual float getSensorAngle()=0;
+        /**
+         * Call Sensor::init() from your sensor subclass's init method if you want smoother startup
+         * The base class init() method calls getSensorAngle() several times to initialize the internal fields
+         * to current values, ensuring there is no discontinuity ("jump from zero") during the first calls
+         * to sensor.getAngle() and sensor.getVelocity()
+         */
+        virtual void init();
+
+        // velocity calculation variables
+        float velocity=0.0f;
+        float angle_prev=0.0f; // result of last call to getSensorAngle(), used for full rotations and velocity
+        long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity
+        float vel_angle_prev=0.0f; // angle at last call to getVelocity, used for velocity
+        long vel_angle_prev_ts=0; // last velocity calculation timestamp
+        int32_t full_rotations=0; // full rotation tracking
+        int32_t vel_full_rotations=0; // previous full rotation value for velocity calculation
+};
+
+#endif

firmware/lib/Arduino-FOC/src/common/base_classes/StepperDriver.h

@@ -0,0 +1,18 @@
+#ifndef STEPPERDRIVER_H
+#define STEPPERDRIVER_H
+
+#include "drivers/hardware_api.h"
+#include "FOCDriver.h"
+
+class StepperDriver: public FOCDriver{
+    public:
+        /** 
+         * Set phase voltages to the harware 
+         * 
+         * @param Ua phase A voltage
+         * @param Ub phase B voltage
+        */
+        virtual void setPwm(float Ua, float Ub) = 0;
+};
+
+#endif

firmware/lib/Arduino-FOC/src/common/defaults.h

@@ -0,0 +1,49 @@
+// default configuration values
+// change this file to optimal values for your application
+
+#define DEF_POWER_SUPPLY 12.0f //!< default power supply voltage
+// velocity PI controller params
+#define DEF_PID_VEL_P 0.5f //!< default PID controller P value
+#define DEF_PID_VEL_I 10.0f //!<  default PID controller I value
+#define DEF_PID_VEL_D 0.0f //!<  default PID controller D value
+#define DEF_PID_VEL_RAMP 1000.0f //!< default PID controller voltage ramp value
+#define DEF_PID_VEL_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
+
+// current sensing PID values
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__)  || defined(__AVR_ATmega2560__)
+// for 16Mhz controllers like Arduino uno and mega
+#define DEF_PID_CURR_P 2 //!< default PID controller P value
+#define DEF_PID_CURR_I 100 //!<  default PID controller I value
+#define DEF_PID_CURR_D 0.0f //!<  default PID controller D value
+#define DEF_PID_CURR_RAMP 1000.0f //!< default PID controller voltage ramp value
+#define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
+#define DEF_CURR_FILTER_Tf 0.01f //!< default velocity filter time constant
+#else
+// for stm32, due, teensy, esp32 and similar
+#define DEF_PID_CURR_P 3 //!< default PID controller P value
+#define DEF_PID_CURR_I 300.0f //!<  default PID controller I value
+#define DEF_PID_CURR_D 0.0f //!<  default PID controller D value
+#define DEF_PID_CURR_RAMP 0  //!< default PID controller voltage ramp value
+#define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit
+#define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant
+#endif
+// default current limit values
+#define DEF_CURRENT_LIM 2.0f //!< 2Amps current limit by default
+
+// default monitor downsample
+#define DEF_MON_DOWNSMAPLE 100 //!< default monitor downsample
+#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable
+
+// angle P params
+#define DEF_P_ANGLE_P 20.0f //!< default P controller P value
+#define DEF_VEL_LIM 20.0f //!< angle velocity limit default
+
+// index search
+#define DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0f //!< default index search velocity
+// align voltage
+#define DEF_VOLTAGE_SENSOR_ALIGN 3.0f //!< default voltage for sensor and motor zero alignemt
+// low pass filter velocity
+#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant
+
+// current sense default parameters
+#define DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf 0.0f  //!< default currnet sense per phase low pass filter time constant 

firmware/lib/Arduino-FOC/src/common/foc_utils.cpp

@@ -0,0 +1,73 @@
+#include "foc_utils.h"
+
+
+// function approximating the sine calculation by using fixed size array
+// uses a 65 element lookup table and interpolation
+// thanks to @dekutree for his work on optimizing this
+__attribute__((weak)) float _sin(float a){
+  // 16bit integer array for sine lookup. interpolation is used for better precision
+  // 16 bit precision on sine value, 8 bit fractional value for interpolation, 6bit LUT size
+  // resulting precision compared to stdlib sine is 0.00006480 (RMS difference in range -PI,PI for 3217 steps)
+  static uint16_t sine_array[65] = {0,804,1608,2411,3212,4011,4808,5602,6393,7180,7962,8740,9512,10279,11039,11793,12540,13279,14010,14733,15447,16151,16846,17531,18205,18868,19520,20160,20788,21403,22006,22595,23170,23732,24279,24812,25330,25833,26320,26791,27246,27684,28106,28511,28899,29269,29622,29957,30274,30572,30853,31114,31357,31581,31786,31972,32138,32286,32413,32522,32610,32679,32729,32758,32768};
+  unsigned int i = (unsigned int)(a * (64*4*256.0 /_2PI));
+  int t1, t2, frac = i & 0xff;
+  i = (i >> 8) & 0xff;
+  if (i < 64) {
+    t1 = sine_array[i]; t2 = sine_array[i+1];
+  }
+  else if(i < 128) {
+    t1 = sine_array[128 - i]; t2 = sine_array[127 - i];
+  }
+  else if(i < 192) {
+    t1 = -sine_array[-128 + i]; t2 = -sine_array[-127 + i];
+  }
+  else {
+    t1 = -sine_array[256 - i]; t2 = -sine_array[255 - i];
+  }
+  return (1.0f/32768.0f) * (t1 + (((t2 - t1) * frac) >> 8));
+}
+
+// function approximating cosine calculation by using fixed size array
+// ~55us (float array)
+// ~56us (int array)
+// precision +-0.005
+// it has to receive an angle in between 0 and 2PI
+__attribute__((weak)) float _cos(float a){
+  float a_sin = a + _PI_2;
+  a_sin = a_sin > _2PI ? a_sin - _2PI : a_sin;
+  return _sin(a_sin);
+}
+
+
+__attribute__((weak)) void _sincos(float a, float* s, float* c){
+  *s = _sin(a);
+  *c = _cos(a);
+}
+
+
+// normalizing radian angle to [0,2PI]
+__attribute__((weak)) float _normalizeAngle(float angle){
+  float a = fmod(angle, _2PI);
+  return a >= 0 ? a : (a + _2PI);
+}
+
+// Electrical angle calculation
+float _electricalAngle(float shaft_angle, int pole_pairs) {
+  return (shaft_angle * pole_pairs);
+}
+
+// square root approximation function using
+// https://reprap.org/forum/read.php?147,219210
+// https://en.wikipedia.org/wiki/Fast_inverse_square_root
+__attribute__((weak)) float _sqrtApprox(float number) {//low in fat
+  // float x;
+  // const float f = 1.5F; // better precision
+
+  // x = number * 0.5F;
+  float y = number;
+  long i = * ( long * ) &y;
+  i = 0x5f375a86 - ( i >> 1 );
+  y = * ( float * ) &i;
+  // y = y * ( f - ( x * y * y ) ); // better precision
+  return number * y;
+}

firmware/lib/Arduino-FOC/src/common/foc_utils.h

@@ -0,0 +1,106 @@
+#ifndef FOCUTILS_LIB_H
+#define FOCUTILS_LIB_H
+
+#include "Arduino.h"
+
+// sign function
+#define _sign(a) ( ( (a) < 0 )  ?  -1   : ( (a) > 0 ) )
+#ifndef _round
+#define _round(x) ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f))
+#endif
+#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+#define _sqrt(a) (_sqrtApprox(a))
+#define _isset(a) ( (a) != (NOT_SET) )
+#define _UNUSED(v) (void) (v)
+#define _powtwo(x) (1 << (x))
+
+// utility defines
+#define _2_SQRT3 1.15470053838f
+#define _SQRT3 1.73205080757f
+#define _1_SQRT3 0.57735026919f
+#define _SQRT3_2 0.86602540378f
+#define _SQRT2 1.41421356237f
+#define _120_D2R 2.09439510239f
+#define _PI 3.14159265359f
+#define _PI_2 1.57079632679f
+#define _PI_3 1.0471975512f
+#define _2PI 6.28318530718f
+#define _3PI_2 4.71238898038f
+#define _PI_6 0.52359877559f
+#define _RPM_TO_RADS 0.10471975512f
+
+#define NOT_SET -12345.0f
+#define _HIGH_IMPEDANCE 0
+#define _HIGH_Z _HIGH_IMPEDANCE
+#define _ACTIVE 1
+#define _NC (NOT_SET)
+
+#define MIN_ANGLE_DETECT_MOVEMENT (_2PI/101.0f)
+
+// dq current structure
+struct DQCurrent_s
+{
+    float d;
+    float q;
+};
+// phase current structure
+struct PhaseCurrent_s
+{
+    float a;
+    float b;
+    float c;
+};
+// dq voltage structs
+struct DQVoltage_s
+{
+    float d;
+    float q;
+};
+
+
+/**
+ *  Function approximating the sine calculation by using fixed size array
+ * - execution time ~40us (Arduino UNO)
+ *
+ * @param a angle in between 0 and 2PI
+ */
+float _sin(float a);
+/**
+ * Function approximating cosine calculation by using fixed size array
+ * - execution time ~50us (Arduino UNO)
+ *
+ * @param a angle in between 0 and 2PI
+ */
+float _cos(float a);
+/**
+ * Function returning both sine and cosine of the angle in one call.
+ * Internally it uses the _sin and _cos functions, but you may wish to
+ * provide your own implementation which is more optimized.
+ */
+void _sincos(float a, float* s, float* c);
+
+
+/**
+ * normalizing radian angle to [0,2PI]
+ * @param angle - angle to be normalized
+ */
+float _normalizeAngle(float angle);
+
+
+/**
+ * Electrical angle calculation
+ *
+ * @param shaft_angle - shaft angle of the motor
+ * @param pole_pairs - number of pole pairs
+ */
+float _electricalAngle(float shaft_angle, int pole_pairs);
+
+/**
+ * Function approximating square root function
+ *  - using fast inverse square root
+ *
+ * @param value - number
+ */
+float _sqrtApprox(float value);
+
+#endif

firmware/lib/Arduino-FOC/src/common/lowpass_filter.cpp

@@ -0,0 +1,28 @@
+#include "lowpass_filter.h"
+
+LowPassFilter::LowPassFilter(float time_constant)
+    : Tf(time_constant)
+    , y_prev(0.0f)
+{
+    timestamp_prev = _micros();
+}
+
+
+float LowPassFilter::operator() (float x)
+{
+    unsigned long timestamp = _micros();
+    float dt = (timestamp - timestamp_prev)*1e-6f;
+
+    if (dt < 0.0f ) dt = 1e-3f;
+    else if(dt > 0.3f) {
+        y_prev = x;
+        timestamp_prev = timestamp;
+        return x;
+    }
+
+    float alpha = Tf/(Tf + dt);
+    float y = alpha*y_prev + (1.0f - alpha)*x;
+    y_prev = y;
+    timestamp_prev = timestamp;
+    return y;
+}

firmware/lib/Arduino-FOC/src/common/lowpass_filter.h

@@ -0,0 +1,28 @@
+#ifndef LOWPASS_FILTER_H
+#define LOWPASS_FILTER_H
+
+
+#include "time_utils.h"
+#include "foc_utils.h"
+
+/**
+ *  Low pass filter class
+ */
+class LowPassFilter
+{
+public:
+    /**
+     * @param Tf - Low pass filter time constant
+     */
+    LowPassFilter(float Tf);
+    ~LowPassFilter() = default;
+
+    float operator() (float x);
+    float Tf; //!< Low pass filter time constant
+
+protected:
+    unsigned long timestamp_prev;  //!< Last execution timestamp
+    float y_prev; //!< filtered value in previous execution step 
+};
+
+#endif // LOWPASS_FILTER_H

firmware/lib/Arduino-FOC/src/common/pid.cpp

@@ -0,0 +1,64 @@
+#include "pid.h"
+
+PIDController::PIDController(float P, float I, float D, float ramp, float limit)
+    : P(P)
+    , I(I)
+    , D(D)
+    , output_ramp(ramp)    // output derivative limit [volts/second]
+    , limit(limit)         // output supply limit     [volts]
+    , error_prev(0.0f)
+    , output_prev(0.0f)
+    , integral_prev(0.0f)
+{
+    timestamp_prev = _micros();
+}
+
+// PID controller function
+float PIDController::operator() (float error){
+    // calculate the time from the last call
+    unsigned long timestamp_now = _micros();
+    float Ts = (timestamp_now - timestamp_prev) * 1e-6f;
+    // quick fix for strange cases (micros overflow)
+    if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
+
+    // u(s) = (P + I/s + Ds)e(s)
+    // Discrete implementations
+    // proportional part
+    // u_p  = P *e(k)
+    float proportional = P * error;
+    // Tustin transform of the integral part
+    // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
+    float integral = integral_prev + I*Ts*0.5f*(error + error_prev);
+    // antiwindup - limit the output
+    integral = _constrain(integral, -limit, limit);
+    // Discrete derivation
+    // u_dk = D(ek - ek_1)/Ts
+    float derivative = D*(error - error_prev)/Ts;
+
+    // sum all the components
+    float output = proportional + integral + derivative;
+    // antiwindup - limit the output variable
+    output = _constrain(output, -limit, limit);
+
+    // if output ramp defined
+    if(output_ramp > 0){
+        // limit the acceleration by ramping the output
+        float output_rate = (output - output_prev)/Ts;
+        if (output_rate > output_ramp)
+            output = output_prev + output_ramp*Ts;
+        else if (output_rate < -output_ramp)
+            output = output_prev - output_ramp*Ts;
+    }
+    // saving for the next pass
+    integral_prev = integral;
+    output_prev = output;
+    error_prev = error;
+    timestamp_prev = timestamp_now;
+    return output;
+}
+
+void PIDController::reset(){
+    integral_prev = 0.0f;
+    output_prev = 0.0f;
+    error_prev = 0.0f;
+}

firmware/lib/Arduino-FOC/src/common/pid.h

@@ -0,0 +1,41 @@
+#ifndef PID_H
+#define PID_H
+
+
+#include "time_utils.h"
+#include "foc_utils.h"
+
+/**
+ *  PID controller class
+ */
+class PIDController
+{
+public:
+    /**
+     *  
+     * @param P - Proportional gain 
+     * @param I - Integral gain
+     * @param D - Derivative gain 
+     * @param ramp - Maximum speed of change of the output value
+     * @param limit - Maximum output value
+     */
+    PIDController(float P, float I, float D, float ramp, float limit);
+    ~PIDController() = default;
+
+    float operator() (float error);
+    void reset();
+
+    float P; //!< Proportional gain 
+    float I; //!< Integral gain 
+    float D; //!< Derivative gain 
+    float output_ramp; //!< Maximum speed of change of the output value
+    float limit; //!< Maximum output value
+
+protected:
+    float error_prev; //!< last tracking error value
+    float output_prev;  //!< last pid output value
+    float integral_prev; //!< last integral component value
+    unsigned long timestamp_prev; //!< Last execution timestamp
+};
+
+#endif // PID_H

firmware/lib/Arduino-FOC/src/common/time_utils.cpp

@@ -0,0 +1,31 @@
+#include "time_utils.h"
+
+// function buffering delay() 
+// arduino uno function doesn't work well with interrupts
+void _delay(unsigned long ms){
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__)  || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__)
+  // if arduino uno and other atmega328p chips
+  // use while instad of delay, 
+  // due to wrong measurement based on changed timer0
+  unsigned long t = _micros() + ms*1000;
+  while( _micros() < t ){}; 
+#else
+  // regular micros
+  delay(ms);
+#endif
+}
+
+
+// function buffering _micros() 
+// arduino function doesn't work well with interrupts
+unsigned long _micros(){
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__)  || defined(__AVR_ATmega2560__) || defined(__AVR_ATmega32U4__)
+// if arduino uno and other atmega328p chips
+    //return the value based on the prescaler
+    if((TCCR0B & 0b00000111) == 0x01) return (micros()/32);
+    else return (micros());
+#else
+  // regular micros
+  return micros();
+#endif
+}

firmware/lib/Arduino-FOC/src/common/time_utils.h

@@ -0,0 +1,22 @@
+#ifndef TIME_UTILS_H
+#define TIME_UTILS_H
+
+#include "foc_utils.h"
+
+/** 
+ * Function implementing delay() function in milliseconds 
+ * - blocking function
+ * - hardware specific
+
+ * @param ms number of milliseconds to wait
+ */
+void _delay(unsigned long ms);
+
+/** 
+ * Function implementing timestamp getting function in microseconds
+ * hardware specific
+ */
+unsigned long _micros();
+
+
+#endif

firmware/lib/Arduino-FOC/src/communication/Commander.cpp

@@ -0,0 +1,685 @@
+#include "Commander.h"
+
+Commander::Commander(Stream& serial, char eol, bool echo){
+  com_port = &serial;
+  this->eol = eol;
+  this->echo = echo;
+}
+Commander::Commander(char eol, bool echo){
+  this->eol = eol;
+  this->echo = echo;
+}
+
+
+void Commander::add(char id, CommandCallback onCommand, char* label ){
+  call_list[call_count] = onCommand;
+  call_ids[call_count] = id;
+  call_label[call_count] = label;
+  call_count++;
+}
+
+
+void Commander::run(){
+  if(!com_port) return;
+  run(*com_port, eol);
+}
+
+void Commander::run(Stream& serial, char eol){
+  Stream* tmp = com_port; // save the serial instance
+  char eol_tmp = this->eol;
+  this->eol = eol;
+  com_port = &serial;
+
+  // a string to hold incoming data
+  while (serial.available()) {
+    // get the new byte:
+    int ch = serial.read();
+    received_chars[rec_cnt++] = (char)ch;
+    // end of user input
+    if(echo)
+      print((char)ch);
+    if (isSentinel(ch)) {
+      // execute the user command
+      run(received_chars);
+
+      // reset the command buffer
+      received_chars[0] = 0;
+      rec_cnt=0;
+    }
+    if (rec_cnt>=MAX_COMMAND_LENGTH) { // prevent buffer overrun if message is too long
+        received_chars[0] = 0;
+        rec_cnt=0;
+    }
+  }
+
+  com_port = tmp; // reset the instance to the internal value
+  this->eol = eol_tmp;
+}
+
+void Commander::run(char* user_input){
+  // execute the user command
+  char id = user_input[0];
+  switch(id){
+    case CMD_SCAN:
+      for(int i=0; i < call_count; i++){
+          printMachineReadable(CMD_SCAN);
+          print(call_ids[i]);
+          print(":");
+          if(call_label[i]) println(call_label[i]);
+          else println("");
+      }
+      break;
+    case CMD_VERBOSE:
+      if(!isSentinel(user_input[1])) verbose = (VerboseMode)atoi(&user_input[1]);
+      printVerbose(F("Verb:"));
+      printMachineReadable(CMD_VERBOSE);
+      switch (verbose){
+      case VerboseMode::nothing:
+        println(F("off!"));
+        break;
+      case VerboseMode::on_request:
+      case VerboseMode::user_friendly:
+        println(F("on!"));
+        break;
+      case VerboseMode::machine_readable:
+        printlnMachineReadable(F("machine"));
+        break;
+      }
+      break;
+    case CMD_DECIMAL:
+      if(!isSentinel(user_input[1])) decimal_places = atoi(&user_input[1]);
+      printVerbose(F("Decimal:"));
+      printMachineReadable(CMD_DECIMAL);
+      println(decimal_places);
+      break;
+    default:
+      for(int i=0; i < call_count; i++){
+        if(id == call_ids[i]){
+          printMachineReadable(user_input[0]);
+          call_list[i](&user_input[1]);
+          break;
+        }
+      }
+      break;
+  }
+}
+
+void Commander::motor(FOCMotor* motor, char* user_command) {
+
+  // if target setting
+  if(isDigit(user_command[0]) || user_command[0] == '-' || user_command[0] == '+' || isSentinel(user_command[0])){
+    target(motor, user_command);
+    return;
+  }
+
+  // parse command letter
+  char cmd = user_command[0];
+  char sub_cmd = user_command[1];
+  // check if there is a subcommand or not
+  int value_index = (sub_cmd >= 'A'  && sub_cmd <= 'Z') ||  (sub_cmd == '#') ?  2 :  1;
+  // check if get command
+  bool GET = isSentinel(user_command[value_index]);
+  // parse command values
+  float value = atof(&user_command[value_index]);
+  printMachineReadable(cmd);
+  if (sub_cmd >= 'A'  && sub_cmd <= 'Z') {
+    printMachineReadable(sub_cmd);
+  }
+
+  // a bit of optimisation of variable memory for Arduino UNO (atmega328)
+  switch(cmd){
+    case CMD_C_Q_PID:      //
+      printVerbose(F("PID curr q| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_q, &user_command[1]);
+      else pid(&motor->PID_current_q,&user_command[1]);
+      break;
+    case CMD_C_D_PID:      //
+      printVerbose(F("PID curr d| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_current_d, &user_command[1]);
+      else pid(&motor->PID_current_d, &user_command[1]);
+      break;
+    case CMD_V_PID:      //
+      printVerbose(F("PID vel| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_velocity, &user_command[1]);
+      else pid(&motor->PID_velocity, &user_command[1]);
+      break;
+    case CMD_A_PID:      //
+      printVerbose(F("PID angle| "));
+      if(sub_cmd == SCMD_LPF_TF) lpf(&motor->LPF_angle, &user_command[1]);
+      else pid(&motor->P_angle, &user_command[1]);
+      break;
+    case CMD_LIMITS:      //
+     printVerbose(F("Limits| "));
+      switch (sub_cmd){
+        case SCMD_LIM_VOLT:      // voltage limit change
+          printVerbose(F("volt: "));
+          if(!GET) {
+            motor->voltage_limit = value;
+            motor->PID_current_d.limit = value;
+            motor->PID_current_q.limit = value;
+            // change velocity pid limit if in voltage mode and no phase resistance set
+            if( !_isset(motor->phase_resistance) && motor->torque_controller==TorqueControlType::voltage) motor->PID_velocity.limit = value;
+          }
+          println(motor->voltage_limit);
+          break;
+        case SCMD_LIM_CURR:      // current limit
+          printVerbose(F("curr: "));
+          if(!GET){
+            motor->current_limit = value;
+            // if phase resistance specified or the current control is on set the current limit to the velocity PID
+            if(_isset(motor->phase_resistance) || motor->torque_controller != TorqueControlType::voltage ) motor->PID_velocity.limit = value;
+          }
+          println(motor->current_limit);
+          break;
+        case SCMD_LIM_VEL:      // velocity limit
+          printVerbose(F("vel: "));
+          if(!GET){
+            motor->velocity_limit = value;
+            motor->P_angle.limit = value;
+          }
+          println(motor->velocity_limit);
+          break;
+        default:
+          printError();
+          break;
+      }
+      break;
+    case CMD_MOTION_TYPE:
+    case CMD_TORQUE_TYPE:
+    case CMD_STATUS:
+      motion(motor, &user_command[0]);
+      break;
+    case CMD_PWMMOD:
+       // PWM modulation change
+       printVerbose(F("PWM Mod | "));
+       switch (sub_cmd){
+        case SCMD_PWMMOD_TYPE:      // zero offset
+          printVerbose(F("type: "));
+          if(!GET) motor->foc_modulation = (FOCModulationType)value;
+          switch(motor->foc_modulation){
+            case FOCModulationType::SinePWM:
+              println(F("SinePWM"));
+              break;
+            case FOCModulationType::SpaceVectorPWM:
+              println(F("SVPWM"));
+              break;
+            case FOCModulationType::Trapezoid_120:
+              println(F("Trap 120"));
+              break;
+            case FOCModulationType::Trapezoid_150:
+              println(F("Trap 150"));
+              break;
+          }
+          break;
+        case SCMD_PWMMOD_CENTER:      // centered modulation
+          printVerbose(F("center: "));
+          if(!GET) motor->modulation_centered = value;
+          println(motor->modulation_centered);
+          break;
+        default:
+          printError();
+          break;
+       }
+      break;
+    case CMD_RESIST:
+      printVerbose(F("R phase: "));
+      if(!GET){
+        motor->phase_resistance = value;
+        if(motor->torque_controller==TorqueControlType::voltage)
+          motor->PID_velocity.limit= motor->current_limit;
+      }
+      if(_isset(motor->phase_resistance)) println(motor->phase_resistance);
+      else println(0);
+      break;
+    case CMD_INDUCTANCE:
+      printVerbose(F("L phase: "));
+      if(!GET){
+        motor->phase_inductance = value;
+      }
+      if(_isset(motor->phase_inductance)) println(motor->phase_inductance);
+      else println(0);
+      break;
+    case CMD_KV_RATING:
+      printVerbose(F("Motor KV: "));
+      if(!GET){
+        motor->KV_rating = value;
+      }
+      if(_isset(motor->KV_rating)) println(motor->KV_rating);
+      else println(0);
+      break;
+    case CMD_SENSOR:
+      // Sensor zero offset
+       printVerbose(F("Sensor | "));
+       switch (sub_cmd){
+        case SCMD_SENS_MECH_OFFSET:      // zero offset
+          printVerbose(F("offset: "));
+          if(!GET) motor->sensor_offset = value;
+          println(motor->sensor_offset);
+          break;
+        case SCMD_SENS_ELEC_OFFSET:      // electrical zero offset - not suggested to touch
+          printVerbose(F("el. offset: "));
+          if(!GET) motor->zero_electric_angle = value;
+          println(motor->zero_electric_angle);
+          break;
+        default:
+          printError();
+          break;
+       }
+      break;
+    case CMD_MONITOR:     // get current values of the state variables
+      printVerbose(F("Monitor | "));
+      switch (sub_cmd){
+        case SCMD_GET:      // get command
+          switch((uint8_t)value){
+            case 0: // get target
+              printVerbose(F("target: "));
+              println(motor->target);
+              break;
+            case 1: // get voltage q
+              printVerbose(F("Vq: "));
+              println(motor->voltage.q);
+              break;
+            case 2: // get voltage d
+              printVerbose(F("Vd: "));
+              println(motor->voltage.d);
+              break;
+            case 3: // get current q
+              printVerbose(F("Cq: "));
+              println(motor->current.q);
+              break;
+            case 4: // get current d
+              printVerbose(F("Cd: "));
+              println(motor->current.d);
+              break;
+            case 5: // get velocity
+              printVerbose(F("vel: "));
+              println(motor->shaft_velocity);
+              break;
+            case 6: // get angle
+              printVerbose(F("angle: "));
+              println(motor->shaft_angle);
+              break;
+            case 7: // get all states
+              printVerbose(F("all: "));
+              print(motor->target);
+              print(";");
+              print(motor->voltage.q);
+              print(";");
+              print(motor->voltage.d);
+              print(";");
+              print(motor->current.q);
+              print(";");
+              print(motor->current.d);
+              print(";");
+              print(motor->shaft_velocity);
+              print(";");
+              println(motor->shaft_angle);
+              break;
+            default:
+              printError();
+              break;
+          }
+          break;
+        case SCMD_DOWNSAMPLE:
+          printVerbose(F("downsample: "));
+          if(!GET) motor->monitor_downsample = value;
+          println((int)motor->monitor_downsample);
+          break;
+        case SCMD_CLEAR:
+          motor->monitor_variables = (uint8_t) 0;
+          println(F("clear"));
+          break;
+        case CMD_DECIMAL:
+          printVerbose(F("decimal: "));
+          motor->monitor_decimals = value;
+          println((int)motor->monitor_decimals);
+          break;
+        case SCMD_SET:
+          if(!GET){
+            // set the variables
+            motor->monitor_variables = (uint8_t) 0;
+            for(int i = 0; i < 7; i++){
+              if(isSentinel(user_command[value_index+i])) break;
+              motor->monitor_variables |=  (user_command[value_index+i] - '0') << (6-i);
+            }
+          }
+          // print the variables
+          for(int i = 0; i < 7; i++){
+            print( (motor->monitor_variables & (1 << (6-i))) >> (6-i));
+          }
+          println("");
+          break;
+        default:
+          printError();
+          break;
+       }
+      break;
+    default:  // unknown cmd
+      printVerbose(F("unknown cmd "));
+      printError();
+  }
+}
+
+void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){
+  char cmd = user_cmd[0];
+  char sub_cmd = user_cmd[1];
+  bool GET  = isSentinel(user_cmd[1]);
+  float value = atof(&user_cmd[(sub_cmd >= 'A'  && sub_cmd <= 'Z') ?  2 :  1]);
+
+  switch(cmd){
+    case CMD_MOTION_TYPE:
+      printVerbose(F("Motion:"));
+      switch(sub_cmd){
+        case SCMD_DOWNSAMPLE:
+            printVerbose(F(" downsample: "));
+            if(!GET) motor->motion_downsample = value;
+            println((int)motor->motion_downsample);
+          break;
+        default:
+          // change control type
+          if(!GET && value >= 0 && (int)value < 5) // if set command
+            motor->controller = (MotionControlType)value;
+          switch(motor->controller){
+            case MotionControlType::torque:
+              println(F("torque"));
+              break;
+            case MotionControlType::velocity:
+              println(F("vel"));
+              break;
+            case MotionControlType::angle:
+              println(F("angle"));
+              break;
+            case MotionControlType::velocity_openloop:
+              println(F("vel open"));
+              break;
+            case MotionControlType::angle_openloop:
+              println(F("angle open"));
+              break;
+          }
+            break;
+        }
+      break;
+    case CMD_TORQUE_TYPE:
+      // change control type
+      printVerbose(F("Torque: "));
+      if(!GET && (int8_t)value >= 0 && (int8_t)value < 3)// if set command
+        motor->torque_controller = (TorqueControlType)value;
+      switch(motor->torque_controller){
+        case TorqueControlType::voltage:
+          println(F("volt"));
+          // change the velocity control limits if necessary
+          if( !_isset(motor->phase_resistance) ) motor->PID_velocity.limit = motor->voltage_limit;
+          break;
+        case TorqueControlType::dc_current:
+          println(F("dc curr"));
+          // change the velocity control limits if necessary
+          motor->PID_velocity.limit = motor->current_limit;
+          break;
+        case TorqueControlType::foc_current:
+          println(F("foc curr"));
+          // change the velocity control limits if necessary
+          motor->PID_velocity.limit = motor->current_limit;
+          break;
+      }
+      break;
+    case CMD_STATUS:
+      // enable/disable
+      printVerbose(F("Status: "));
+      if(!GET) (bool)value ? motor->enable() : motor->disable();
+       println(motor->enabled);
+      break;
+    default:
+      target(motor,  user_cmd, separator);
+      break;
+  }
+}
+
+void Commander::pid(PIDController* pid, char* user_cmd){
+  char cmd = user_cmd[0];
+  bool GET  = isSentinel(user_cmd[1]);
+  float value = atof(&user_cmd[1]);
+
+  switch (cmd){
+    case SCMD_PID_P:      // P gain change
+      printVerbose("P: ");
+      if(!GET) pid->P = value;
+      println(pid->P);
+      break;
+    case SCMD_PID_I:      // I gain change
+      printVerbose("I: ");
+      if(!GET) pid->I = value;
+      println(pid->I);
+      break;
+    case SCMD_PID_D:      // D gain change
+      printVerbose("D: ");
+      if(!GET) pid->D = value;
+      println(pid->D);
+      break;
+    case SCMD_PID_RAMP:      //  ramp change
+      printVerbose("ramp: ");
+      if(!GET) pid->output_ramp = value;
+      println(pid->output_ramp);
+      break;
+    case SCMD_PID_LIM:      //  limit change
+      printVerbose("limit: ");
+      if(!GET) pid->limit = value;
+      println(pid->limit);
+      break;
+    default:
+      printError();
+      break;
+  }
+}
+
+void Commander::lpf(LowPassFilter* lpf, char* user_cmd){
+  char cmd = user_cmd[0];
+  bool GET  = isSentinel(user_cmd[1]);
+  float value = atof(&user_cmd[1]);
+
+  switch (cmd){
+    case SCMD_LPF_TF:      // Tf value change
+      printVerbose(F("Tf: "));
+      if(!GET) lpf->Tf = value;
+      println(lpf->Tf);
+      break;
+    default:
+      printError();
+      break;
+  }
+}
+
+void Commander::scalar(float* value,  char* user_cmd){
+  bool GET  = isSentinel(user_cmd[0]);
+  if(!GET) *value = atof(user_cmd);
+  println(*value);
+}
+
+
+void Commander::target(FOCMotor* motor,  char* user_cmd, char* separator){
+  // if no values sent
+  if(isSentinel(user_cmd[0])) {
+    printlnMachineReadable(motor->target);
+    return;
+  };
+
+  float pos, vel, torque;
+  char* next_value;
+  switch(motor->controller){
+    case MotionControlType::torque: // setting torque target
+      torque = atof(strtok (user_cmd, separator));
+      motor->target = torque;
+      break;
+    case MotionControlType::velocity: // setting velocity target + torque limit
+      // set the target
+      vel= atof(strtok (user_cmd, separator));
+      motor->target = vel;
+
+      // allow for setting only the target velocity without chaning the torque limit
+      next_value = strtok (NULL, separator);
+      if (next_value){
+        torque = atof(next_value);
+        motor->PID_velocity.limit = torque;
+        // torque command can be voltage or current
+        if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
+        else  motor->current_limit = torque;
+      }
+      break;
+    case MotionControlType::angle: // setting angle target + torque, velocity limit
+      // setting the target position
+      pos= atof(strtok (user_cmd, separator));
+      motor->target = pos;
+
+      // allow for setting only the target position without chaning the velocity/torque limits 
+      next_value = strtok (NULL, separator);
+      if( next_value ){
+        vel = atof(next_value);
+        motor->velocity_limit = vel;
+        motor->P_angle.limit = vel;
+  
+        // allow for setting only the target position and velocity limit without the torque limit 
+        next_value = strtok (NULL, separator);
+        if( next_value ){
+          torque= atof(next_value);
+          motor->PID_velocity.limit = torque;
+          // torque command can be voltage or current
+          if(!_isset(motor->phase_resistance) && motor->torque_controller == TorqueControlType::voltage) motor->voltage_limit = torque;
+          else  motor->current_limit = torque;
+        }
+      }
+      break;
+    case MotionControlType::velocity_openloop: // setting velocity target + torque limit
+      // set the target
+      vel= atof(strtok (user_cmd, separator));
+      motor->target = vel;
+      // allow for setting only the target velocity without chaning the torque limit
+      next_value = strtok (NULL, separator);
+      if (next_value ){
+        torque = atof(next_value);
+        // torque command can be voltage or current
+        if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
+        else  motor->current_limit = torque;
+      }
+      break;
+    case MotionControlType::angle_openloop: // setting angle target + torque, velocity limit
+      // set the target
+      pos= atof(strtok (user_cmd, separator));
+      motor->target = pos; 
+      
+      // allow for setting only the target position without chaning the velocity/torque limits 
+      next_value = strtok (NULL, separator);
+      if( next_value ){
+        vel = atof(next_value);
+        motor->velocity_limit = vel;
+        // allow for setting only the target velocity without chaning the torque limit
+        next_value = strtok (NULL, separator);
+        if (next_value ){
+          torque = atof(next_value);
+          // torque command can be voltage or current
+          if(!_isset(motor->phase_resistance)) motor->voltage_limit = torque;
+          else  motor->current_limit = torque;
+        }
+      }
+      break;
+  }
+  printVerbose(F("Target: "));
+  println(motor->target);
+}
+
+
+bool Commander::isSentinel(char ch)
+{
+  if(ch == eol)
+    return true;
+  else if (ch == '\r')
+  {
+      printVerbose(F("Warn: \\r detected! \n"));
+      return true; // lets still consider it to end the line...
+  }
+  return false;
+}
+
+void Commander::print(const int number){
+  if( !com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(number);
+}
+void Commander::print(const float number){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print((float)number,(int)decimal_places);
+}
+void Commander::print(const char* message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(message);
+}
+void Commander::print(const __FlashStringHelper *message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(message);
+}
+void Commander::print(const char message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->print(message);
+}
+
+void Commander::println(const int number){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(number);
+}
+void Commander::println(const float number){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println((float)number, (int)decimal_places);
+}
+void Commander::println(const char* message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(message);
+}
+void Commander::println(const __FlashStringHelper *message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(message);
+}
+void Commander::println(const char message){
+  if(!com_port || verbose == VerboseMode::nothing ) return;
+  com_port->println(message);
+}
+
+
+void Commander::printVerbose(const char* message){
+  if(verbose == VerboseMode::user_friendly) print(message);
+}
+void Commander::printVerbose(const __FlashStringHelper *message){
+  if(verbose == VerboseMode::user_friendly) print(message);
+}
+
+void Commander::printMachineReadable(const int number){
+  if(verbose == VerboseMode::machine_readable) print(number);
+}
+void Commander::printMachineReadable(const float number){
+  if(verbose == VerboseMode::machine_readable) print(number);
+}
+void Commander::printMachineReadable(const char* message){
+  if(verbose == VerboseMode::machine_readable) print(message);
+}
+void Commander::printMachineReadable(const __FlashStringHelper *message){
+  if(verbose == VerboseMode::machine_readable) print(message);
+}
+void Commander::printMachineReadable(const char message){
+  if(verbose == VerboseMode::machine_readable) print(message);
+}
+
+void Commander::printlnMachineReadable(const int number){
+  if(verbose == VerboseMode::machine_readable) println(number);
+}
+void Commander::printlnMachineReadable(const float number){
+  if(verbose == VerboseMode::machine_readable) println(number);
+}
+void Commander::printlnMachineReadable(const char* message){
+  if(verbose == VerboseMode::machine_readable) println(message);
+}
+void Commander::printlnMachineReadable(const __FlashStringHelper *message){
+  if(verbose == VerboseMode::machine_readable) println(message);
+}
+void Commander::printlnMachineReadable(const char message){
+  if(verbose == VerboseMode::machine_readable) println(message);
+}
+
+void Commander::printError(){
+ println(F("err"));
+}

firmware/lib/Arduino-FOC/src/communication/Commander.h

@@ -0,0 +1,301 @@
+#ifndef COMMANDS_H
+#define COMMANDS_H
+
+#include "Arduino.h"
+#include "../common/base_classes/FOCMotor.h"
+#include "../common/pid.h"
+#include "../common/lowpass_filter.h"
+#include "commands.h"
+
+
+#define MAX_COMMAND_LENGTH 20
+
+
+// Commander verbose display to the user type
+enum VerboseMode : uint8_t {
+  nothing       = 0x00, // display nothing - good for monitoring
+  on_request    = 0x01, // display only on user request
+  user_friendly = 0x02,  // display textual messages to the user
+  machine_readable = 0x03 // display machine readable commands, matching commands to set each settings
+};
+
+
+// callback function pointer definiton
+typedef void (* CommandCallback)(char*); //!< command callback function pointer
+
+/**
+ * Commander class implementing string communication protocol based on IDvalue (ex AB5.321 - command id `A`, sub-command id `B`,value `5.321`)
+ *
+ *  - This class can be used in combination with HardwareSerial instance which it would read and write
+ *    or it can be used to parse strings that have been received from the user outside this library
+ *  - Commander class implements command protocol for few standard components of the SimpleFOC library
+ *     - FOCMotor
+ *     - PIDController
+ *     - LowPassFilter
+ *  - Commander also provides a very simple command > callback interface that enables user to
+ *    attach a callback function to certain command id - see function add()
+ */
+class Commander
+{
+  public:
+    /**
+     * Default constructor receiving a serial interface that it uses to output the values to
+     * Also if the function run() is used it uses this serial instance to read the serial for user commands
+     *
+     * @param serial - Serial com port instance
+     * @param eol - the end of line sentinel character
+     * @param echo - echo last typed character (for command line feedback)
+     */
+    Commander(Stream &serial, char eol = '\n', bool echo = false);
+    Commander(char eol = '\n', bool echo = false);
+
+    /**
+     * Function reading the serial port and firing callbacks that have been added to the commander
+     * once the user has requested them - when he sends the command
+     *
+     *  - It has default commands (the letters can be changed in the commands.h file)
+     *    '@' - Verbose mode
+     *    '#' - Number of decimal places
+     *    '?' - Scan command - displays all the labels of attached nodes
+     */
+    void run();
+    /**
+     * Function reading the string of user input and firing callbacks that have been added to the commander
+     * once the user has requested them - when he sends the command
+     *
+     *  - It has default commands (the letters can be changed in the commands.h file)
+     *    '@' - Verbose mode
+     *    '#' - Number of decimal places
+     *    '?' - Scan command - displays all the labels of attached nodes
+     *
+     * @param reader - temporary stream to read user input
+     * @param eol - temporary end of line sentinel
+     */
+    void run(Stream &reader, char eol = '\n');
+    /**
+     * Function reading the string of user input and firing callbacks that have been added to the commander
+     * once the user has requested them - when he sends the command
+     *
+     *  - It has default commands (the letters can be changed in the commands.h file)
+     *    '@' - Verbose mode
+     *    '#' - Number of decimal places
+     *    '?' - Scan command - displays all the labels of attached nodes
+     *
+     * @param user_input - string of user inputs
+     */
+    void run(char* user_input);
+
+    /**
+     *  Function adding a callback to the coomander withe the command id
+     * @param id         - char command letter
+     * @param onCommand  - function pointer void function(char*)
+     * @param label      - string label to be displayed when scan command sent
+     */
+    void add(char id , CommandCallback onCommand, char* label = nullptr);
+
+    // printing variables
+    VerboseMode verbose = VerboseMode::user_friendly; //!< flag signaling that the commands should output user understanable text
+    uint8_t decimal_places = 3; //!< number of decimal places to be used when displaying numbers
+
+    // monitoring functions
+    Stream* com_port = nullptr; //!< Serial terminal variable if provided
+    char eol = '\n'; //!< end of line sentinel character
+    bool echo = false; //!< echo last typed character (for command line feedback)
+
+    /**
+     *
+     * FOC motor (StepperMotor and BLDCMotor) command interface
+     * @param motor    - FOCMotor (BLDCMotor or StepperMotor) instance 
+     * @param user_cmd - the string command
+     * 
+     *  - It has several paramters (the letters can be changed in the commands.h file)
+     *    'Q' - Q current PID controller & LPF (see function pid and lpf for commands)
+     *    'D' - D current PID controller & LPF (see function pid and lpf for commands)
+     *    'V' - Velocity PID controller & LPF  (see function pid and lpf for commands)
+     *    'A' - Angle PID controller & LPF     (see function pid and lpf for commands)
+     *    'L' - Limits
+     *           sub-commands:
+     *           'C' - Current
+     *           'U' - Voltage
+     *           'V' - Velocity
+     *    'C' - Motion control type config
+     *          sub-commands:
+     *          'D' - downsample motiron loop
+     *          '0' - torque
+     *          '1' - velocity
+     *          '2' - angle
+     *    'T' - Torque control type
+     *          sub-commands:
+     *          '0' - voltage
+     *          '1' - current
+     *          '2' - foc_current
+     *    'E' - Motor status (enable/disable)
+     *          sub-commands:
+     *          '0' - enable
+     *          '1' - disable
+     *    'R' - Motor resistance
+     *    'S' - Sensor offsets
+     *          sub-commands:
+     *          'M' - sensor offset
+     *          'E' - sensor electrical zero
+     *    'M' - Monitoring control
+     *          sub-commands:
+     *          'D' - downsample monitoring
+     *          'C' - clear monitor
+     *          'S' - set monitoring variables
+     *          'G' - get variable value
+     *    '' - Target setting interface 
+     *         Depends of the motion control mode:
+     *          - torque                          : torque (ex. M2.5) 
+     *          - velocity (open and closed loop) : velocity torque (ex.M10 2.5 or M10 to only chanage the target witout limits)
+     *          - angle    (open and closed loop) : angle velocity torque (ex.M3.5 10 2.5 or M3.5 to only chanage the target witout limits)
+     *
+     *  - Each of them can be get by sening the command letter -(ex. 'R' - to get the phase resistance)
+     *  - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5f)
+     *
+     */
+    void motor(FOCMotor* motor, char* user_cmd);
+
+    /**
+     * Low pass fileter command interface
+     * @param lpf      - LowPassFilter instance 
+     * @param user_cmd - the string command
+     * 
+     *  - It only has one property - filtering time constant Tf
+     *  - It can be get by sending 'F'
+     *  - It can be set by sending 'Fvalue' - (ex. F0.01 for settin Tf=0.01)
+     */
+    void lpf(LowPassFilter* lpf, char* user_cmd);
+    /**
+     * PID controller command interface
+     * @param pid      - PIDController instance 
+     * @param user_cmd - the string command
+     * 
+     *  - It has several paramters (the letters can be changed in the commands.h file)
+     *     - P gain       - 'P'
+     *     - I gain       - 'I'
+     *     - D gain       - 'D'
+     *     - output ramp  - 'R'
+     *     - output limit - 'L'
+     *  - Each of them can be get by sening the command letter -(ex. 'D' - to get the D gain)
+     *  - Each of them can be set by sending 'IDvalue' - (ex. I1.5 for setting I=1.5)
+     */
+    void pid(PIDController* pid, char* user_cmd);
+    /**
+     * Float variable scalar command interface
+     * @param value    - float variable pointer 
+     * @param user_cmd - the string command
+     * 
+     *  - It only has one property - one float value
+     *  - It can be get by sending an empty string '\n'
+     *  - It can be set by sending 'value' - (ex. 0.01f for settin *value=0.01)
+     */
+    void scalar(float* value, char* user_cmd);
+    /**
+     *  Target setting interface, enables setting the target and limiting variables at once. 
+     *  The values are sent separated by a separator specified as the third argument. The default separator is the space.
+     * 
+     * @param motor     - FOCMotor (BLDCMotor or StepperMotor) instance 
+     * @param user_cmd  - the string command
+     * @param separator - the string separator in between target and limit values, default is space - " "
+     *  
+     *  Example: P2.34 70 2
+     *  `P` is the user defined command, `2.34` is the target angle `70` is the target 
+     *  velocity and `2` is the desired max current.
+     *  
+     *  It depends of the motion control mode:
+     *    - torque   : torque (ex. P2.5) 
+     *    - velocity : velocity torque (ex.P10 2.5 or P10 to only chanage the target witout limits)
+     *    - angle    : angle velocity torque (ex.P3.5 10 2.5 or P3.5 to only chanage the target witout limits)
+     */
+    void target(FOCMotor* motor, char* user_cmd, char* separator = (char *)" ");
+
+    /**
+     * FOC motor (StepperMotor and BLDCMotor) motion control interfaces
+     * @param motor     - FOCMotor (BLDCMotor or StepperMotor) instance 
+     * @param user_cmd  - the string command
+     * @param separator - the string separator in between target and limit values, default is space - " "
+     * 
+     * Commands:
+     *    'C' - Motion control type config
+     *          sub-commands:
+     *          'D' - downsample motiron loop
+     *          '0' - torque
+     *          '1' - velocity
+     *          '2' - angle
+     *    'T' - Torque control type
+     *          sub-commands:
+     *          '0' - voltage
+     *          '1' - current
+     *          '2' - foc_current
+     *    'E' - Motor status (enable/disable)
+     *          sub-commands:
+     *          '0' - enable
+     *          '1' - disable
+     *    '' - Target setting interface 
+     *         Depends of the motion control mode:
+     *          - torque                          : torque (ex. M2.5) 
+     *          - velocity (open and closed loop) : velocity torque (ex.M10 2.5 or M10 to only chanage the target witout limits)
+     *          - angle    (open and closed loop) : angle velocity torque (ex.M3.5 10 2.5 or M3.5 to only chanage the target witout limits)
+     */
+    void motion(FOCMotor* motor, char* user_cmd, char* separator = (char *)" ");
+
+    bool isSentinel(char ch);
+  private:
+    // Subscribed command callback variables
+    CommandCallback call_list[20];//!< array of command callback pointers - 20 is an arbitrary number
+    char call_ids[20]; //!< added callback commands
+    char* call_label[20]; //!< added callback labels
+    int call_count = 0;//!< number callbacks that are subscribed
+
+    // helping variable for serial communication reading
+    char received_chars[MAX_COMMAND_LENGTH] = {0}; //!< so far received user message - waiting for newline
+    int rec_cnt = 0; //!< number of characters receives
+
+    // serial printing functions
+    /**
+     *  print the string message only if verbose mode on
+     *  @param message - message to be printed
+     */
+    void printVerbose(const char* message);
+    /**
+     *  Print the string message only if verbose mode on
+     *  - Function handling the case for strings defined by F macro
+     *  @param message - message to be printed
+     */
+    void printVerbose(const __FlashStringHelper *message);
+    /**
+     *  print the numbers to the serial with desired decimal point number
+     *  @param message - number to be printed
+     *  @param newline - if needs lewline (1) otherwise (0)
+     */
+
+    void print(const float number);
+    void print(const int number);
+    void print(const char* message);
+    void print(const __FlashStringHelper *message);
+    void print(const char message);
+    void println(const float number);
+    void println(const int number);
+    void println(const char* message);
+    void println(const __FlashStringHelper *message);
+    void println(const char message);
+
+    void printMachineReadable(const float number);
+    void printMachineReadable(const int number);
+    void printMachineReadable(const char* message);
+    void printMachineReadable(const __FlashStringHelper *message);
+    void printMachineReadable(const char message);
+
+    void printlnMachineReadable(const float number);
+    void printlnMachineReadable(const int number);
+    void printlnMachineReadable(const char* message);
+    void printlnMachineReadable(const __FlashStringHelper *message);
+    void printlnMachineReadable(const char message);
+
+
+    void printError();
+};
+
+
+#endif

firmware/lib/Arduino-FOC/src/communication/SimpleFOCDebug.cpp

@@ -0,0 +1,104 @@
+
+#include "SimpleFOCDebug.h"
+
+#ifndef SIMPLEFOC_DISABLE_DEBUG
+
+
+Print* SimpleFOCDebug::_debugPrint = NULL;
+
+
+void SimpleFOCDebug::enable(Print* debugPrint) {
+    _debugPrint = debugPrint;
+}
+
+
+void SimpleFOCDebug::println(int val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->println(val);
+    }
+}
+
+void SimpleFOCDebug::println(float val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->println(val);
+    }
+}
+
+
+
+void SimpleFOCDebug::println(const char* str) {
+    if (_debugPrint != NULL) {
+        _debugPrint->println(str);
+    }
+}
+
+void SimpleFOCDebug::println(const __FlashStringHelper* str) {
+    if (_debugPrint != NULL) {
+        _debugPrint->println(str);
+    }
+}
+
+void SimpleFOCDebug::println(const char* str, float val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+        _debugPrint->println(val);
+    }
+}
+
+void SimpleFOCDebug::println(const __FlashStringHelper* str, float val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+        _debugPrint->println(val);
+    }
+}
+
+void SimpleFOCDebug::println(const char* str, int val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+        _debugPrint->println(val);
+    }
+}
+
+void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+        _debugPrint->println(val);
+    }
+}
+
+
+void SimpleFOCDebug::print(const char* str) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+    }
+}
+
+
+void SimpleFOCDebug::print(const __FlashStringHelper* str) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(str);
+    }
+}
+
+
+void SimpleFOCDebug::print(int val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(val);
+    }
+}
+
+
+void SimpleFOCDebug::print(float val) {
+    if (_debugPrint != NULL) {
+        _debugPrint->print(val);
+    }
+}
+
+
+void SimpleFOCDebug::println() {
+    if (_debugPrint != NULL) {
+        _debugPrint->println();
+    }
+}
+
+#endif

firmware/lib/Arduino-FOC/src/communication/SimpleFOCDebug.h

@@ -0,0 +1,79 @@
+
+#ifndef __SIMPLEFOCDEBUG_H__
+#define __SIMPLEFOCDEBUG_H__
+
+#include "Arduino.h"
+
+
+/**
+ * SimpleFOCDebug class
+ * 
+ * This class is used to print debug messages to a chosen output.
+ * Currently, Print instances are supported as targets, e.g. serial port.
+ * 
+ * Activate debug output globally by calling enable(), optionally passing
+ * in a Print instance. If none is provided "Serial" is used by default.
+ * 
+ * To produce debug output, use the macro SIMPLEFOC_DEBUG:
+ *   SIMPLEFOC_DEBUG("Debug message!");
+ *   SIMPLEFOC_DEBUG("a float value:", 123.456f);
+ *   SIMPLEFOC_DEBUG("an integer value: ", 123);
+ * 
+ * Keep debugging output short and simple. Some of our MCUs have limited
+ * RAM and limited serial output capabilities.
+ * 
+ * By default, the SIMPLEFOC_DEBUG macro uses the flash string helper to
+ * help preserve memory on Arduino boards.
+ * 
+ * You can also disable debug output completely. In this case all debug output 
+ * and the SimpleFOCDebug class is removed from the compiled code.
+ * Add -DSIMPLEFOC_DISABLE_DEBUG to your compiler flags to disable debug in
+ * this way.
+ * 
+ **/
+
+
+#ifndef SIMPLEFOC_DISABLE_DEBUG
+
+class SimpleFOCDebug {
+public:
+    static void enable(Print* debugPrint = &Serial);
+
+    static void println(const __FlashStringHelper* msg);
+    static void println(const char* msg);
+    static void println(const __FlashStringHelper* msg, float val);
+    static void println(const char* msg, float val);
+    static void println(const __FlashStringHelper* msg, int val);
+    static void println(const char* msg, int val);
+    static void println();
+    static void println(int val);
+    static void println(float val);
+
+    static void print(const char* msg);
+    static void print(const __FlashStringHelper* msg);
+    static void print(int val);
+    static void print(float val);
+
+protected:
+    static Print* _debugPrint;
+};
+
+
+#define SIMPLEFOC_DEBUG(msg, ...) \
+    SimpleFOCDebug::println(F(msg), ##__VA_ARGS__)
+
+
+
+
+
+#else //ifndef SIMPLEFOC_DISABLE_DEBUG
+
+
+
+#define SIMPLEFOC_DEBUG(msg, ...)
+
+
+
+#endif //ifndef SIMPLEFOC_DISABLE_DEBUG
+#endif
+

firmware/lib/Arduino-FOC/src/communication/StepDirListener.cpp

@@ -0,0 +1,40 @@
+#include "StepDirListener.h"
+
+StepDirListener::StepDirListener(int _pinStep, int _pinDir, float _counter_to_value){
+    pin_step = _pinStep;
+    pin_dir = _pinDir;
+    counter_to_value = _counter_to_value;
+}
+
+void StepDirListener::init(){
+    pinMode(pin_dir, INPUT);
+    pinMode(pin_step, INPUT_PULLUP);
+    count = 0;
+}
+
+void StepDirListener::enableInterrupt(void (*doA)()){
+    attachInterrupt(digitalPinToInterrupt(pin_step), doA, polarity);
+}
+
+void StepDirListener::attach(float* variable){
+    attached_variable = variable;
+}
+
+void StepDirListener::handle(){ 
+  // read step status
+  //bool step = digitalRead(pin_step);
+  // update counter only on rising edge 
+  //if(step && step != step_active){
+    if(digitalRead(pin_dir)) 
+        count++;
+    else 
+        count--;
+   //}
+   //step_active = step;
+   // if attached variable update it
+   if(attached_variable) *attached_variable = getValue();
+}
+// calculate the position from counter
+float StepDirListener::getValue(){
+    return (float) count * counter_to_value;
+}

firmware/lib/Arduino-FOC/src/communication/StepDirListener.h

@@ -0,0 +1,65 @@
+#ifndef STEPDIR_H
+#define STEPDIR_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+
+
+#if !defined(TARGET_RP2040) && !defined(_SAMD21_) && !defined(_SAMD51_) && !defined(_SAME51_) && !defined(ARDUINO_UNOR4_WIFI) && !defined(ARDUINO_UNOR4_MINIMA) && !defined(NRF52_SERIES) && !defined(ARDUINO_ARCH_MEGAAVR)
+#define PinStatus int
+#endif
+
+
+/**
+ * Step/Dir listenner class for easier interraction with this communication interface.
+ */
+class StepDirListener
+{
+  public:
+
+    /**
+     * Constructor for step/direction interface
+     *  @param step              - pin
+     *  @param direction         - pin
+     *  @param counter_to_value  - step counter to value
+     */
+    StepDirListener(int pinStep, int pinDir, float counter_to_value = 1);
+    /**
+     * Start listenning for step commands
+     * 
+     * @param handleStep - on step received handler
+     */
+    void enableInterrupt(void (*handleStep)());
+
+    /**
+     * Initialise dir and step commands
+     */
+    void init();
+    /**
+     * step handler
+     */
+    void handle();
+    /**
+     * Get so far received valued
+     */
+    float getValue();
+    /**
+     * Attach the value to be updated on each step receive 
+     * - no need to call getValue function
+     */
+    void attach(float* variable);
+
+    // variables
+    int pin_step; //!< step pin
+    int pin_dir; //!< direction pin
+    long count; //!< current counter value - should be set to 0 for homing
+    PinStatus polarity = RISING; //!< polarity of the step pin
+
+  private:
+    float* attached_variable = nullptr; //!< pointer to the attached variable 
+    float counter_to_value; //!< step counter to value 
+    //bool step_active = 0; //!< current step pin status (HIGH/LOW) - debouncing variable
+
+};
+
+#endif

firmware/lib/Arduino-FOC/src/communication/commands.h

@@ -0,0 +1,52 @@
+#ifndef COMMANDS_h
+#define  COMMANDS_h
+
+// see docs.simplefoc.com for in depth explanation of the commands
+
+// list of commands
+ #define CMD_C_D_PID   'D' //!< current d PID & LPF
+ #define CMD_C_Q_PID   'Q' //!< current d PID & LPF
+ #define CMD_V_PID     'V' //!< velocity PID & LPF
+ #define CMD_A_PID     'A' //!< angle PID & LPF
+ #define CMD_STATUS    'E' //!< motor status enable/disable
+ #define CMD_LIMITS    'L' //!< limits current/voltage/velocity
+ #define CMD_MOTION_TYPE  'C' //!< motion control type
+ #define CMD_TORQUE_TYPE  'T' //!< torque control type
+ #define CMD_SENSOR    'S' //!< sensor offsets
+ #define CMD_MONITOR   'M' //!< monitoring
+ #define CMD_RESIST    'R' //!< motor phase resistance
+ #define CMD_INDUCTANCE    'I' //!< motor phase inductance
+ #define CMD_KV_RATING 'K' //!< motor kv rating
+ #define CMD_PWMMOD   'W' //!< pwm modulation
+
+ // commander configuration
+ #define CMD_SCAN    '?' //!< command scaning the network - only for commander
+ #define CMD_VERBOSE '@' //!< command setting output mode - only for commander
+ #define CMD_DECIMAL '#' //!< command setting decimal places - only for commander
+
+ // subcomands
+ //pid - lpf
+ #define SCMD_PID_P     'P' //!< PID gain P
+ #define SCMD_PID_I     'I' //!< PID gain I
+ #define SCMD_PID_D     'D' //!< PID gain D
+ #define SCMD_PID_RAMP  'R' //!< PID ramp
+ #define SCMD_PID_LIM   'L' //!< PID limit
+ #define SCMD_LPF_TF    'F' //!< LPF time constant
+ // limits
+ #define SCMD_LIM_CURR  'C' //!< Limit current
+ #define SCMD_LIM_VOLT  'U' //!< Limit voltage
+ #define SCMD_LIM_VEL   'V' //!< Limit velocity
+ //sensor
+ #define SCMD_SENS_MECH_OFFSET 'M' //!< Sensor offset
+ #define SCMD_SENS_ELEC_OFFSET 'E' //!< Sensor electrical zero offset
+ // monitoring
+ #define SCMD_DOWNSAMPLE 'D' //!< Monitoring downsample value
+ #define SCMD_CLEAR      'C' //!< Clear all monitored variables
+ #define SCMD_GET        'G' //!< Get variable only one value
+ #define SCMD_SET        'S' //!< Set variables to be monitored
+
+ #define SCMD_PWMMOD_TYPE   'T'  //!<< Pwm modulation type
+ #define SCMD_PWMMOD_CENTER 'C'  //!<< Pwm modulation center flag
+
+
+#endif

firmware/lib/Arduino-FOC/src/current_sense/GenericCurrentSense.cpp

@@ -0,0 +1,163 @@
+#include "GenericCurrentSense.h"
+
+// GenericCurrentSense constructor
+GenericCurrentSense::GenericCurrentSense(PhaseCurrent_s (*readCallback)(), void (*initCallback)()){
+  // if function provided add it to the 
+  if(readCallback != nullptr) this->readCallback = readCallback;
+  if(initCallback != nullptr) this->initCallback = initCallback;
+}
+
+// Inline sensor init function
+int GenericCurrentSense::init(){
+    // configure ADC variables
+    if(initCallback != nullptr) initCallback();
+    // calibrate zero offsets
+    calibrateOffsets();
+    // set the initialized flag
+    initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
+    // return success
+    return 1;
+}
+// Function finding zero offsets of the ADC
+void GenericCurrentSense::calibrateOffsets(){
+    const int calibration_rounds = 1000;
+
+    // find adc offset = zero current voltage
+    offset_ia = 0;
+    offset_ib = 0;
+    offset_ic = 0;
+    // read the adc voltage 1000 times ( arbitrary number )
+    for (int i = 0; i < calibration_rounds; i++) {
+        PhaseCurrent_s current = readCallback();
+        offset_ia += current.a;
+        offset_ib += current.b;
+        offset_ic += current.c;
+        _delay(1);
+    }
+    // calculate the mean offsets
+    offset_ia = offset_ia / calibration_rounds;
+    offset_ib = offset_ib / calibration_rounds;
+    offset_ic = offset_ic / calibration_rounds;
+}
+
+// read all three phase currents (if possible 2 or 3)
+PhaseCurrent_s GenericCurrentSense::getPhaseCurrents(){
+    PhaseCurrent_s current = readCallback();
+    current.a = (current.a - offset_ia); // amps
+    current.b = (current.b - offset_ib); // amps
+    current.c = (current.c - offset_ic); // amps
+    return current;
+}
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int GenericCurrentSense::driverAlign(float voltage){
+    _UNUSED(voltage) ; // remove unused parameter warning
+    int exit_flag = 1;
+    if(skip_align) return exit_flag;
+
+    if (!initialized) return 0;
+
+    // // set phase A active and phases B and C down
+    // driver->setPwm(voltage, 0, 0);
+    // _delay(200);
+    // PhaseCurrent_s c = getPhaseCurrents();
+    // // read the current 100 times ( arbitrary number )
+    // for (int i = 0; i < 100; i++) {
+    //     PhaseCurrent_s c1 = getPhaseCurrents();
+    //     c.a = c.a*0.6f + 0.4f*c1.a;
+    //     c.b = c.b*0.6f + 0.4f*c1.b;
+    //     c.c = c.c*0.6f + 0.4f*c1.c;
+    //     _delay(3);
+    // }
+    // driver->setPwm(0, 0, 0);
+    // // align phase A
+    // float ab_ratio = fabs(c.a / c.b);
+    // float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
+    // if( ab_ratio > 1.5f ){ // should be ~2
+    //     gain_a *= _sign(c.a);
+    // }else if( ab_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and B
+    //     int tmp_pinA = pinA;
+    //     pinA = pinB;
+    //     pinB = tmp_pinA;
+    //     gain_a *= _sign(c.b);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else if(_isset(pinC) &&  ac_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and C
+    //     int tmp_pinA = pinA;
+    //     pinA = pinC;
+    //     pinC= tmp_pinA;
+    //     gain_a *= _sign(c.c);
+    //     exit_flag = 2;// signal that pins have been switched
+    // }else{
+    //     // error in current sense - phase either not measured or bad connection
+    //     return 0;
+    // }
+
+    // // set phase B active and phases A and C down
+    // driver->setPwm(0, voltage, 0);
+    // _delay(200);
+    // c = getPhaseCurrents();
+    // // read the current 50 times
+    // for (int i = 0; i < 100; i++) {
+    //     PhaseCurrent_s c1 = getPhaseCurrents();
+    //     c.a = c.a*0.6f + 0.4f*c1.a;
+    //     c.b = c.b*0.6f + 0.4f*c1.b;
+    //     c.c = c.c*0.6f + 0.4f*c1.c;
+    //     _delay(3);
+    // }
+    // driver->setPwm(0, 0, 0);
+    // float ba_ratio = fabs(c.b/c.a);
+    // float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
+    //  if( ba_ratio > 1.5f ){ // should be ~2
+    //     gain_b *= _sign(c.b);
+    // }else if( ba_ratio < 0.7f ){ // it should be ~0.5
+    //     // switch phase A and B
+    //     int tmp_pinB = pinB;
+    //     pinB = pinA;
+    //     pinA = tmp_pinB;
+    //     gain_b *= _sign(c.a);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
+    //     // switch phase A and C
+    //     int tmp_pinB = pinB;
+    //     pinB = pinC;
+    //     pinC = tmp_pinB;
+    //     gain_b *= _sign(c.c);
+    //     exit_flag = 2; // signal that pins have been switched
+    // }else{
+    //     // error in current sense - phase either not measured or bad connection
+    //     return 0;
+    // }
+
+    // // if phase C measured
+    // if(_isset(pinC)){
+    //     // set phase B active and phases A and C down
+    //     driver->setPwm(0, 0, voltage);
+    //     _delay(200);
+    //     c = getPhaseCurrents();
+    //     // read the adc voltage 500 times ( arbitrary number )
+    //     for (int i = 0; i < 50; i++) {
+    //         PhaseCurrent_s c1 = getPhaseCurrents();
+    //         c.c = (c.c+c1.c)/50.0f;
+    //     }
+    //     driver->setPwm(0, 0, 0);
+    //     gain_c *= _sign(c.c);
+    // }
+
+    // if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
+    // exit flag is either
+    // 0 - fail
+    // 1 - success and nothing changed
+    // 2 - success but pins reconfigured
+    // 3 - success but gains inverted
+    // 4 - success but pins reconfigured and gains inverted
+    return exit_flag;
+}

firmware/lib/Arduino-FOC/src/current_sense/GenericCurrentSense.h

@@ -0,0 +1,40 @@
+#ifndef GENERIC_CS_LIB_H
+#define GENERIC_CS_LIB_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "../common/base_classes/CurrentSense.h"
+#include "../common/lowpass_filter.h"
+#include "hardware_api.h"
+
+
+class GenericCurrentSense: public CurrentSense{
+  public:
+    /**
+      GenericCurrentSense class constructor
+    */
+    GenericCurrentSense(PhaseCurrent_s (*readCallback)() = nullptr, void (*initCallback)() = nullptr);
+
+    // CurrentSense interface implementing functions 
+    int init() override;
+    PhaseCurrent_s getPhaseCurrents() override;
+    int driverAlign(float align_voltage) override;
+
+
+    PhaseCurrent_s (*readCallback)() = nullptr; //!< function pointer to sensor reading
+    void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation
+
+  private:
+    /**
+     *  Function finding zero offsets of the ADC
+     */
+    void calibrateOffsets();
+    float offset_ia; //!< zero current A voltage value (center of the adc reading)
+    float offset_ib; //!< zero current B voltage value (center of the adc reading)
+    float offset_ic; //!< zero current C voltage value (center of the adc reading)
+
+};
+
+#endif

firmware/lib/Arduino-FOC/src/current_sense/InlineCurrentSense.cpp

@@ -0,0 +1,253 @@
+#include "InlineCurrentSense.h"
+#include "communication/SimpleFOCDebug.h"
+// InlineCurrentSensor constructor
+//  - shunt_resistor  - shunt resistor value
+//  - gain  - current-sense op-amp gain
+//  - phA   - A phase adc pin
+//  - phB   - B phase adc pin
+//  - phC   - C phase adc pin (optional)
+InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){
+    pinA = _pinA;
+    pinB = _pinB;
+    pinC = _pinC;
+
+    shunt_resistor = _shunt_resistor;
+    amp_gain  = _gain;
+    volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps
+    // gains for each phase
+    gain_a = volts_to_amps_ratio;
+    gain_b = volts_to_amps_ratio;
+    gain_c = volts_to_amps_ratio;
+};
+
+
+InlineCurrentSense::InlineCurrentSense(float _mVpA, int _pinA, int _pinB, int _pinC){
+    pinA = _pinA;
+    pinB = _pinB;
+    pinC = _pinC;
+
+    volts_to_amps_ratio = 1000.0f / _mVpA; // mV to amps
+    // gains for each phase
+    gain_a = volts_to_amps_ratio;
+    gain_b = volts_to_amps_ratio;
+    gain_c = volts_to_amps_ratio;
+};
+
+
+
+// Inline sensor init function
+int InlineCurrentSense::init(){
+    // if no linked driver its fine in this case 
+    // at least for init()
+    void* drv_params = driver ? driver->params : nullptr;
+    // configure ADC variables
+    params = _configureADCInline(drv_params,pinA,pinB,pinC);
+    // if init failed return fail
+    if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; 
+    // calibrate zero offsets
+    calibrateOffsets();
+    // set the initialized flag
+    initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
+    // return success
+    return 1;
+}
+// Function finding zero offsets of the ADC
+void InlineCurrentSense::calibrateOffsets(){
+    const int calibration_rounds = 1000;
+    
+    // find adc offset = zero current voltage
+    offset_ia = 0;
+    offset_ib = 0;
+    offset_ic = 0;
+    // read the adc voltage 1000 times ( arbitrary number )
+    for (int i = 0; i < calibration_rounds; i++) {
+        if(_isset(pinA)) offset_ia += _readADCVoltageInline(pinA, params);
+        if(_isset(pinB)) offset_ib += _readADCVoltageInline(pinB, params);
+        if(_isset(pinC)) offset_ic += _readADCVoltageInline(pinC, params);
+        _delay(1);
+    }
+    // calculate the mean offsets
+    if(_isset(pinA)) offset_ia = offset_ia / calibration_rounds;
+    if(_isset(pinB)) offset_ib = offset_ib / calibration_rounds;
+    if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
+}
+
+// read all three phase currents (if possible 2 or 3)
+PhaseCurrent_s InlineCurrentSense::getPhaseCurrents(){
+    PhaseCurrent_s current;
+    current.a = (!_isset(pinA)) ? 0 : (_readADCVoltageInline(pinA, params) - offset_ia)*gain_a;// amps
+    current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageInline(pinB, params) - offset_ib)*gain_b;// amps
+    current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageInline(pinC, params) - offset_ic)*gain_c; // amps
+    return current;
+}
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int InlineCurrentSense::driverAlign(float voltage){
+    
+    int exit_flag = 1;
+    if(skip_align) return exit_flag;
+
+    if (driver==nullptr) {
+        SIMPLEFOC_DEBUG("CUR: No driver linked!");
+        return 0;
+    }
+
+    if (!initialized) return 0;
+
+    if(_isset(pinA)){
+        // set phase A active and phases B and C down
+        driver->setPwm(voltage, 0, 0);
+        _delay(2000);
+        PhaseCurrent_s c = getPhaseCurrents();
+        // read the current 100 times ( arbitrary number )
+        for (int i = 0; i < 100; i++) {
+            PhaseCurrent_s c1 = getPhaseCurrents();
+            c.a = c.a*0.6f + 0.4f*c1.a;
+            c.b = c.b*0.6f + 0.4f*c1.b;
+            c.c = c.c*0.6f + 0.4f*c1.c;
+            _delay(3);
+        }
+        driver->setPwm(0, 0, 0);
+        // align phase A
+        float ab_ratio = c.b ? fabs(c.a / c.b) : 0;
+        float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
+        if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2
+            gain_a *= _sign(c.a);
+        }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2
+            gain_a *= _sign(c.a);
+        }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5
+            // switch phase A and B
+            int tmp_pinA = pinA;
+            pinA = pinB;
+            pinB = tmp_pinA;
+            float tmp_offsetA = offset_ia;
+            offset_ia = offset_ib;
+            offset_ib = tmp_offsetA;
+            gain_a *= _sign(c.b);
+            exit_flag = 2; // signal that pins have been switched
+        }else if(_isset(pinC) &&  ac_ratio < 0.7f ){ // should be ~0.5
+            // switch phase A and C
+            int tmp_pinA = pinA;
+            pinA = pinC;
+            pinC= tmp_pinA;
+            float tmp_offsetA = offset_ia;
+            offset_ia = offset_ic;
+            offset_ic = tmp_offsetA;
+            gain_a *= _sign(c.c);
+            exit_flag = 2;// signal that pins have been switched
+        }else{
+            // error in current sense - phase either not measured or bad connection
+            return 0;
+        }
+    }
+
+    if(_isset(pinB)){
+        // set phase B active and phases A and C down
+        driver->setPwm(0, voltage, 0);
+        _delay(200);
+        PhaseCurrent_s c = getPhaseCurrents();
+        // read the current 50 times
+        for (int i = 0; i < 100; i++) {
+            PhaseCurrent_s c1 = getPhaseCurrents();
+            c.a = c.a*0.6 + 0.4f*c1.a;
+            c.b = c.b*0.6 + 0.4f*c1.b;
+            c.c = c.c*0.6 + 0.4f*c1.c;
+            _delay(3);
+        }
+        driver->setPwm(0, 0, 0);
+        float ba_ratio = c.a ? fabs(c.b / c.a) : 0;
+        float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
+        if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2
+            gain_b *= _sign(c.b);
+        }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2
+            gain_b *= _sign(c.b);
+        }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5
+            // switch phase A and B
+            int tmp_pinB = pinB;
+            pinB = pinA;
+            pinA = tmp_pinB;
+            float tmp_offsetB = offset_ib;
+            offset_ib = offset_ia;
+            offset_ia = tmp_offsetB;
+            gain_b *= _sign(c.a);
+            exit_flag = 2; // signal that pins have been switched
+        }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
+            // switch phase A and C
+            int tmp_pinB = pinB;
+            pinB = pinC;
+            pinC = tmp_pinB;
+            float tmp_offsetB = offset_ib;
+            offset_ib = offset_ic;
+            offset_ic = tmp_offsetB;
+            gain_b *= _sign(c.c);
+            exit_flag = 2; // signal that pins have been switched
+        }else{
+            // error in current sense - phase either not measured or bad connection
+            return 0;
+        }   
+    }
+
+    // if phase C measured
+    if(_isset(pinC)){
+        // set phase C active and phases A and B down
+        driver->setPwm(0, 0, voltage);
+        _delay(200);
+        PhaseCurrent_s c = getPhaseCurrents();
+        // read the adc voltage 500 times ( arbitrary number )
+        for (int i = 0; i < 100; i++) {
+            PhaseCurrent_s c1 = getPhaseCurrents();
+            c.a = c.a*0.6 + 0.4f*c1.a;
+            c.b = c.b*0.6 + 0.4f*c1.b;
+            c.c = c.c*0.6 + 0.4f*c1.c;
+            _delay(3);
+        }
+        driver->setPwm(0, 0, 0);
+        float ca_ratio = c.a ? fabs(c.c / c.a) : 0;
+        float cb_ratio = c.b ? fabs(c.c / c.b) : 0;
+        if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2
+            gain_c *= _sign(c.c);
+        }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2
+            gain_c *= _sign(c.c);
+        }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5
+            // switch phase A and C
+            int tmp_pinC = pinC;
+            pinC = pinA;
+            pinA = tmp_pinC;
+            float tmp_offsetC = offset_ic;
+            offset_ic = offset_ia;
+            offset_ia = tmp_offsetC;
+            gain_c *= _sign(c.a);
+            exit_flag = 2; // signal that pins have been switched
+        }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5
+            // switch phase B and C
+            int tmp_pinC = pinC;
+            pinC = pinB;
+            pinB = tmp_pinC;
+            float tmp_offsetC = offset_ic;
+            offset_ic = offset_ib;
+            offset_ib = tmp_offsetC;
+            gain_c *= _sign(c.b);
+            exit_flag = 2; // signal that pins have been switched
+        }else{
+            // error in current sense - phase either not measured or bad connection
+            return 0;
+        }   
+    }
+
+    if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
+    // exit flag is either
+    // 0 - fail
+    // 1 - success and nothing changed
+    // 2 - success but pins reconfigured
+    // 3 - success but gains inverted
+    // 4 - success but pins reconfigured and gains inverted
+
+    return exit_flag;
+}

firmware/lib/Arduino-FOC/src/current_sense/InlineCurrentSense.h

@@ -0,0 +1,73 @@
+#ifndef INLINE_CS_LIB_H
+#define INLINE_CS_LIB_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "../common/base_classes/CurrentSense.h"
+#include "../common/lowpass_filter.h"
+#include "hardware_api.h"
+
+
+class InlineCurrentSense: public CurrentSense{
+  public:
+    /**
+      InlineCurrentSense class constructor
+      @param shunt_resistor shunt resistor value
+      @param gain current-sense op-amp gain
+      @param phA A phase adc pin
+      @param phB B phase adc pin
+      @param phC C phase adc pin (optional)
+    */
+    InlineCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = NOT_SET);
+    /**
+      InlineCurrentSense class constructor
+      @param mVpA mV per Amp ratio
+      @param phA A phase adc pin
+      @param phB B phase adc pin
+      @param phC C phase adc pin (optional)
+    */
+    InlineCurrentSense(float mVpA, int pinA, int pinB, int pinC = NOT_SET);
+
+    // CurrentSense interface implementing functions 
+    int init() override;
+    PhaseCurrent_s getPhaseCurrents() override;
+    int driverAlign(float align_voltage) override;
+
+    // ADC measuremnet gain for each phase
+    // support for different gains for different phases of more commonly - inverted phase currents
+    // this should be automated later
+    float gain_a; //!< phase A gain
+    float gain_b; //!< phase B gain
+    float gain_c; //!< phase C gain
+
+    // // per phase low pass fileters
+    // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current A low pass filter
+    // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current B low pass filter
+    // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current C low pass filter
+
+    float offset_ia; //!< zero current A voltage value (center of the adc reading)
+    float offset_ib; //!< zero current B voltage value (center of the adc reading)
+    float offset_ic; //!< zero current C voltage value (center of the adc reading)
+    
+  private:
+  
+    // hardware variables
+  	int pinA; //!< pin A analog pin for current measurement
+  	int pinB; //!< pin B analog pin for current measurement
+  	int pinC; //!< pin C analog pin for current measurement
+
+    // gain variables
+    float shunt_resistor; //!< Shunt resistor value
+    float amp_gain; //!< amp gain value
+    float volts_to_amps_ratio; //!< Volts to amps ratio
+    
+    /**
+     *  Function finding zero offsets of the ADC
+     */
+    void calibrateOffsets();
+
+};
+
+#endif

firmware/lib/Arduino-FOC/src/current_sense/LowsideCurrentSense.cpp

@@ -0,0 +1,254 @@
+#include "LowsideCurrentSense.h"
+#include "communication/SimpleFOCDebug.h"
+// LowsideCurrentSensor constructor
+//  - shunt_resistor  - shunt resistor value
+//  - gain  - current-sense op-amp gain
+//  - phA   - A phase adc pin
+//  - phB   - B phase adc pin
+//  - phC   - C phase adc pin (optional)
+LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int _pinA, int _pinB, int _pinC){
+    pinA = _pinA;
+    pinB = _pinB;
+    pinC = _pinC;
+
+    shunt_resistor = _shunt_resistor;
+    amp_gain  = _gain;
+    volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps
+    // gains for each phase
+    gain_a = volts_to_amps_ratio;
+    gain_b = volts_to_amps_ratio;
+    gain_c = volts_to_amps_ratio;
+}
+
+
+LowsideCurrentSense::LowsideCurrentSense(float _mVpA, int _pinA, int _pinB, int _pinC){
+    pinA = _pinA;
+    pinB = _pinB;
+    pinC = _pinC;
+
+    volts_to_amps_ratio = 1000.0f / _mVpA; // mV to amps
+    // gains for each phase
+    gain_a = volts_to_amps_ratio;
+    gain_b = volts_to_amps_ratio;
+    gain_c = volts_to_amps_ratio;
+}   
+
+
+// Lowside sensor init function
+int LowsideCurrentSense::init(){
+
+    if (driver==nullptr) {
+        SIMPLEFOC_DEBUG("CUR: Driver not linked!");
+        return 0;
+    }
+
+    // configure ADC variables
+    params = _configureADCLowSide(driver->params,pinA,pinB,pinC);
+    // if init failed return fail
+    if (params == SIMPLEFOC_CURRENT_SENSE_INIT_FAILED) return 0; 
+    // sync the driver
+    _driverSyncLowSide(driver->params, params);
+    // calibrate zero offsets
+    calibrateOffsets();
+    // set the initialized flag
+    initialized = (params!=SIMPLEFOC_CURRENT_SENSE_INIT_FAILED);
+    // return success
+    return 1;
+}
+// Function finding zero offsets of the ADC
+void LowsideCurrentSense::calibrateOffsets(){    
+    const int calibration_rounds = 2000;
+
+    // find adc offset = zero current voltage
+    offset_ia = 0;
+    offset_ib = 0;
+    offset_ic = 0;
+    // read the adc voltage 1000 times ( arbitrary number )
+    for (int i = 0; i < calibration_rounds; i++) {
+        _startADC3PinConversionLowSide();
+        if(_isset(pinA)) offset_ia += (_readADCVoltageLowSide(pinA, params));
+        if(_isset(pinB)) offset_ib += (_readADCVoltageLowSide(pinB, params));
+        if(_isset(pinC)) offset_ic += (_readADCVoltageLowSide(pinC, params));
+        _delay(1);
+    }
+    // calculate the mean offsets
+    if(_isset(pinA)) offset_ia = offset_ia / calibration_rounds;
+    if(_isset(pinB)) offset_ib = offset_ib / calibration_rounds;
+    if(_isset(pinC)) offset_ic = offset_ic / calibration_rounds;
+}
+
+// read all three phase currents (if possible 2 or 3)
+PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){
+    PhaseCurrent_s current;
+    _startADC3PinConversionLowSide();
+    current.a = (!_isset(pinA)) ? 0 : (_readADCVoltageLowSide(pinA, params) - offset_ia)*gain_a;// amps
+    current.b = (!_isset(pinB)) ? 0 : (_readADCVoltageLowSide(pinB, params) - offset_ib)*gain_b;// amps
+    current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC, params) - offset_ic)*gain_c; // amps
+    return current;
+}
+
+// Function aligning the current sense with motor driver
+// if all pins are connected well none of this is really necessary! - can be avoided
+// returns flag
+// 0 - fail
+// 1 - success and nothing changed
+// 2 - success but pins reconfigured
+// 3 - success but gains inverted
+// 4 - success but pins reconfigured and gains inverted
+int LowsideCurrentSense::driverAlign(float voltage){
+        
+    int exit_flag = 1;
+    if(skip_align) return exit_flag;
+
+    if (!initialized) return 0;
+
+    if(_isset(pinA)){
+        // set phase A active and phases B and C down
+        driver->setPwm(voltage, 0, 0);
+        _delay(2000);
+        PhaseCurrent_s c = getPhaseCurrents();
+        // read the current 100 times ( arbitrary number )
+        for (int i = 0; i < 100; i++) {
+            PhaseCurrent_s c1 = getPhaseCurrents();
+            c.a = c.a*0.6f + 0.4f*c1.a;
+            c.b = c.b*0.6f + 0.4f*c1.b;
+            c.c = c.c*0.6f + 0.4f*c1.c;
+            _delay(3);
+        }
+        driver->setPwm(0, 0, 0);
+        // align phase A
+        float ab_ratio = c.b ? fabs(c.a / c.b) : 0;
+        float ac_ratio = c.c ? fabs(c.a / c.c) : 0;
+        if(_isset(pinB) && ab_ratio > 1.5f ){ // should be ~2
+            gain_a *= _sign(c.a);
+        }else if(_isset(pinC) && ac_ratio > 1.5f ){ // should be ~2
+            gain_a *= _sign(c.a);
+        }else if(_isset(pinB) && ab_ratio < 0.7f ){ // should be ~0.5
+            // switch phase A and B
+            int tmp_pinA = pinA;
+            pinA = pinB;
+            pinB = tmp_pinA;
+            float tmp_offsetA = offset_ia;
+            offset_ia = offset_ib;
+            offset_ib = tmp_offsetA;
+            gain_a *= _sign(c.b);
+            exit_flag = 2; // signal that pins have been switched
+        }else if(_isset(pinC) &&  ac_ratio < 0.7f ){ // should be ~0.5
+            // switch phase A and C
+            int tmp_pinA = pinA;
+            pinA = pinC;
+            pinC= tmp_pinA;
+            float tmp_offsetA = offset_ia;
+            offset_ia = offset_ic;
+            offset_ic = tmp_offsetA;
+            gain_a *= _sign(c.c);
+            exit_flag = 2;// signal that pins have been switched
+        }else{
+            // error in current sense - phase either not measured or bad connection
+            return 0;
+        }
+    }
+
+    if(_isset(pinB)){
+        // set phase B active and phases A and C down
+        driver->setPwm(0, voltage, 0);
+        _delay(200);
+        PhaseCurrent_s c = getPhaseCurrents();
+        // read the current 50 times
+        for (int i = 0; i < 100; i++) {
+            PhaseCurrent_s c1 = getPhaseCurrents();
+            c.a = c.a*0.6 + 0.4f*c1.a;
+            c.b = c.b*0.6 + 0.4f*c1.b;
+            c.c = c.c*0.6 + 0.4f*c1.c;
+            _delay(3);
+        }
+        driver->setPwm(0, 0, 0);
+        float ba_ratio = c.a ? fabs(c.b / c.a) : 0;
+        float bc_ratio = c.c ? fabs(c.b / c.c) : 0;
+        if(_isset(pinA) && ba_ratio > 1.5f ){ // should be ~2
+            gain_b *= _sign(c.b);
+        }else if(_isset(pinC) && bc_ratio > 1.5f ){ // should be ~2
+            gain_b *= _sign(c.b);
+        }else if(_isset(pinA) && ba_ratio < 0.7f ){ // it should be ~0.5
+            // switch phase A and B
+            int tmp_pinB = pinB;
+            pinB = pinA;
+            pinA = tmp_pinB;
+            float tmp_offsetB = offset_ib;
+            offset_ib = offset_ia;
+            offset_ia = tmp_offsetB;
+            gain_b *= _sign(c.a);
+            exit_flag = 2; // signal that pins have been switched
+        }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5
+            // switch phase A and C
+            int tmp_pinB = pinB;
+            pinB = pinC;
+            pinC = tmp_pinB;
+            float tmp_offsetB = offset_ib;
+            offset_ib = offset_ic;
+            offset_ic = tmp_offsetB;
+            gain_b *= _sign(c.c);
+            exit_flag = 2; // signal that pins have been switched
+        }else{
+            // error in current sense - phase either not measured or bad connection
+            return 0;
+        }   
+    }
+
+    // if phase C measured
+    if(_isset(pinC)){
+        // set phase C active and phases A and B down
+        driver->setPwm(0, 0, voltage);
+        _delay(200);
+        PhaseCurrent_s c = getPhaseCurrents();
+        // read the adc voltage 500 times ( arbitrary number )
+        for (int i = 0; i < 100; i++) {
+            PhaseCurrent_s c1 = getPhaseCurrents();
+            c.a = c.a*0.6 + 0.4f*c1.a;
+            c.b = c.b*0.6 + 0.4f*c1.b;
+            c.c = c.c*0.6 + 0.4f*c1.c;
+            _delay(3);
+        }
+        driver->setPwm(0, 0, 0);
+        float ca_ratio = c.a ? fabs(c.c / c.a) : 0;
+        float cb_ratio = c.b ? fabs(c.c / c.b) : 0;
+        if(_isset(pinA) && ca_ratio > 1.5f ){ // should be ~2
+            gain_c *= _sign(c.c);
+        }else if(_isset(pinB) && cb_ratio > 1.5f ){ // should be ~2
+            gain_c *= _sign(c.c);
+        }else if(_isset(pinA) && ca_ratio < 0.7f ){ // it should be ~0.5
+            // switch phase A and C
+            int tmp_pinC = pinC;
+            pinC = pinA;
+            pinA = tmp_pinC;
+            float tmp_offsetC = offset_ic;
+            offset_ic = offset_ia;
+            offset_ia = tmp_offsetC;
+            gain_c *= _sign(c.a);
+            exit_flag = 2; // signal that pins have been switched
+        }else if(_isset(pinB) && cb_ratio < 0.7f ){ // should be ~0.5
+            // switch phase B and C
+            int tmp_pinC = pinC;
+            pinC = pinB;
+            pinB = tmp_pinC;
+            float tmp_offsetC = offset_ic;
+            offset_ic = offset_ib;
+            offset_ib = tmp_offsetC;
+            gain_c *= _sign(c.b);
+            exit_flag = 2; // signal that pins have been switched
+        }else{
+            // error in current sense - phase either not measured or bad connection
+            return 0;
+        }   
+    }
+
+    if(gain_a < 0 || gain_b < 0 || gain_c < 0) exit_flag +=2;
+    // exit flag is either
+    // 0 - fail
+    // 1 - success and nothing changed
+    // 2 - success but pins reconfigured
+    // 3 - success but gains inverted
+    // 4 - success but pins reconfigured and gains inverted
+
+    return exit_flag;
+}

firmware/lib/Arduino-FOC/src/current_sense/LowsideCurrentSense.h

@@ -0,0 +1,73 @@
+#ifndef LOWSIDE_CS_LIB_H
+#define LOWSIDE_CS_LIB_H
+
+#include "Arduino.h"
+#include "../common/foc_utils.h"
+#include "../common/time_utils.h"
+#include "../common/defaults.h"
+#include "../common/base_classes/CurrentSense.h"
+#include "../common/base_classes/FOCMotor.h"
+#include "../common/lowpass_filter.h"
+#include "hardware_api.h"
+
+
+class LowsideCurrentSense: public CurrentSense{
+  public:
+    /**
+      LowsideCurrentSense class constructor
+      @param shunt_resistor shunt resistor value
+      @param gain current-sense op-amp gain
+      @param phA A phase adc pin
+      @param phB B phase adc pin
+      @param phC C phase adc pin (optional)
+    */
+    LowsideCurrentSense(float shunt_resistor, float gain, int pinA, int pinB, int pinC = _NC);
+    /**
+      LowsideCurrentSense class constructor
+      @param mVpA mV per Amp ratio
+      @param phA A phase adc pin
+      @param phB B phase adc pin
+      @param phC C phase adc pin (optional)
+    */
+    LowsideCurrentSense(float mVpA, int pinA, int pinB, int pinC = _NC);
+
+    // CurrentSense interface implementing functions
+    int init() override;
+    PhaseCurrent_s getPhaseCurrents() override;
+    int driverAlign(float align_voltage) override;
+
+    // ADC measuremnet gain for each phase
+    // support for different gains for different phases of more commonly - inverted phase currents
+    // this should be automated later
+    float gain_a; //!< phase A gain
+    float gain_b; //!< phase B gain
+    float gain_c; //!< phase C gain
+
+    // // per phase low pass fileters
+    // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current A low pass filter
+    // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current B low pass filter
+    // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!<  current C low pass filter
+
+    float offset_ia; //!< zero current A voltage value (center of the adc reading)
+    float offset_ib; //!< zero current B voltage value (center of the adc reading)
+    float offset_ic; //!< zero current C voltage value (center of the adc reading)
+  private:
+
+    // hardware variables
+  	int pinA; //!< pin A analog pin for current measurement
+  	int pinB; //!< pin B analog pin for current measurement
+  	int pinC; //!< pin C analog pin for current measurement
+
+    // gain variables
+    float shunt_resistor; //!< Shunt resistor value
+    float amp_gain; //!< amp gain value
+    float volts_to_amps_ratio; //!< Volts to amps ratio
+
+    /**
+     *  Function finding zero offsets of the ADC
+     */
+    void calibrateOffsets();
+
+};
+
+#endif

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

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/atmega_mcu.cpp

@@ -0,0 +1,40 @@
+#include "../hardware_api.h"
+
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) 
+
+#define _ADC_VOLTAGE 5.0f
+#define _ADC_RESOLUTION 1024.0f
+
+#ifndef cbi
+  #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+#endif
+#ifndef sbi
+  #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
+#endif
+
+// function reading an ADC value and returning the read voltage
+void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){
+  _UNUSED(driver_params);
+
+  if( _isset(pinA) ) pinMode(pinA, INPUT);
+  if( _isset(pinB) ) pinMode(pinB, INPUT);
+  if( _isset(pinC) ) pinMode(pinC, INPUT);
+
+  
+  GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
+    .pins = { pinA, pinB, pinC },
+    .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION)
+  };
+
+  // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default)  
+  // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz 
+  // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample 
+  cbi(ADCSRA, ADPS2);
+  sbi(ADCSRA, ADPS1);
+  sbi(ADCSRA, ADPS0);
+
+  return params;
+}
+
+
+#endif

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

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/esp32/esp32_adc_driver.cpp

@@ -0,0 +1,259 @@
+#include "esp32_adc_driver.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && !defined(CONFIG_IDF_TARGET_ESP32S2) && !defined(CONFIG_IDF_TARGET_ESP32S3)
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "rom/ets_sys.h"
+#include "esp_attr.h"
+#include "esp_intr.h"
+#include "soc/rtc_io_reg.h"
+#include "soc/rtc_cntl_reg.h"
+#include "soc/sens_reg.h"
+
+static uint8_t __analogAttenuation = 3;//11db
+static uint8_t __analogWidth = 3;//12 bits
+static uint8_t __analogCycles = 8;
+static uint8_t __analogSamples = 0;//1 sample
+static uint8_t __analogClockDiv = 1;
+
+// Width of returned answer ()
+static uint8_t __analogReturnedWidth = 12;
+
+void __analogSetWidth(uint8_t bits){
+    if(bits < 9){
+        bits = 9;
+    } else if(bits > 12){
+        bits = 12;
+    }
+    __analogReturnedWidth = bits;
+    __analogWidth = bits - 9;
+    SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S);
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S);
+
+    SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S);
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S);
+}
+
+void __analogSetCycles(uint8_t cycles){
+    __analogCycles = cycles;
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S);
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S);
+}
+
+void __analogSetSamples(uint8_t samples){
+    if(!samples){
+        return;
+    }
+    __analogSamples = samples - 1;
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S);
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S);
+}
+
+void __analogSetClockDiv(uint8_t clockDiv){
+    if(!clockDiv){
+        return;
+    }
+    __analogClockDiv = clockDiv;
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S);
+    SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S);
+}
+
+void __analogSetAttenuation(uint8_t attenuation)
+{
+    __analogAttenuation = attenuation & 3;
+    uint32_t att_data = 0;
+    int i = 10;
+    while(i--){
+        att_data |= __analogAttenuation << (i * 2);
+    }
+    WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels
+    WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data);
+}
+
+void IRAM_ATTR __analogInit(){
+    static bool initialized = false;
+    if(initialized){
+        return;
+    }
+
+    __analogSetAttenuation(__analogAttenuation);
+    __analogSetCycles(__analogCycles);
+    __analogSetSamples(__analogSamples + 1);//in samples
+    __analogSetClockDiv(__analogClockDiv);
+    __analogSetWidth(__analogWidth + 9);//in bits
+
+    SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV);
+    SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV);
+
+    SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW
+    SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW
+    SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW
+    SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW
+
+    CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM
+    SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0
+
+    CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_CTRL_REG, 0xfff << SENS_AMP_RST_FB_FSM_S);  //clear FSM
+    SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S);
+    SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S);
+    SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S);
+    while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm==
+
+    initialized = true;
+}
+
+void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation)
+{
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0 || attenuation > 3){
+        return ;
+    }
+    __analogInit();
+    if(channel > 7){
+        SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2));
+    } else {
+        SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2));
+    }
+}
+
+bool IRAM_ATTR __adcAttachPin(uint8_t pin){
+
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return false;//not adc pin
+    }
+
+    int8_t pad = digitalPinToTouchChannel(pin);
+    if(pad >= 0){
+        uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG);
+        if(touch & (1 << pad)){
+            touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S))
+                    | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S))
+                    | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S)));
+            WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch);
+        }
+    } else if(pin == 25){
+        CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1
+    } else if(pin == 26){
+        CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2
+    }
+
+    pinMode(pin, ANALOG);
+
+    __analogInit();
+    return true;
+}
+
+bool IRAM_ATTR __adcStart(uint8_t pin){
+
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return false;//not adc pin
+    }
+
+    if(channel > 9){
+        channel -= 10;
+        CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
+        SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
+    } else {
+        CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S);
+        SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
+    }
+    return true;
+}
+
+bool IRAM_ATTR __adcBusy(uint8_t pin){
+
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return false;//not adc pin
+    }
+
+    if(channel > 7){
+        return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0);
+    }
+    return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0);
+}
+
+uint16_t IRAM_ATTR __adcEnd(uint8_t pin)
+{
+
+    uint16_t value = 0;
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return 0;//not adc pin
+    }
+    if(channel > 7){
+        while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion
+        value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S);
+    } else {
+        while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion
+        value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S);
+    }
+
+    // Shift result if necessary
+    uint8_t from = __analogWidth + 9;
+    if (from == __analogReturnedWidth) {
+        return value;
+    }
+    if (from > __analogReturnedWidth) {
+        return value >> (from - __analogReturnedWidth);
+    }
+    return value << (__analogReturnedWidth - from);
+}
+
+void __analogReadResolution(uint8_t bits)
+{
+    if(!bits || bits > 16){
+        return;
+    }
+    __analogSetWidth(bits);         // hadware from 9 to 12
+    __analogReturnedWidth = bits;   // software from 1 to 16
+}
+
+uint16_t IRAM_ATTR adcRead(uint8_t pin)
+{
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return false;//not adc pin
+    }
+
+    __analogInit();
+
+    if(channel > 9){
+        channel -= 10;
+        CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
+        SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M);
+    } else {
+        CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S);
+        SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M);
+    }
+
+    uint16_t value = 0;
+
+    if(channel > 7){
+        while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion
+        value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S);
+    } else {
+        while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion
+        value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S);
+    }
+
+    // Shift result if necessary
+    uint8_t from = __analogWidth + 9;
+    if (from == __analogReturnedWidth) {
+        return value;
+    }
+    if (from > __analogReturnedWidth) {
+        return value >> (from - __analogReturnedWidth);
+    }
+    return value << (__analogReturnedWidth - from);
+}
+
+
+#endif

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/esp32/esp32_adc_driver.h

@@ -0,0 +1,88 @@
+
+
+#ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ 
+#define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_
+
+#include "Arduino.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) 
+/*
+ * Get ADC value for pin
+ * */
+uint16_t adcRead(uint8_t pin);
+
+/*
+ * Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096).
+ * If between 9 and 12, it will equal the set hardware resolution, else value will be shifted.
+ * Range is 1 - 16
+ *
+ * Note: compatibility with Arduino SAM
+ */
+void __analogReadResolution(uint8_t bits);
+
+/*
+ * Sets the sample bits and read resolution
+ * Default is 12bit (0 - 4095)
+ * Range is 9 - 12
+ * */
+void __analogSetWidth(uint8_t bits);
+
+/*
+ * Set number of cycles per sample
+ * Default is 8 and seems to do well
+ * Range is 1 - 255
+ * */
+void __analogSetCycles(uint8_t cycles);
+
+/*
+ * Set number of samples in the range.
+ * Default is 1
+ * Range is 1 - 255
+ * This setting splits the range into
+ * "samples" pieces, which could look
+ * like the sensitivity has been multiplied
+ * that many times
+ * */
+void __analogSetSamples(uint8_t samples);
+
+/*
+ * Set the divider for the ADC clock.
+ * Default is 1
+ * Range is 1 - 255
+ * */
+void __analogSetClockDiv(uint8_t clockDiv);
+
+/*
+ * Set the attenuation for all channels
+ * Default is 11db
+ * */
+void __analogSetAttenuation(uint8_t attenuation);
+
+/*
+ * Set the attenuation for particular pin
+ * Default is 11db
+ * */
+void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation);
+
+/*
+ * Attach pin to ADC (will also clear any other analog mode that could be on)
+ * */
+bool __adcAttachPin(uint8_t pin);
+
+/*
+ * Start ADC conversion on attached pin's bus
+ * */
+bool __adcStart(uint8_t pin);
+
+/*
+ * Check if conversion on the pin's ADC bus is currently running
+ * */
+bool __adcBusy(uint8_t pin);
+
+/*
+ * Get the result of the conversion (will wait if it have not finished)
+ * */
+uint16_t __adcEnd(uint8_t pin);
+
+#endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */
+#endif /* ESP32 */

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

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

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

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/esp32/esp32s_adc_driver.cpp

@@ -0,0 +1,258 @@
+#include "esp32_adc_driver.h"
+
+#if defined(ESP_H) && defined(ARDUINO_ARCH_ESP32) && defined(SOC_MCPWM_SUPPORTED) && (defined(CONFIG_IDF_TARGET_ESP32S2) || defined(CONFIG_IDF_TARGET_ESP32S3))
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "rom/ets_sys.h"
+#include "esp_attr.h"
+//#include "esp_intr.h" // deprecated
+#include "esp_intr_alloc.h"
+#include "soc/rtc_io_reg.h"
+#include "soc/rtc_cntl_reg.h"
+#include "soc/sens_reg.h"
+
+static uint8_t __analogAttenuation = 3;//11db
+static uint8_t __analogWidth = 3;//12 bits
+static uint8_t __analogCycles = 8;
+static uint8_t __analogSamples = 0;//1 sample
+static uint8_t __analogClockDiv = 1;
+
+// Width of returned answer ()
+static uint8_t __analogReturnedWidth = 12;
+
+void __analogSetWidth(uint8_t bits){
+    if(bits < 9){
+        bits = 9;
+    } else if(bits > 12){
+        bits = 12;
+    }
+    __analogReturnedWidth = bits;
+    __analogWidth = bits - 9;
+    // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S);
+    // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S);
+
+    // SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S);
+    // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S);
+}
+
+void __analogSetCycles(uint8_t cycles){
+    __analogCycles = cycles;
+    // SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S);
+    // SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S);
+}
+
+void __analogSetSamples(uint8_t samples){
+    if(!samples){
+        return;
+    }
+    __analogSamples = samples - 1;
+    SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S);
+    SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S);
+}
+
+void __analogSetClockDiv(uint8_t clockDiv){
+    if(!clockDiv){
+        return;
+    }
+    __analogClockDiv = clockDiv;
+    SET_PERI_REG_BITS(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S);
+    SET_PERI_REG_BITS(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S);
+}
+
+void __analogSetAttenuation(uint8_t attenuation)
+{
+    __analogAttenuation = attenuation & 3;
+    uint32_t att_data = 0;
+    int i = 10;
+    while(i--){
+        att_data |= __analogAttenuation << (i * 2);
+    }
+    WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels
+    WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data);
+}
+
+void IRAM_ATTR __analogInit(){
+    static bool initialized = false;
+    if(initialized){
+        return;
+    }
+
+    __analogSetAttenuation(__analogAttenuation);
+    __analogSetCycles(__analogCycles);
+    __analogSetSamples(__analogSamples + 1);//in samples
+    __analogSetClockDiv(__analogClockDiv);
+    __analogSetWidth(__analogWidth + 9);//in bits
+
+    SET_PERI_REG_MASK(SENS_SAR_READER1_CTRL_REG, SENS_SAR1_DATA_INV);
+    SET_PERI_REG_MASK(SENS_SAR_READER2_CTRL_REG, SENS_SAR2_DATA_INV);
+
+    SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW
+    SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW
+    SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW
+    SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW
+
+    CLEAR_PERI_REG_MASK(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM
+    SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0
+
+    CLEAR_PERI_REG_MASK(SENS_SAR_AMP_CTRL3_REG, 0xfff << SENS_AMP_RST_FB_FSM_S);  //clear FSM
+    SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S);
+    SET_PERI_REG_BITS(SENS_SAR_AMP_CTRL1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S);
+    SET_PERI_REG_BITS(SENS_SAR_POWER_XPD_SAR_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S);
+    while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_SARADC_MEAS_STATUS_S) != 0); //wait det_fsm==
+
+    initialized = true;
+}
+
+void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation)
+{
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0 || attenuation > 3){
+        return ;
+    }
+    __analogInit();
+    if(channel > 7){
+        SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2));
+    } else {
+        SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2));
+    }
+}
+
+bool IRAM_ATTR __adcAttachPin(uint8_t pin){
+
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return false;//not adc pin
+    }
+
+    int8_t pad = digitalPinToTouchChannel(pin);
+    if(pad >= 0){
+        uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_CONF_REG);
+        if(touch & (1 << pad)){
+            touch &= ~((1 << (pad + SENS_TOUCH_OUTEN_S)));
+            WRITE_PERI_REG(SENS_SAR_TOUCH_CONF_REG, touch);
+        }
+    } else if(pin == 25){
+        CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1
+    } else if(pin == 26){
+        CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2
+    }
+
+    pinMode(pin, ANALOG);
+
+    __analogInit();
+    return true;
+}
+
+bool IRAM_ATTR __adcStart(uint8_t pin){
+
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return false;//not adc pin
+    }
+
+    if(channel > 9){
+        channel -= 10;
+        CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
+        SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M);
+    } else {
+        CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S);
+        SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M);
+    }
+    return true;
+}
+
+bool IRAM_ATTR __adcBusy(uint8_t pin){
+
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return false;//not adc pin
+    }
+
+    if(channel > 7){
+        return (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0);
+    }
+    return (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0);
+}
+
+uint16_t IRAM_ATTR __adcEnd(uint8_t pin)
+{
+
+    uint16_t value = 0;
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return 0;//not adc pin
+    }
+    if(channel > 7){
+        while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion
+        value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S);
+    } else {
+        while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion
+        value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S);
+    }
+
+    // Shift result if necessary
+    uint8_t from = __analogWidth + 9;
+    if (from == __analogReturnedWidth) {
+        return value;
+    }
+    if (from > __analogReturnedWidth) {
+        return value >> (from - __analogReturnedWidth);
+    }
+    return value << (__analogReturnedWidth - from);
+}
+
+void __analogReadResolution(uint8_t bits)
+{
+    if(!bits || bits > 16){
+        return;
+    }
+    __analogSetWidth(bits);         // hadware from 9 to 12
+    __analogReturnedWidth = bits;   // software from 1 to 16
+}
+
+uint16_t IRAM_ATTR adcRead(uint8_t pin)
+{
+    int8_t channel = digitalPinToAnalogChannel(pin);
+    if(channel < 0){
+        return false;//not adc pin
+    }
+
+    __analogInit();
+
+    if(channel > 9){
+        channel -= 10;
+        CLEAR_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS2_CTRL2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S);
+        SET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_START_SAR_M);
+    } else {
+        CLEAR_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M);
+        SET_PERI_REG_BITS(SENS_SAR_MEAS1_CTRL2_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S);
+        SET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_START_SAR_M);
+    }
+
+    uint16_t value = 0;
+
+    if(channel > 7){
+        while (GET_PERI_REG_MASK(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion
+        value = GET_PERI_REG_BITS2(SENS_SAR_MEAS2_CTRL2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S);
+    } else {
+        while (GET_PERI_REG_MASK(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion
+        value = GET_PERI_REG_BITS2(SENS_SAR_MEAS1_CTRL2_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S);
+    }
+
+    // Shift result if necessary
+    uint8_t from = __analogWidth + 9;
+    if (from == __analogReturnedWidth) {
+        return value;
+    }
+    if (from > __analogReturnedWidth) {
+        return value >> (from - __analogReturnedWidth);
+    }
+    return value << (__analogReturnedWidth - from);
+}
+
+
+#endif

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

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

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

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

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

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

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

@@ -0,0 +1,318 @@
+#ifdef _SAMD21_
+
+#include "samd21_mcu.h"
+#include "../../hardware_api.h"
+
+
+static bool freeRunning = false;
+static int _pinA, _pinB, _pinC;
+static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode
+static SAMDCurrentSenseADCDMA instance;
+
+// function configuring low-side current sensing 
+void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC)
+{
+  _UNUSED(driver_params);
+
+  _pinA = pinA;
+  _pinB = pinB; 
+  _pinC = pinC;
+  freeRunning = true;
+  instance.init(pinA, pinB, pinC);
+
+  GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
+    .pins = { pinA, pinB, pinC }
+  };
+
+  return params;
+}
+void _startADC3PinConversionLowSide()
+{
+  instance.startADCScan();
+}
+/**
+ *  function reading an ADC value and returning the read voltage
+ * 
+ * @param pinA - the arduino pin to be read (it has to be ADC pin)
+ */
+float _readADCVoltageLowSide(const int pinA, const void* cs_params)
+{
+  _UNUSED(cs_params);
+
+  instance.readResults(a, b, c);
+  
+  if(pinA == _pinA)
+    return instance.toVolts(a);
+  if(pinA == _pinB)
+    return instance.toVolts(b);
+  if(pinA == _pinC)
+    return instance.toVolts(c);
+
+  return NAN;
+}
+
+/**
+ *  function syncing the Driver with the ADC  for the LowSide Sensing
+ */
+void _driverSyncLowSide(void* driver_params, void* cs_params)
+{
+  _UNUSED(driver_params);
+  _UNUSED(cs_params);
+
+  SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented"));
+  instance.startADCScan();
+  //TODO: hook with PWM interrupts
+}
+
+
+
+
+
+
+
+
+
+
+ // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless
+
+static void adcStopWithDMA(void);
+static void adcStartWithDMA(void);
+
+/**
+ * @brief  ADC sync wait 
+ * @retval void
+ */
+static __inline__ void ADCsync() __attribute__((always_inline, unused));
+static void   ADCsync() {
+  while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free
+}
+
+//  ADC DMA sequential free running (6) with Interrupts /////////////////
+
+SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() 
+{
+  
+  return &instance;
+}
+
+SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA()
+{
+}
+
+void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA)
+{
+  this->pinA = pinA;
+  this->pinB = pinB;
+  this->pinC = pinC;
+  this->pinAREF = pinAREF;
+  this->channelDMA = channelDMA;
+  this->voltageAREF = voltageAREF;
+  this->maxCountsADC = 1 << adcBits;
+  countsToVolts = ( voltageAREF / maxCountsADC );
+
+  initPins();
+  initADC();
+  initDMA();
+  startADCScan(); //so we have something to read next time we call readResults()
+}
+
+
+void SAMDCurrentSenseADCDMA::startADCScan(){
+  adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize);
+  adcStartWithDMA();
+}
+
+bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){
+  if(ADC->CTRLA.bit.ENABLE)
+    return false;
+  uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber;
+  uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber;
+  a = adcBuffer[ainA];
+  b = adcBuffer[ainB];
+  if(_isset(pinC))
+  {
+    uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber;
+    c = adcBuffer[ainC];
+  }
+  return true;
+}
+
+
+float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) {
+  return counts * countsToVolts;
+}
+
+void SAMDCurrentSenseADCDMA::initPins(){
+  
+  if (pinAREF>=0)
+    pinMode(pinAREF, INPUT);
+  pinMode(pinA, INPUT);
+  pinMode(pinB, INPUT);
+
+  uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber;
+  uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber;
+  firstAIN = min(ainA, ainB);
+  lastAIN = max(ainA, ainB);
+  if( _isset(pinC) ) 
+  {
+    uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber;
+    pinMode(pinC, INPUT);
+    firstAIN = min(firstAIN, ainC);
+    lastAIN = max(lastAIN, ainC);
+  }
+
+  oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout
+  BufferSize = lastAIN - oneBeforeFirstAIN + 1;
+
+}
+
+void SAMDCurrentSenseADCDMA::initADC(){
+
+  analogRead(pinA);  // do some pin init  pinPeripheral() 
+  analogRead(pinB);  // do some pin init  pinPeripheral() 
+  analogRead(pinC);  // do some pin init  pinPeripheral() 
+
+  ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC
+  ADCsync();
+  //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; //  2.2297 V Supply VDDANA
+  ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X
+  // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val;  // default
+  if (pinAREF>=0)
+    ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA;
+  else
+    ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0;
+  ADCsync();  //  ref 31.6.16
+
+  /*
+  Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan
+  This register gives the number of input sources included in the pin scan. The number of input sources included is
+  INPUTSCAN + 1. The input channels included are in the range from MUXPOS + INPUTOFFSET to MUXPOS +
+  INPUTOFFSET + INPUTSCAN.
+  The range of the scan mode must not exceed the number of input channels available on the device.
+  Bits 4:0 – MUXPOS[4:0]: Positive Mux Input Selection
+  These bits define the Mux selection for the positive ADC input. Table 32-14 shows the possible input selections. If
+  the internal bandgap voltage or temperature sensor input channel is selected, then the Sampling Time Length bit
+  group in the SamplingControl register must be written.
+  Table 32-14. Positive Mux Input Selection
+  MUXPOS[4:0] Group configuration Description
+  0x00 PIN0 ADC AIN0 pin
+  0x01 PIN1 ADC AIN1 pin
+  0x02 PIN2 ADC AIN2 pin
+  0x03 PIN3 ADC AIN3 pin
+  0x04 PIN4 ADC AIN4 pin
+  0x05 PIN5 ADC AIN5 pin
+  0x06 PIN6 ADC AIN6 pin
+  0x07 PIN7 ADC AIN7 pin
+  0x08 PIN8 ADC AIN8 pin
+  0x09 PIN9 ADC AIN9 pin
+  0x0A PIN10 ADC AIN10 pin
+  0x0B PIN11 ADC AIN11 pin
+  0x0C PIN12 ADC AIN12 pin
+  0x0D PIN13 ADC AIN13 pin
+  0x0E PIN14 ADC AIN14 pin
+  0x0F PIN15 ADC AIN15 pin
+  0x10 PIN16 ADC AIN16 pin
+  0x11 PIN17 ADC AIN17 pin
+  0x12 PIN18 ADC AIN18 pin
+  0x13 PIN19 ADC AIN19 pin
+  0x14-0x17 Reserved
+  0x18 TEMP Temperature reference
+  0x19 BANDGAP Bandgap voltage
+  0x1A SCALEDCOREVCC 1/4 scaled core supply
+  0x1B SCALEDIOVCC 1/4 scaled I/O supply
+  0x1C DAC DAC output
+  0x1D-0x1F Reserved
+  */
+  ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN;
+  ADCsync();
+  ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) 
+  ADCsync();
+  ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor
+  ADCsync();
+  ADC->AVGCTRL.reg = 0x00 ;  //no averaging
+  ADC->SAMPCTRL.reg = 0x05;  ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16
+  // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU
+  ADCsync();
+  ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT;
+  ADCsync();
+}
+
+volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16)));
+
+void SAMDCurrentSenseADCDMA::initDMA() {
+  // probably on by default
+  PM->AHBMASK.reg |= PM_AHBMASK_DMAC ;
+  PM->APBBMASK.reg |= PM_APBBMASK_DMAC ;
+  NVIC_EnableIRQ( DMAC_IRQn ) ;
+  DMAC->BASEADDR.reg = (uint32_t)descriptor_section;
+  DMAC->WRBADDR.reg = (uint32_t)wrb;
+  DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf);
+}
+
+
+void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata,  uint32_t hwords) {
+
+  DMAC->CHID.reg = DMAC_CHID_ID(channelDMA);
+  DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE;
+  DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST;
+  DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA));
+  
+  DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) 
+  | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) 
+  | DMAC_CHCTRLB_TRIGACT_BEAT;
+  DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts
+  descriptor.descaddr = 0;
+  descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg;
+  descriptor.btcnt =  hwords;
+  descriptor.dstaddr = (uint32_t)rxdata + hwords*2;   // end address
+  descriptor.btctrl =  DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID;
+  memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor));
+
+  // start channel
+  DMAC->CHID.reg = DMAC_CHID_ID(channelDMA);
+  DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
+}
+
+
+int iii = 0;
+
+void adcStopWithDMA(void){
+  ADCsync();
+  ADC->CTRLA.bit.ENABLE = 0x00;
+  // ADCsync();
+  // if(iii++ % 1000 == 0)
+  // {
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a);
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: ");
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b);
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: ");
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c);
+  //   SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!");
+  // }
+
+
+}
+
+void adcStartWithDMA(void){
+  ADCsync();
+  ADC->INPUTCTRL.bit.INPUTOFFSET = 0;
+  ADCsync();
+  ADC->SWTRIG.bit.FLUSH = 1;
+  ADCsync();
+  ADC->CTRLA.bit.ENABLE = 0x01; 
+  ADCsync();
+}
+
+void DMAC_Handler() {
+  uint8_t active_channel;
+  active_channel =  DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number
+  DMAC->CHID.reg = DMAC_CHID_ID(active_channel);
+  adcStopWithDMA();
+  DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear
+  DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR;
+  DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP;
+
+}
+
+
+#endif

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/samd/samd21_mcu.h

@@ -0,0 +1,69 @@
+#ifdef _SAMD21_
+
+#ifndef CURRENT_SENSE_SAMD21_H
+#define CURRENT_SENSE_SAMD21_H
+
+#define SIMPLEFOC_SAMD_DEBUG
+#if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL)
+#define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial
+#endif
+
+#include <stdint.h>
+  typedef struct {
+    uint16_t btctrl;
+    uint16_t btcnt;
+    uint32_t srcaddr;
+    uint32_t dstaddr;
+    uint32_t descaddr;
+  } dmacdescriptor ;
+
+
+// AREF pin is 42
+
+class SAMDCurrentSenseADCDMA
+{
+
+public:
+  static SAMDCurrentSenseADCDMA * getHardwareAPIInstance();
+  SAMDCurrentSenseADCDMA();
+  void init(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3);
+  void startADCScan();
+  bool readResults(uint16_t & a, uint16_t & b, uint16_t & c);
+  float toVolts(uint16_t counts);
+private:
+
+  void adcToDMATransfer(void *rxdata,  uint32_t hwords);
+
+  void initPins();
+  void initADC();
+  void initDMA();
+ 
+  uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout
+  uint32_t firstAIN;
+  uint32_t lastAIN; 
+  uint32_t BufferSize = 0;
+
+  uint16_t adcBuffer[20];
+
+
+  uint32_t pinA;
+  uint32_t pinB;
+  uint32_t pinC;
+  uint32_t pinAREF;
+  uint32_t channelDMA;  // DMA channel
+  bool freeRunning;
+
+  float voltageAREF;
+  float maxCountsADC;
+  float countsToVolts;
+  
+  dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16)));
+  dmacdescriptor descriptor __attribute__ ((aligned (16)));
+
+};
+
+#endif
+
+
+
+#endif

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

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.cpp

@@ -0,0 +1,364 @@
+#include "../../../hardware_api.h"
+#if defined(ARDUINO_B_G431B_ESC1)
+
+#include "communication/SimpleFOCDebug.h"
+
+#include "stm32g4xx_hal.h"
+#include "stm32g4xx_ll_pwr.h"
+#include "stm32g4xx_hal_adc.h"
+#include "b_g431_hal.h"
+
+// From STM32 cube IDE
+/**
+  ******************************************************************************
+  * @attention
+  *
+  * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
+  * All rights reserved.</center></h2>
+  *
+  * This software component is licensed by ST under BSD 3-Clause license,
+  * the "License"; You may not use this file except in compliance with the
+  * License. You may obtain a copy of the License at:
+  *                        opensource.org/licenses/BSD-3-Clause
+  *
+  ******************************************************************************
+  */
+
+
+/**
+  * @brief GPIO Initialization Function
+  * @param None
+  * @retval None
+  */
+void MX_GPIO_Init(void)
+{
+  /* GPIO Ports Clock Enable */
+  __HAL_RCC_GPIOC_CLK_ENABLE();
+  __HAL_RCC_GPIOF_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
+  __HAL_RCC_GPIOB_CLK_ENABLE();
+
+  __HAL_RCC_ADC12_CLK_ENABLE();
+}
+
+/** 
+  * Enable DMA controller clock
+  */
+void  MX_DMA_Init(void) 
+{
+  /* DMA controller clock enable */
+  __HAL_RCC_DMAMUX1_CLK_ENABLE();
+  __HAL_RCC_DMA1_CLK_ENABLE();
+  __HAL_RCC_DMA2_CLK_ENABLE();
+
+  /* DMA interrupt init */
+  /* DMA1_Channel1_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn);
+  /* DMA1_Channel2_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
+
+  // Enable external clock for ADC
+  RCC_PeriphCLKInitTypeDef PeriphClkInit;
+  PeriphClkInit.PeriphClockSelection   = RCC_PERIPHCLK_ADC12;
+  PeriphClkInit.Adc12ClockSelection    = RCC_ADC12CLKSOURCE_PLL;
+  HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);
+}
+
+
+/**
+  * @brief ADC1 Initialization Function
+  * @param None
+  * @retval None
+  */
+void  MX_ADC1_Init(ADC_HandleTypeDef* hadc1)
+{
+  /* USER CODE BEGIN ADC1_Init 0 */
+
+  /* USER CODE END ADC1_Init 0 */
+
+  ADC_MultiModeTypeDef multimode = {0};
+  ADC_ChannelConfTypeDef sConfig = {0};
+
+  /* USER CODE BEGIN ADC1_Init 1 */
+
+  /* USER CODE END ADC1_Init 1 */
+  /** Common config 
+  */
+  hadc1->Instance = ADC1;
+  hadc1->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16;
+  hadc1->Init.Resolution = ADC_RESOLUTION_12B;
+  hadc1->Init.DataAlign = ADC_DATAALIGN_RIGHT;
+  hadc1->Init.GainCompensation = 0;
+  hadc1->Init.ScanConvMode = ADC_SCAN_ENABLE;
+  hadc1->Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  hadc1->Init.LowPowerAutoWait = DISABLE;
+  hadc1->Init.ContinuousConvMode = DISABLE;
+  hadc1->Init.NbrOfConversion = 2;
+  hadc1->Init.DiscontinuousConvMode = DISABLE;
+  hadc1->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T3_TRGO;
+  hadc1->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
+  hadc1->Init.DMAContinuousRequests = ENABLE;
+  hadc1->Init.Overrun = ADC_OVR_DATA_PRESERVED;
+
+  if (HAL_ADC_Init(hadc1) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("HAL_ADC_Init failed!");
+  }
+
+  /** Configure the ADC multi-mode 
+  */
+  multimode.Mode = ADC_MODE_INDEPENDENT;
+  if (HAL_ADCEx_MultiModeConfigChannel(hadc1, &multimode) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("HAL_ADCEx_MultiModeConfigChannel failed!");
+  }
+  /** Configure Regular Channel 
+  */
+  sConfig.Channel = ADC_CHANNEL_12;     // ADC1_IN12 = PB1 = OP3_OUT
+  sConfig.Rank = ADC_REGULAR_RANK_1;
+  sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5;
+  sConfig.SingleDiff = ADC_SINGLE_ENDED;
+  sConfig.OffsetNumber = ADC_OFFSET_NONE;
+  sConfig.Offset = 0;
+  if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!");
+  }
+  /** Configure Regular Channel 
+  */
+  sConfig.Channel = ADC_CHANNEL_3;  // ADC1_IN3 = PA2 = OP1_OUT
+  sConfig.Rank = ADC_REGULAR_RANK_2;
+  if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!");
+  }
+
+  //******************************************************************
+  // Temp, Poti ....
+  /** Configure Regular Channel (PB14, Temperature)
+  */
+  sConfig.Channel = ADC_CHANNEL_5;
+  sConfig.Rank = ADC_REGULAR_RANK_4;
+  sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5;
+  sConfig.SingleDiff = ADC_SINGLE_ENDED;
+  sConfig.OffsetNumber = ADC_OFFSET_NONE;
+  sConfig.Offset = 0;
+  if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!");
+  }
+
+  /** Configure Regular Channel (PB14, Temperature)
+  */
+  sConfig.Channel = ADC_CHANNEL_1;
+  sConfig.Rank = ADC_REGULAR_RANK_5;
+  sConfig.SamplingTime = ADC_SAMPLETIME_47CYCLES_5;
+  sConfig.SingleDiff = ADC_SINGLE_ENDED;
+  sConfig.OffsetNumber = ADC_OFFSET_NONE;
+  sConfig.Offset = 0;
+  if (HAL_ADC_ConfigChannel(hadc1, &sConfig) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!");
+  }
+  /* USER CODE BEGIN ADC1_Init 2 */
+
+  /* USER CODE END ADC1_Init 2 */
+
+}
+
+/**
+  * @brief ADC2 Initialization Function
+  * @param None
+  * @retval None
+  */
+void MX_ADC2_Init(ADC_HandleTypeDef* hadc2)
+{
+  /* USER CODE BEGIN ADC2_Init 0 */
+
+  /* USER CODE END ADC2_Init 0 */
+
+  ADC_ChannelConfTypeDef sConfig = {0};
+
+  /* USER CODE BEGIN ADC2_Init 1 */
+
+  /* USER CODE END ADC2_Init 1 */
+  /** Common config 
+  */
+  hadc2->Instance = ADC2;
+  hadc2->Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV16;
+  hadc2->Init.Resolution = ADC_RESOLUTION_12B;
+  hadc2->Init.DataAlign = ADC_DATAALIGN_RIGHT;
+  hadc2->Init.GainCompensation = 0;
+  hadc2->Init.ScanConvMode = ADC_SCAN_ENABLE;
+  hadc2->Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+  hadc2->Init.LowPowerAutoWait = DISABLE;
+  hadc2->Init.ContinuousConvMode = DISABLE;
+  hadc2->Init.NbrOfConversion = 1;
+  hadc2->Init.DiscontinuousConvMode = DISABLE;
+  hadc2->Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO;
+  hadc2->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
+  hadc2->Init.DMAContinuousRequests = ENABLE;
+  hadc2->Init.Overrun = ADC_OVR_DATA_PRESERVED;
+
+  if (HAL_ADC_Init(hadc2) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("HAL_ADC_Init failed!");
+  }
+  /** Configure Regular Channel 
+  */
+  sConfig.Channel = ADC_CHANNEL_3;  // ADC2_IN3 = PA6
+  sConfig.Rank = ADC_REGULAR_RANK_1;
+  sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5;
+  sConfig.SingleDiff = ADC_SINGLE_ENDED;
+  sConfig.OffsetNumber = ADC_OFFSET_NONE;
+  sConfig.Offset = 0;
+  if (HAL_ADC_ConfigChannel(hadc2, &sConfig) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("HAL_ADC_ConfigChannel failed!");
+  }
+  /* USER CODE BEGIN ADC2_Init 2 */
+
+  /* USER CODE END ADC2_Init 2 */
+
+}
+
+/**
+* @brief OPAMP MSP Initialization
+* This function configures the hardware resources used in this example
+* @param hopamp-> OPAMP handle pointer
+* @retval None
+*/
+void HAL_OPAMP_MspInit(OPAMP_HandleTypeDef* hopamp)
+{
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(hopamp->Instance==OPAMP1)
+  {
+  /* USER CODE BEGIN OPAMP1_MspInit 0 */
+
+  /* USER CODE END OPAMP1_MspInit 0 */
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**OPAMP1 GPIO Configuration    
+    PA1     ------> OPAMP1_VINP
+    PA2     ------> OPAMP1_VOUT
+    PA3     ------> OPAMP1_VINM 
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3;
+    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN OPAMP1_MspInit 1 */
+
+  /* USER CODE END OPAMP1_MspInit 1 */
+  }
+  else if(hopamp->Instance==OPAMP2)
+  {
+  /* USER CODE BEGIN OPAMP2_MspInit 0 */
+
+  /* USER CODE END OPAMP2_MspInit 0 */
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**OPAMP2 GPIO Configuration    
+    PA5     ------> OPAMP2_VINM
+    PA6     ------> OPAMP2_VOUT
+    PA7     ------> OPAMP2_VINP 
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7;
+    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN OPAMP2_MspInit 1 */
+
+  /* USER CODE END OPAMP2_MspInit 1 */
+  }
+  else if(hopamp->Instance==OPAMP3)
+  {
+  /* USER CODE BEGIN OPAMP3_MspInit 0 */
+
+  /* USER CODE END OPAMP3_MspInit 0 */
+
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+    /**OPAMP3 GPIO Configuration    
+    PB0     ------> OPAMP3_VINP
+    PB1     ------> OPAMP3_VOUT
+    PB2     ------> OPAMP3_VINM 
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2;
+    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN OPAMP3_MspInit 1 */
+
+  /* USER CODE END OPAMP3_MspInit 1 */
+  }
+
+}
+
+/**
+* @brief OPAMP MSP De-Initialization
+* This function freeze the hardware resources used in this example
+* @param hopamp-> OPAMP handle pointer
+* @retval None
+*/
+void HAL_OPAMP_MspDeInit(OPAMP_HandleTypeDef* hopamp)
+{
+  if(hopamp->Instance==OPAMP1)
+  {
+  /* USER CODE BEGIN OPAMP1_MspDeInit 0 */
+
+  /* USER CODE END OPAMP1_MspDeInit 0 */
+
+    /**OPAMP1 GPIO Configuration    
+    PA1     ------> OPAMP1_VINP
+    PA2     ------> OPAMP1_VOUT
+    PA3     ------> OPAMP1_VINM 
+    */
+    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3);
+
+  /* USER CODE BEGIN OPAMP1_MspDeInit 1 */
+
+  /* USER CODE END OPAMP1_MspDeInit 1 */
+  }
+  else if(hopamp->Instance==OPAMP2)
+  {
+  /* USER CODE BEGIN OPAMP2_MspDeInit 0 */
+
+  /* USER CODE END OPAMP2_MspDeInit 0 */
+
+    /**OPAMP2 GPIO Configuration    
+    PA5     ------> OPAMP2_VINM
+    PA6     ------> OPAMP2_VOUT
+    PA7     ------> OPAMP2_VINP 
+    */
+    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7);
+
+  /* USER CODE BEGIN OPAMP2_MspDeInit 1 */
+
+  /* USER CODE END OPAMP2_MspDeInit 1 */
+  }
+  else if(hopamp->Instance==OPAMP3)
+  {
+  /* USER CODE BEGIN OPAMP3_MspDeInit 0 */
+
+  /* USER CODE END OPAMP3_MspDeInit 0 */
+
+    /**OPAMP3 GPIO Configuration    
+    PB0     ------> OPAMP3_VINP
+    PB1     ------> OPAMP3_VOUT
+    PB2     ------> OPAMP3_VINM 
+    */
+    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2);
+
+  /* USER CODE BEGIN OPAMP3_MspDeInit 1 */
+
+  /* USER CODE END OPAMP3_MspDeInit 1 */
+  }
+
+}
+
+#endif 

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/b_g431/b_g431_hal.h

@@ -0,0 +1,18 @@
+#ifndef B_G431_ESC1_HAL
+#define B_G431_ESC1_HAL
+
+#if defined(ARDUINO_B_G431B_ESC1)
+
+#include <stm32g4xx_hal_adc.h>
+#include <stm32g4xx_hal_opamp.h>
+
+void MX_GPIO_Init(void);
+void MX_DMA_Init(void);
+void MX_ADC1_Init(ADC_HandleTypeDef* hadc1);
+void MX_ADC2_Init(ADC_HandleTypeDef* hadc2);
+void MX_OPAMP1_Init(OPAMP_HandleTypeDef* hopamp);
+void MX_OPAMP2_Init(OPAMP_HandleTypeDef* hopamp);
+void MX_OPAMP3_Init(OPAMP_HandleTypeDef* hopamp);
+#endif
+
+#endif

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/b_g431/b_g431_mcu.cpp

@@ -0,0 +1,164 @@
+#include "../../../hardware_api.h"
+
+#if defined(ARDUINO_B_G431B_ESC1) 
+
+#include "b_g431_hal.h"
+#include "Arduino.h"
+#include "../stm32_mcu.h"
+#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
+#include "communication/SimpleFOCDebug.h"
+
+#define _ADC_VOLTAGE 3.3f
+#define _ADC_RESOLUTION 4096.0f
+#define ADC_BUF_LEN_1 5
+#define ADC_BUF_LEN_2 1
+
+static ADC_HandleTypeDef hadc1;
+static ADC_HandleTypeDef hadc2;
+static OPAMP_HandleTypeDef hopamp1;
+static OPAMP_HandleTypeDef hopamp2;
+static OPAMP_HandleTypeDef hopamp3;
+
+static DMA_HandleTypeDef hdma_adc1;
+static DMA_HandleTypeDef hdma_adc2;
+
+volatile uint16_t adcBuffer1[ADC_BUF_LEN_1] = {0}; // Buffer for store the results of the ADC conversion
+volatile uint16_t adcBuffer2[ADC_BUF_LEN_2] = {0}; // Buffer for store the results of the ADC conversion
+
+// function reading an ADC value and returning the read voltage
+// As DMA is being used just return the DMA result
+float _readADCVoltageInline(const int pin, const void* cs_params){
+  uint32_t raw_adc = 0;
+  if(pin == PA2)  // = ADC1_IN3 = phase U (OP1_OUT) on B-G431B-ESC1
+    raw_adc = adcBuffer1[1];
+  else if(pin == PA6) // = ADC2_IN3 = phase V (OP2_OUT) on B-G431B-ESC1
+    raw_adc = adcBuffer2[0];
+  else if(pin == PB1) // = ADC1_IN12 = phase W (OP3_OUT) on B-G431B-ESC1
+    raw_adc = adcBuffer1[0];
+  else if (pin == A_TEMPERATURE)
+    raw_adc = adcBuffer1[3];
+
+  return raw_adc * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv;
+}
+
+void _configureOPAMP(OPAMP_HandleTypeDef *hopamp, OPAMP_TypeDef *OPAMPx_Def){
+  // could this be replaced with LL_OPAMP calls??
+  hopamp->Instance = OPAMPx_Def;
+  hopamp->Init.PowerMode = OPAMP_POWERMODE_HIGHSPEED;
+  hopamp->Init.Mode = OPAMP_PGA_MODE;
+  hopamp->Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0;
+  hopamp->Init.InternalOutput = DISABLE;
+  hopamp->Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE;
+  hopamp->Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_IO0_BIAS;
+  hopamp->Init.PgaGain = OPAMP_PGA_GAIN_16_OR_MINUS_15;
+  hopamp->Init.UserTrimming = OPAMP_TRIMMING_FACTORY;
+  if (HAL_OPAMP_Init(hopamp) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("HAL_OPAMP_Init failed!");
+  }
+}
+void _configureOPAMPs(OPAMP_HandleTypeDef *OPAMPA, OPAMP_HandleTypeDef *OPAMPB, OPAMP_HandleTypeDef *OPAMPC){
+  // Configure the opamps
+  _configureOPAMP(OPAMPA, OPAMP1);
+  _configureOPAMP(OPAMPB, OPAMP2);
+  _configureOPAMP(OPAMPC, OPAMP3);
+}
+
+void MX_DMA1_Init(ADC_HandleTypeDef *hadc, DMA_HandleTypeDef *hdma_adc, DMA_Channel_TypeDef* channel, uint32_t request) {
+  hdma_adc->Instance = channel;
+  hdma_adc->Init.Request = request;
+  hdma_adc->Init.Direction = DMA_PERIPH_TO_MEMORY;
+  hdma_adc->Init.PeriphInc = DMA_PINC_DISABLE;
+  hdma_adc->Init.MemInc = DMA_MINC_ENABLE;
+  hdma_adc->Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
+  hdma_adc->Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
+  hdma_adc->Init.Mode = DMA_CIRCULAR;
+  hdma_adc->Init.Priority = DMA_PRIORITY_LOW;
+  HAL_DMA_DeInit(hdma_adc);
+  if (HAL_DMA_Init(hdma_adc) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("HAL_DMA_Init failed!");
+  }
+  __HAL_LINKDMA(hadc, DMA_Handle, *hdma_adc);
+}
+
+void* _configureADCInline(const void* driver_params, const int pinA,const int pinB,const int pinC){
+  _UNUSED(driver_params);
+  _UNUSED(pinA);
+  _UNUSED(pinB);
+  _UNUSED(pinC);
+
+  SIMPLEFOC_DEBUG("B-G431B does not implement inline current sense. Use low-side current sense instead.");
+  return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
+}
+
+
+void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){
+  _UNUSED(driver_params);
+
+  HAL_Init();
+  MX_GPIO_Init();
+  MX_DMA_Init(); 
+  _configureOPAMPs(&hopamp1, &hopamp3, &hopamp2);
+  MX_ADC1_Init(&hadc1);
+  MX_ADC2_Init(&hadc2);
+
+  MX_DMA1_Init(&hadc1, &hdma_adc1, DMA1_Channel1, DMA_REQUEST_ADC1);
+  MX_DMA1_Init(&hadc2, &hdma_adc2, DMA1_Channel2, DMA_REQUEST_ADC2);
+
+  if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adcBuffer1, ADC_BUF_LEN_1) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("DMA read init failed");
+  }
+  if (HAL_ADC_Start_DMA(&hadc2, (uint32_t*)adcBuffer2, ADC_BUF_LEN_2) != HAL_OK)
+  {
+    SIMPLEFOC_DEBUG("DMA read init failed");
+  }
+
+  HAL_OPAMP_Start(&hopamp1);
+  HAL_OPAMP_Start(&hopamp2);
+  HAL_OPAMP_Start(&hopamp3); 
+  
+  Stm32CurrentSenseParams* params = new Stm32CurrentSenseParams {
+    .pins = { pinA, pinB, pinC },
+    .adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION),
+    .timer_handle = (HardwareTimer *)(HardwareTimer_Handle[get_timer_index(TIM1)]->__this)
+  };
+
+  return params;
+}
+
+extern "C" {
+void DMA1_Channel1_IRQHandler(void) {
+  HAL_DMA_IRQHandler(&hdma_adc1);
+}
+
+void DMA1_Channel2_IRQHandler(void) {
+   HAL_DMA_IRQHandler(&hdma_adc2);
+}
+}
+
+void _driverSyncLowSide(void* _driver_params, void* _cs_params){
+  STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params;
+  Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params;
+   
+  // stop all the timers for the driver
+  _stopTimers(driver_params->timers, 6);
+
+  // if timer has repetition counter - it will downsample using it
+  // and it does not need the software downsample
+  if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){
+    // adjust the initial timer state such that the trigger for DMA transfer aligns with the pwm peaks instead of throughs.
+    // only necessary for the timers that have repetition counters
+    cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR;
+    cs_params->timer_handle->getHandle()->Instance->CNT =  cs_params->timer_handle->getHandle()->Instance->ARR;
+  }
+  // set the trigger output event
+  LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE);
+
+  // restart all the timers of the driver
+  _startTimers(driver_params->timers, 6);
+
+}
+
+#endif

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

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

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

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.cpp

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_hal.h

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f1/stm32f1_mcu.cpp

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.cpp

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_hal.h

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_mcu.cpp

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.cpp

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32f4/stm32f4_utils.h

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.cpp

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_hal.h

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_mcu.cpp

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.cpp

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32g4/stm32g4_utils.h

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h

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

firmware/lib/Arduino-FOC/src/current_sense/hardware_specific/teensy_mcu.cpp

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

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