drivers/hardware_specific/due_mcu.cpp

Go to the documentation of this file.

#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