Problem due to glitches with servos and refreshing rate

system · July 24, 2013, 7:59pm

Hi everybody.
Maybe it could be an annoying question but really I searched in the forum before posting and I didnt find any answer
Ok, let s start: for a RC Plane I have developed a small autopilot with Arduino Nano 3.0 and one Spektrum RX.
The software runs very well and the values from the RX are correctly received, here an example:
throttle, aileron, elevator, rudder

936,1448,1444,1624
936,1448,1448,1628
936,1448,1448,1628
936,1448,1448,1628
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1464,1624
936,1444,1464,1624
936,1444,1448,1628
936,1444,1448,1628
936,1444,1444,1624
936,1444,1444,1624
936,1444,1444,1624
936,1444,1444,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1460,1624
936,1444,1460,1624
936,1444,1460,1624
936,1440,1444,1624
936,1440,1444,1624
936,1444,1444,1624
936,1444,1444,1624
936,1448,1448,1624
936,1448,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1448,1448,1624
936,1448,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
932,1448,1444,1624
932,1448,1444,1624
936,1444,1448,1608
936,1444,1448,1608
936,1444,1444,1624
936,1444,1444,1624
932,1448,1444,1624
932,1448,1444,1624
952,1444,1448,1624
952,1444,1448,1624
936,1448,1448,1624
936,1448,1448,1624
936,1448,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1444,1624
936,1444,1444,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1444,1624
936,1444,1444,1624
936,1448,1448,1624
936,1448,1448,1624
940,1440,1460,1624
940,1440,1460,1624
936,1444,1444,1628
936,1444,1444,1628
936,1444,1444,1628
936,1444,1444,1628
936,1444,1448,1624
936,1444,1448,1624
936,1448,1448,1624
936,1448,1448,1624
936,1448,1448,1624
940,1448,1444,1628
940,1448,1444,1628
936,1444,1448,1624
936,1444,1448,1624
936,1448,1444,1628
936,1448,1444,1628
936,1448,1448,1624
936,1448,1448,1624
936,1444,1448,1616
936,1444,1448,1616
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
944,1444,1448,1624
944,1444,1448,1624
952,1448,1448,1624
952,1448,1448,1624
936,1448,1444,1624
936,1448,1444,1624
936,1448,1444,1624
936,1448,1448,1624
936,1448,1448,1624
936,1444,1448,1624
936,1444,1448,1624
940,1444,1444,1624
940,1444,1444,1624
940,1444,1444,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1444,1624
936,1444,1444,1624
936,1444,1448,1624
936,1444,1448,1624
936,1444,1460,1624
936,1444,1460,1624
936,1436,1448,1624
936,1436,1448,1624
936,1436,1448,1624
936,1444,1444,1624
936,1444,1444,1624
936,1448,1448,1624
936,1448,1448,1624
936,1448,1448,1624
936,1448,1448,1624

so I get reasonable values.
But when I send those data to the servos I experience something like jitter or glitches with the servos. As you can see here is the routine and they are refreshed every 20 ms.

#include <avr/interrupt.h>
#include <avr/io.h>
#include <Servo.h>

#include <FreeRTOS_AVR.h>

#include "define_AP6.h"

/***************************************************************
* Variablen, die durch den ausgeführten Interrupt gelesen werden
***************************************************************/
volatile static unsigned long cnt;
volatile static byte PIND_SHADOW;

/****************************
* Data storage from the Radio
****************************/
struct fifoItem {
  unsigned long throttlePulse;
  unsigned long throttlePulseTemp;
  unsigned long ailerPulse;
  unsigned long ailerPulseTemp;
  unsigned long elevPulse;
  unsigned long elevPulseTemp;
  unsigned long rudderPulse;
  unsigned long rudderPulseTemp;
  unsigned long ldgPulse;
  unsigned long ldgPulseTemp;
  bool thrFlag;
  bool ailFlag;
  bool elvFlag;
  bool rudFlag;
  bool ldgFlag;
};

// Array for the data incoming from the radio
fifoItem fifoArray[FIFO_SIZE];

/****************************
* Data storage from the Radio
****************************/
struct servoItem {
  unsigned int throttle;
  unsigned int aileron;
  unsigned int elevator;
  unsigned int rudder;
} dataServo;

/********************
* Data for the servos
********************/
Servo throttleServo, aileronServo, elevatorServo, rudderServo;

/*******************************
* Pin definitions for the servos
*******************************/
const int throttleServoPin = 8;
const int aileronServoPin = 9;
const int elevatorServoPin = 10;
const int rudderServoPin = 11;

/***************************************************************
* Semaphore definitions
***************************************************************/
xSemaphoreHandle interrupt;  // semaphore from the interrupt routine
xSemaphoreHandle fifoData;   // semaphore for counting data storage in the array
xSemaphoreHandle fifoSpace;  // counter of free buffers in FIFO

//###########################################################################################
static void vTaskRadio( void *pvParameters );
static void vTaskSendData( void *pvParameters );
static void vTaskServo( void *pvParameters );
//###########################################################################################
/**************
*
* Initial Setup
*
**************/
void setup(){
   
   Serial.begin( BAUDRATE );
   Serial.flush();
   
   #if DEBUGLEVEL
   delay( 1000 );
   Serial.println( F(" frAP6_Nano 3.0") );
   delay( 1000 );
   #endif

   set_interrupt();

   /******************
   * Servo definition
   ******************/
   throttleServo.attach( throttleServoPin );
   aileronServo.attach( aileronServoPin );
   elevatorServo.attach( elevatorServoPin );
   rudderServo.attach( rudderServoPin );

   /* Create the Semaphore and variable for tasks */
   vSemaphoreCreateBinary( interrupt );
   fifoData = xSemaphoreCreateCounting( FIFO_SIZE, 0 );
   fifoSpace = xSemaphoreCreateCounting( FIFO_SIZE, FIFO_SIZE );

   portBASE_TYPE s1, s2, s3;

   /* Check if the semaphore has been created */
   if( interrupt == NULL || fifoData == NULL || fifoSpace == NULL ) {

    Serial.println( F("Cannot create the semaphore") );
   }

   s1 = xTaskCreate( vTaskRadio, 
                     (signed char *)"T1", 
                     configMINIMAL_STACK_SIZE + 210, 
                     NULL, 
                     3, 
                     NULL );
   s2 = xTaskCreate( vTaskSendData, 
                     (signed char *)"T2", 
                     configMINIMAL_STACK_SIZE + 20, 
                     NULL, 
                     1, 
                     NULL );
   s3 = xTaskCreate( vTaskServo,
                     (signed char *)"T3",
                     configMINIMAL_STACK_SIZE + 50,
                     NULL,
                     2,
                     NULL );

   /* Check Tasks creations */
   if( s1 != pdPASS || s2 != pdPASS || s3 != pdPASS ) {
    Serial.println( F("Creation problem") );
    Serial.print( s1 );
    Serial.print( ',' );
    Serial.print( s2 );
    Serial.print( ',' );
    Serial.print( s3 );
    while(1);
   }

   Serial.print( F("Tasks created...") );
   Serial.println( F("OK") );
   /* Start activities... */
   vTaskStartScheduler();
   Serial.println( F("Insufficient RAM") );
   while(1);
}
void loop(){
  /* Nothing here */
}


void set_interrupt(void){
  
   EICRA = 0x00;
   
   DDRD &= ~((1 << PIND2));   // Input Pin - Es werden nur die ersten D2 und D3 als Interrupt verwendet 
   PORTB &= ~((1 << PIND2));  // Pull - up disabled
   
   // Configure external interrupts on PCINT2 (from PD0 to PD7)
   PCMSK2 = 0;                 // Disable interrupts on over a single port
   PCICR |= (1 << PCIE2);      // Enabled interrupts on PCINT23-16
   PCMSK2 |= ((1 << PCINT21)); // At the moment only PD2 enabled

}

and here the ISR routine

ISR(PCINT2_vect){
  
  cnt = micros();
  PIND_SHADOW = PIND;
  
  /* Give the semaphore to unblock the task */
  xSemaphoreGiveFromISR( interrupt, NULL );

}

and here the tasks definition:

system · July 24, 2013, 7:59pm

//------------------------------------------------------------------------------
/*
 * Thread 1, read data from the RC-receiver
 */
// Declare the thread function for thread 1.
static void vTaskRadio( void *pvParameters ) {

  // index of record to be filled
  size_t fifoHead = 0;
  // local register for bitwise operations
  byte REG;
  boolean send = false;

  /* Take the semaphore once before starting the loop
  The reason is that at the very beginning the task must be put in sleep
  mode to wait the interrupt routine */
  xSemaphoreTake( interrupt, 0 );

  while(1){
    /* Reread the semaphore, it should be empty. Then wait for a while
    The Interrupt will ripristinate you filling the semaphore */
  	xSemaphoreTake( interrupt, portMAX_DELAY );
  	
    /* Structure to hold all information about the lenght of the pulses 
    from the radio */
    fifoItem *radioPulse = &fifoArray[fifoHead];

    /* Since PORTD is shared with the serial PIND0 and PIND1 the first 2 bits
  	must be masked to be used in the following switch statement */
    REG = PIND_SHADOW & 0b11111100;

    switch( PCMSK2 ){

      case 0x04 : // 0b00000100  ( PCINT18 active!!!)
           if( ((REG & (1<< PIND2)) == 0b00000100) && !(radioPulse->ldgFlag) ) {
                radioPulse->ldgFlag = 1;
                radioPulse->ldgPulseTemp = cnt;
                break;
              }
           if( ((REG & (1<< PIND2)) == 0b00000000) && (radioPulse->ldgFlag) ) {

                radioPulse->ldgPulse = cnt - ( radioPulse->ldgPulseTemp );
                radioPulse->ldgFlag = 0;
                /* Change the Interrupt on the next pin */
                PCMSK2 &= ~( 1 << PCINT18 );    // Disable interrupts on this pin
                PCMSK2 |= ( 1 << PCINT22);     // Enable the interrupts for the next channel
                break;
              }

      case 0x08 : // 0b00001000  ( PCINT19 active!!! )
            if( ((REG & (1<< PIND3)) == 0b00001000) && !(radioPulse->rudFlag) ) {
                radioPulse->rudFlag = 1;
                radioPulse->rudderPulseTemp = cnt;
                break;
              }
           if( ((REG & (1<< PIND3)) == 0b00000000) && (radioPulse->rudFlag) ) {
                radioPulse->rudderPulse = cnt - (radioPulse->rudderPulseTemp);
                radioPulse->rudFlag = 0;
                /* Change the Interrupt on the next pin */
                PCMSK2 &= ~(1 << PCINT19);    // Disable interrupts on this pin
                PCMSK2 |= (1 << PCINT21);     // Enable the interrupts for the next channel
                send = true;
                break;
              }

      case 0x10 : // 0b00010000  ( PCINT20 active!!! )
            if( ((REG & (1<< PIND4)) == 0b00010000) && !(radioPulse->elvFlag) ) {
                radioPulse->elvFlag = 1;
                radioPulse->elevPulseTemp = cnt;
                break;
              }
           if( ((REG & (1<< PIND4)) == 0b00000000) && (radioPulse->elvFlag) ) {
                radioPulse->elevPulse = cnt - (radioPulse->elevPulseTemp);
                radioPulse->elvFlag = 0;
                /* Change the Interrupt on the next pin */
                PCMSK2 &= ~(1 << PCINT20);    // Disable interrupts on this pin
                PCMSK2 |= (1 << PCINT19);     // Enable the interrupts for the next channel
                break;
              }

      case 0x20 : // 0b00100000  ( PCINT21 active!!! )
            if( ((REG & (1<< PIND5)) == 0b00100000) && !(radioPulse->ailFlag) ) {
                radioPulse->ailFlag = 1;
                radioPulse->ailerPulseTemp = cnt;
                break;
              }
           if( ((REG & (1<< PIND5)) == 0b00000000) && (radioPulse->ailFlag) ) {
                radioPulse->ailerPulse = cnt - (radioPulse->ailerPulseTemp);
                radioPulse->ailFlag = 0;
                /* Change the Interrupt on the next pin */
                PCMSK2 &= ~(1 << PCINT21);    // Disable interrupts on this pin
                PCMSK2 |= (1 << PCINT18);     // Enable the interrupts for the next channel                
                break;
              }          

      case 0x40 : // 0b01000000  ( PCINT22 active!!! )
            if( ((REG & (1<< PIND6)) == 0b01000000) && !(radioPulse->thrFlag) ) {
                radioPulse->thrFlag = 1;
                radioPulse->throttlePulseTemp = cnt;
                break;
              }
           if( ((REG & (1<< PIND6)) == 0b00000000) && (radioPulse->thrFlag) ) {
                radioPulse->throttlePulse = cnt - (radioPulse->throttlePulseTemp);
                radioPulse->thrFlag = 0;
                /* Change the Interrupt on the next pin */
                PCMSK2 &= ~(1 << PCINT22);    // Disable interrupts on this pin
                PCMSK2 |= (1 << PCINT20);     // Enable the interrupts for the next channel
                break;
              }   

      default:
        break;
    }

  if( send == true ) {
    send = false;
    /* get a buffer */
    if( xSemaphoreTake( fifoSpace, 0 ) != pdTRUE ) {
      Serial.println( F("Overrun error in the FIFO ARRAY") );
    }
    /* signal new data available */
    xSemaphoreGive( fifoData );
    // and advance in the FIFO Array
    fifoHead = fifoHead < (FIFO_SIZE - 1) ? fifoHead + 1 : 0;
   }

  }
}
//------------------------------------------------------------------------------
/*
 * Thread 2, prepare data for the servo
 */
// Declare the thread function for thread 2.
static void vTaskSendData( void *pvParameters ) {
  // FIFO index to be written
  size_t fifoTail = 0;

  // struct for data to be send to the servos
  servoItem *ds = &dataServo;

	while(1){

    /* Read the semaphore to see if there are new data available , ready to be
    transmitted */
    xSemaphoreTake( fifoData, portMAX_DELAY );
     
    // declare a new structure (pointer) to read data from the FIFO
    fifoItem *radioPulse = &fifoArray[fifoTail];
    
    // now starts for checking if data are ok and will be filtered
    if( ( radioPulse->throttlePulse > minSignalLength ) &&
        ( radioPulse->throttlePulse < maxSignalLength ) ) {
      ds->throttle = radioPulse->throttlePulse;
    }

    if( ( radioPulse->ailerPulse > minSignalLength ) &&
        ( radioPulse->ailerPulse < maxSignalLength ) ) {
      ds->aileron = radioPulse->ailerPulse;
    }

    if( ( radioPulse->elevPulse > minSignalLength ) &&
        ( radioPulse->elevPulse < maxSignalLength ) ) {
      ds->elevator = radioPulse->elevPulse;
    }

    if( ( radioPulse->rudderPulse > minSignalLength ) &&
        ( radioPulse->rudderPulse < maxSignalLength ) ) {
      ds->rudder = radioPulse->rudderPulse;
    }

    // release record
    xSemaphoreGive( fifoSpace );

    // advance FIFO index
    fifoTail = fifoTail < (FIFO_SIZE - 1) ? fifoTail + 1 : 0;

    }
}
//------------------------------------------------------------------------------
/*
 * Thread 3, send data to the servo
 */
// Declare the thread function for thread 3.
static void vTaskServo( void *pvParameters ) {
  // struct for data to be send to the servos
  servoItem *ds = &dataServo;

  // initialize the data to avoid cold start
  ds->throttle = 0;
  ds->aileron = 1500;
  ds->elevator = 1500;
  ds->rudder = 1500;
  
  portTickType xLastWakeTime;

  /* The xLastWakeTime variable needs to be initialized with the current tick
  count. Note that this is the only time the variable is written to explicitly.
  After this xLastWakeTime is updated automatically internally within
  vTaskDelayUntil(). */
  xLastWakeTime = xTaskGetTickCount();

    while(1){

        throttleServo.writeMicroseconds( ds->throttle );
        aileronServo.writeMicroseconds( ds->aileron );
        elevatorServo.writeMicroseconds( ds->elevator );
        rudderServo.writeMicroseconds( ds->rudder );
        
        /* This task should execute exactly every 20 milliseconds. As per
        the vTaskDelay() function, time is measured in ticks, and the
        portTICK_RATE_MS constant is used to convert milliseconds into ticks.
        xLastWakeTime is automatically updated within vTaskDelayUntil() so is not
        explicitly updated by the task. */
        vTaskDelayUntil( &xLastWakeTime, ( 20 / portTICK_RATE_MS ) );

    }
}

Perhaps you find the freeRTOS part and tasks a little confusing, but it is not. Just keep in mind that the third thread, which sends data to the servos, does it with a 20 ms refreshing rate.

As here explained: Glitches with Arduino and servo control - Programming Questions - Arduino Forum
I tried to run the sketch with the same board for a test and the servos don't show any glitch. So it could not depend on the ground (which is common), on the source (both are powered by the LiPo battery in the model) or the servo.
More...I tried with an logic analyzer to read the output from the Arduino and the pulses are ok. So I think really that the problem is somewehre in the software. I presume the refresh rate is to high, and every time it keeps servo to be updated with a new value.

The model plane flies without problem, even if the servos glitch. Today it was a super day and the plane stayed in the air more then 20 min, but I don t like that servos does glitch or make some strange rumors all the time.
So could you bring me a good tip?

Thanks

3dprinter · July 24, 2013, 8:21pm

OK, I'll admit I have not read/understood the freeRTOS bit (bit it looks nice, must take a closer look another day) but soley looked if you've fallen into a "known limitation" of the servo.h (well, I have not looked if it is fixed in the latest version). And I think you have.

My experience was that on every .write() .writeMicroseconds() the 20 ms timer feeding the servos would "jiggle", and thus the servo got confused for while resulting in uneven movement. This was the case for code that was supposed to slowy/sloothly sweep the servo. So less frequent position change command solved my problem, and I think yours too.

The Servo position does not have to be "refreshed" at all as you seem to be doing. You only input new values into the .writeMicroseconds() method if you do want to move. In most of my code I explicitly check if the new value(.write() is different from the previous one by doing a .read() and skipping the .write() if the value already is there.

Someone (dont have the refernce handy on this machine) wrote a better servo library, ellimintaing this problem, and allowing for slow sweep movements too.

system · July 24, 2013, 9:20pm

I would be moderately skeptical about the RTOS framework you're using, because it has the potential to muck up libraries that are not designed to work in that environment, and anyway it hardly seems necessary for the sort of small application that would be suitable to run on a typical Arduino.

I wouldn't have expected the Servo library to twitch the servos when the position is updated even if the position is updated very frequently. Just for reassurance, I took a simple 'sweep' sketch and modified it to re-update the servo position every time through loop, which will be far more frequently than every 20 ms. There was no twitching or unusual behaviour.

My guess is your RTOS is mucking up the Servo library and I suggest you get rid of the RTOS.

system · July 24, 2013, 10:13pm

Hi Msquare
Thanks. First of all let me you assure, that FreeRTOS for Arduino is rrally worthy. It is simple awesome for small applications and I cannot imagine an application without using FreeRTOS. At the beginning was a pain, but after I understood RT Operative Systems my code is easier to understand, to check and to perform what I want to do.

Anyway...so my suspiocius are right...the problem is the too high refreshing rate of the servos.
I wrote right now the code with a check routine, to see if datas are different and if yes update the servo. The problem persists. Maybe a little bit better but the servos jitters all the time.
Maybe it is too late and I better sleep now so tomorrow I will get an idea. Anyway I would thank you very much, if you can send me the servo library about aou are talking about. It would be really appreciated.

Thank again

system · July 24, 2013, 10:20pm

PeterH:
I wouldn't have expected the Servo library to twitch the servos when the position is updated even if the position is updated very frequently. Just for reassurance, I took a simple 'sweep' sketch and modified it to re-update the servo position every time through loop, which will be far more frequently than every 20 ms. There was no twitching or unusual behaviour.

My guess is your RTOS is mucking up the Servo library and I suggest you get rid of the RTOS.

Hi Peter.
I really need that library because this is just the beginning of the application. Sensors and GPS are going to be added and regurarly read.
But I think you are right saying that the library could be not designed for that environment.
So I was thinking to buy a servo controller, to be driven via I2C or serial.
What do you think about it?

Regards

system · July 24, 2013, 10:35pm

wilhem:
What do you think about it?

So far nothing you've said gives me any reason to agree that a real time operating system is necessary or appropriate for this problem. I don't know what other sensors you have in mind, but for example a RTOS is completely irrelevant to dealing with a GPS input stream.

system · July 26, 2013, 7:54pm

Hi
Could you please tell me which Servo library are available?
I d like to teast with different library and then try with a servo controller.

Regards

system · July 26, 2013, 10:34pm

You're searching for an Arduino servo library? Where have you looked so far?

Topic		Replies	Views
PWM management General Guidance	5	841	May 5, 2021
Servo motors ticking Robotics	4	1122	May 6, 2021
Problem with servos General Electronics	8	919	July 18, 2021
RC Servo jitter frustration Interfacing	7	4747	May 6, 2021
Help required- Servo Jitter Programming	8	1136	May 6, 2021

Problem due to glitches with servos and refreshing rate

Related topics