Go Down

Topic: OneShot125 ESC (Read 1 time) previous topic - next topic

barbosafs

Hello guys, newbie here.

So, I have an Arduino Due and a BLHeli flashed ESC (Afro), and I want to use the OneShot feature.

Is there any library for this? Or has anyone already coded for this aplication?
How should I set my PWM frequency and dutycycle?

I have found this code, but I don't get it, how do I make it work?

Code: [Select]
void pwm_OneShot(uint32_t ch, uint32_t value){
  PWMC_SetDutyCycle(PWM, ch, value);
 
  PWMC_EnableChannel(PWM, ch);          //counter is reset here, pulse starts right now
 
  PWMC_SetDutyCycle(PWM, ch, 0);       //this stops pulsing until this function is called again (this gets written in the update register and does not affect the current pulse)
 
  PWMC_DisableChannel(PWM, CH);         //channel is disabled at the end of current period
}


Thanks!

MartinL

#1
Oct 14, 2016, 10:16 pm Last Edit: Oct 15, 2016, 10:37 am by MartinL
Hi barbosafs,

Unfortunately the Arduino Due, unlike the Arduino Zero doesn't have PWM oneshot built in. Although, as you code above suggests, it's possible by enabling the PWM, loading the buffered duty cycle update register (REG_PWM_CDTYUPD0), then disabling PWM to generate a single oneshot pulse on the output.

The first consideration is the timer speed. For a standard multi-rotor ESC we use 1000us to 2000us, PWM pulse widths at a frequency from 250Hz to 500Hz. For single slope PWM this is achieved using a base PWM timer frequency of 1MHz. Using this method it's possible to load the duty cycle register with the standard 1000us to 2000us values and output corresponding 1000us to 2000us pulses.

For Oneshot125 though, we require 125us to 250us pulses. To achieve these pulse widths we just need to increase the timer speed 8 times, giving us a base timer frequency of 8MHz. Using this method it's possible to load the duty cycle register with the standard 1000us to 2000us values and output corresponding 125us to 250us pulses.

The next issue is that the 84MHz clock on the Arduino Due isn't divisible by 8. However, I remembered reading an article for the Taulab's CC3D flight controller: https://github.com/TauLabs/TauLabs/wiki/User-Guide:-OneShot125. They instead used a 12MHz PWM timer base frequency and multiplied the standard 1000us to 2000us values by 1.5, before loading them into the duty cycle register. 12MHz works nicely for the Due as we can divide 84MHz by 7.

The oneshot is achieved as you code snippet mentioned, just by enabling PWM, loading the buffered duty cycle update register (REG_PWM_CDTYUPD0) then disabling PWM. By the way, the period is set to a value greater than 250us. This value isn't critical, as we're using oneshot rather than repeating the pulse at a given frequency. I've set the period to 2500us * 1.5 = 3750 (or 312.5us).

In the code below just load the REG_PWM_CDTYUPD0 register with the throttle value (1000us - 2000us), multiplied by 1.5. I've set it to half throttle in this example. It goes without saying, if your using a propeller, it's best to test it first with the prop off.

The following code sets up Oneshot125 output on the DAC1 pin (PWML0):

Code: [Select]
// Output Oneshot125 PWM on pin DAC1 (PWML0)
void setup()
{
  // PWM Set-up on pin: DAC1
  REG_PMC_PCER1 |= PMC_PCER1_PID36;                     // Enable PWM
  REG_PIOB_ABSR |= PIO_ABSR_P16;                        // Set PWM pin perhipheral type A or B, in this case B
  REG_PIOB_PDR |= PIO_PDR_P16;                          // Set PWM pin to an output
  REG_PWM_CLK = PWM_CLK_PREA(0) | PWM_CLK_DIVA(7);      // Set the PWM clock rate to 12MHz (84MHz/7)
  REG_PWM_CMR0 = PWM_CMR_CPRE_CLKA;                     // Enable single slope PWM and set the clock source as CLKA
  REG_PWM_CPRD0 = 3750;                                 // Set the PWM frequency less than 4kHz to 12MHz/3750 = 3.2kHz
  REG_PWM_CDTY0 = 0;                                    // Set the PWM duty cycle to 0
  REG_PWM_ENA = PWM_ENA_CHID0;                          // Enable the PWM channel 0   
}

void loop()
{
  // Delay - normally replace this line with your motor control code
  delay(2);                                             // Simulate looptime at 2ms
 
  // Note that 1500 below is your standard ESC PWM value 1000us - min, 1500us - half and 2000us - max throttle
  // Multiplied by 1.5 to account for the 12MHz timer clock, (the Due is unable to generate the 8MHz clock we require)
  REG_PWM_ENA = PWM_ENA_CHID0;                          // Enable the PWM channel - retrigger the Oneshot125 pulse
  REG_PWM_CDTYUPD0 = (unsigned long)(1500 * 1.5);       // Set throttle to half throttle - WARNING this will activate the motor
  REG_PWM_DIS = PWM_DIS_CHID0;                          // Disable the PWM channel - however the buffered pulse is still output
}

Markishere123

Hi,
I would like to use oneshot125 protocol on arduino due for my quadcopter. I am using pins 4,5, 6, and 7 for communicating with the esc's of the four motors. Using standard servo signal are not fast enough.

Can you please help me with writing code for the registers/timers to allow the use of oneshot125 pwm on arduino due for all 4 of my pins (pins 4,5,6,7)? I do not know how to use registers as the way you have in the previous code for controlling the esc through pin DAC1 (67 found on this website: https://www.arduino.cc/en/Hacking/PinMappingSAM3X).

Thank you for your help I greatly appreciate it!

MartinL

Hi Markishere123,

The above code uses the Due's 8-channel PWM controller. These channels are only available on certain pins.

Digital pins D6 and D7 are connected to PWM channels 6 (PWML6) and 7 (PWML7) respectively.

Digital pins D4 and D5 however are not connected to the PWM controller, but instead are connected to channels A and B of timer 6. Timers are capable of PWM output, but with more limited functionality.

The question is how easy is it for you to move pins D4 and D5?

If it's just a case of changing wires on a breadboard then I'd move D4 and D5 to other available PWM controller pins. If though it's a more permanent fixture then it would be better to implement a workaround in software using timer 6.


Markishere123

Hi MartinL,

I did some quick research and I believe I can use pins 8 and 9 instead of 4 and 5 since pins 8 and 9 are connected to PWM channels 5 and 4 respectively (  https://forum.arduino.cc/index.php?topic=367154.0   ).

I have no problem with changing the pins to any other pins since I wrote all the code myself for my quadcopter. My arduino due receives signals and sends the motor signals to the esc's directly.



I understand that I will be changing the value of 1500 in the line of code

REG_PWM_CDTYUPD0 = (unsigned long)(1500 * 1.5);       // Set throttle to half throttle - WARNING this will activate the motor

to control the motor speed. I do not know how to use registers and if you will be able to help me with rewriting the code posted above to work for four different pins (preferable 6, 7, 8, 9) or any other 4 I would greatly appreciate it. I assume the code needs to be copied and pasted to create 4 instances and the correct register values should be changed but I don't know how to do this and I don't understand what you have done previously with the registers but I know how to use the code and changing the 1500 with my own values.

Thank you for your help and quick response!

MartinL

#5
May 16, 2018, 07:39 pm Last Edit: May 16, 2018, 08:09 pm by MartinL
Hi Markishere123,

I attempted to extend Oneshot125 to PWM channels 4 to 7, but unfortunately this doesn't work that well.

The reason is, is that when then PWM is disabled the line goes high impedance. It's just that this effect isn't noticable on channels 0 to 3 (on DAC1, A8, A9 and A10), so I missed it, but should've known. However, it's possible to see a small kick at the end of the time cycle on channels 4 to 7 (on D9, D8, D7 and D6).

A quick solution is to use a 10K resistor pull-down resistors, to hold the lines low, hardly ideal.

The image shows the characteristic 125us pulses on channels 0 and 2 at the top, followed by 4 and 5 at the bottom on my scope:



I guess I'll have to think of some means of generating a Oneshot125 pulse with actively driven outputs.

In the meantime, do your ESC's support standard 490Hz PWM? I've got some Due code that supports this. I used it to fly one of my tricopters.

Markishere123

Hi MartinL,

Thanks for the information!
I am able to use essentially any 4 pins on the Arduino Due for communication with the 4 ESC's. I need to use pins 20 & 21 and 18 & 19 or a different serial port for communication with my radio and gyroscope but all other pins can be used.

Are there four pins on the Due that are capable of being used for oneshot125, since it is not a problem at all to switch wires for me?

I would like to use oneshot125 since it is much faster than standard servo library pulse signals. I am currently using the servo library but would like to use a faster protocol. With the servo library and current code I am able to run 100 loops per second but would like to increase my loops per second.
Also, I don't know how to program registers and would need help with that when creating code for using oneshot125 on four pins.

Thank you for your great help!

MartinL

#7
May 17, 2018, 10:52 am Last Edit: May 17, 2018, 02:01 pm by MartinL
Hi Markishere123,

Ok, I finally have a solution and have implemented Oneshot125 on the Due with actively driven outputs.

The problem was I kept on bumping up to limitations of the Due's PWM Controller. The only way to reset the controller's counters is to disable the corresponding channel, but as I mentioned previously this causes the output to go into a high impedance state. Also, it's not possible to access the counter registers as they're read only. So another approach had to be found.

The solution is to allow the PWM Controller's ISR (Interrupt Service Routine) to generate a single pulse. In the code below if you change the motor output values in the loop() portion of the code, the ISR will generate the corresponding single Oneshot125 pulse on the output.

This works on digital outputs D6, D7, D8 and D9.

If you look at the output on a scope you'll notice that there's some jitter between successive pulses, this is because the loop() code is working asynchronously with respect to PWM Controller's timers. When the ISR is enabled in the loop(), it just finds the next available timer cycle on which to output the pulse. I imagine this shouldn't be too much of an issue though.

Anyway, here's the code for the Oneshot125 on the Due:

Code: [Select]
// Enable Oneshot125 on four channels (4 to 7)
volatile uint16_t motorOutput[] = { 0, 0, 0, 0 };                                 // Motor outputs

void setup() {
  // PWM set-up on pins D9, D8, D7 and D6 for channels 4 through to 7 respectively
  PMC->PMC_PCER1 |= PMC_PCER1_PID36;                                              // Enable PWM
  PIOC->PIO_ABSR |= PIO_ABSR_P24 | PIO_ABSR_P23 | PIO_ABSR_P22 | PIO_ABSR_P21;    // Set the port C PWM pins to peripheral type B 
  PIOC->PIO_PDR |= PIO_PDR_P24 | PIO_PDR_P23 | PIO_PDR_P22 | PIO_PDR_P21;         // Set the port C PWM pins to outputs
  PWM->PWM_CLK = PWM_CLK_PREA(0) | PWM_CLK_DIVA(7);                               // Set the PWM clock A rate to 12MHz (84MHz/7)         
 
  for (uint8_t i = 4; i < PWMCH_NUM_NUMBER; i++)                                  // Loop for each PWM channel (4 in total)
  { 
    PWM->PWM_CH_NUM[i].PWM_CMR = PWM_CMR_CPRE_CLKA;                               // Enable single slope PWM and set the clock source as CLKA
    PWM->PWM_CH_NUM[i].PWM_CPRD = 3750;                                           // Set the PWM frequency less than 4kHz to 12MHz/3750 = 3.2kHz
    PWM->PWM_CH_NUM[i].PWM_CDTY = 0;                                              // Set the PWM duty cycle to 0% duty-cycle
  }
  NVIC_SetPriority(PWM_IRQn, 0);               // Set the Nested Vector Interrupt Controller (NVIC) priority for the PWM controller to 0 (highest)
  NVIC_EnableIRQ(PWM_IRQn);                    // Connect PWM Controller to Nested Vector Interrupt Controller (NVIC)
  PWM->PWM_ENA = PWM_ENA_CHID7 | PWM_ENA_CHID6 | PWM_ENA_CHID5 | PWM_ENA_CHID4;   // Enable all four PWM channels
}

void loop()
{
  delay(2);                                                                             // Delay to simulate flight control calculations
 
  // Note that 1500 below is your standard ESC PWM value 1000us - min, 1500us - half and 2000us - max throttle
  // Multiplied by 1.5 to account for the 12MHz timer clock, (the Due is unable to generate the 8MHz clock we require)
  // Change your motor output here ...
  motorOutput[0] = (uint16_t)(1000 * 1.5f);
  motorOutput[1] = (uint16_t)(1500 * 1.5f);
  motorOutput[2] = (uint16_t)(2000 * 1.5f);
  motorOutput[3] = (uint16_t)(1000 * 1.5f);
  PWM->PWM_IER1 = PWM_IER1_CHID7 | PWM_IER1_CHID6 | PWM_IER1_CHID5 | PWM_IER1_CHID4;    // Enable update interrupts on all PWM channels
}

void PWM_Handler()                                                // PWM Controller ISR (Interrupt Service Routine)
{
  static uint8_t counter[4];                                      // Channel counter array
  uint32_t status = PWM->PWM_ISR1;                                // Read the interrupt status register - clears interrupt flags
 
  for (uint8_t i = 4; i < PWMCH_NUM_NUMBER; i++)                  // Loop for each PWM channel (4 in total)
  {   
    if (status & PWM_ISR1_CHID0 << i)                             // Check if an compare match condition has occured on each channel
    {   
      if (counter[i - 4] == 0)                                    // Check if the channel counter has been reset
      {
        counter[i - 4]++;                                         // Increment the channel counter
        PWM->PWM_CH_NUM[i].PWM_CDTYUPD = motorOutput[i - 4];      // Set output to the pulse width for the channel
      }
      else
      {
        counter[i - 4] = 0;                                       // Reset the channel counter
        PWM->PWM_CH_NUM[i].PWM_CDTYUPD = 0;                       // Set output to 0V for the channel
        PWM->PWM_IDR1 = PWM_IDR1_CHID0 << i;                      // Disable compare update interrupt for the channel
      }
    }
  }
}

MartinL

#8
May 17, 2018, 11:08 am Last Edit: May 17, 2018, 11:46 am by MartinL
With regard to the servo library, this operates at 50Hz and doesn't work too well with multi-rotor ESCs, which in the past have traditionally used a standard 200Hz to 490Hz PWM signal. Although this has now been superseded, at least for racing quads, by Oneshot, Multishot and currently DShot.

Below is the code that outputs 490Hz PWM on all 8 of the PWM Controller's outputs. As you can see, the code is somewhat simpler that the Oneshot125 implementation above and fits better with the way the Due was intended to operate:

Code: [Select]
// Enable dual slope, 11-bit resolution PWM at 490Hz on 8 channels
void setup() {
  // PWM set-up on pins DAC1, A8, A9, A10, D9, D8, D7 and D6 for channels 0 through to 7 respectively
  PMC->PMC_PCER1 |= PMC_PCER1_PID36;                                               // Enable PWM
  PIOB->PIO_ABSR |= PIO_ABSR_P19 | PIO_ABSR_P18 | PIO_ABSR_P17 | PIO_ABSR_P16;     // Set the port B PWM pins to peripheral type B
  PIOC->PIO_ABSR |= PIO_ABSR_P24 | PIO_ABSR_P23 | PIO_ABSR_P22 | PIO_ABSR_P21;     // Set the port C PWM pins to peripheral type B
  PIOB->PIO_PDR |= PIO_PDR_P19 | PIO_PDR_P18 | PIO_PDR_P17 | PIO_PDR_P16;          // Set the port B PWM pins to outputs
  PIOC->PIO_PDR |= PIO_PDR_P24 | PIO_PDR_P23 | PIO_PDR_P22 | PIO_PDR_P21;          // Set the port C PWM pins to outputs
  PWM->PWM_CLK = PWM_CLK_PREA(0) | PWM_CLK_DIVA(42);                               // Set the PWM clock A rate to 2MHz (84MHz/42)
 
  for (uint8_t i = 0; i < PWMCH_NUM_NUMBER; i++)                       // Loop for each PWM channel (8 in total)
  {
    PWM->PWM_CH_NUM[i].PWM_CMR = PWM_CMR_CALG | PWM_CMR_CPRE_CLKA;     // Enable dual slope PWM and set the clock source as CLKA
    PWM->PWM_CH_NUM[i].PWM_CPRD = 2040;                                // Set the PWM frequency 2MHz/(2 * 2040) = 490Hz;
  }
  PWM->PWM_ENA = PWM_ENA_CHID7 | PWM_ENA_CHID6 | PWM_ENA_CHID5 | PWM_ENA_CHID4 |  // Enable all PWM channels
                 PWM_ENA_CHID3 | PWM_ENA_CHID2 | PWM_ENA_CHID1 | PWM_ENA_CHID0;
}

void loop()
{
  delay(2);                                                // Delay to simulate flight controller calculations
  for (uint8_t i = 0; i < PWMCH_NUM_NUMBER; i++)           // Loop for each PWM channel (8 in total)
  {
    PWM->PWM_CH_NUM[i].PWM_CDTYUPD = 1000;                 // Set the PWM duty cycle to 1000 min throttle, 2000 max throttle
  }
}

Just load the duty cycle update registers (CDTYUPD) with the throttle value: 0 = off, 1000 = standby, 1500 = mid throttle, 2000 = max throttle.

Markishere123

Hi MartinL,
Thank you very much for both codes, I will soon be experimenting with the Oneshot125 code. I have been able to change the header files for the servo library to lower the wait between updating new values and thereby increasing from 50HZ to about 300HZ and after experimenting and researching I also changed values in the MPU6050 header files to increase gyroscope collection rate. My code currently runs at about 166 times a second and I am getting pretty good stability. I will soon be trying the Oneshot125 protocol also.
Thank you for your help!

MartinL

#10
Today at 12:52 pm Last Edit: Today at 12:53 pm by MartinL
Hi Markishere123,

If you're using the Due soley for the purpose of basic piloted flight control, then I'd say that 166 times a second is somewhat slow. For a high performance micro-controller like the Due, I'd be expecting looptimes in excess of 1000 times per second.

Have you set the I2C Wire library to operate in fast mode at 400kHz for the MPU6050 communication?

Go Up