Read RC receiver - Futaba Multi-prop 4+4 - Servo jitter issue;

BogdanM · February 22, 2021, 7:50am

Dear All,

Its related to the topic opened on "Project Guidance", here. Since then, we come up with a pretty good setup and code but I am facing right now a very annoying issue, a small servo jitter which might be related with something in the code. This issue puts unnecessary load on the battery and has to be resolved one way or another.

I am currently working on a RC submarine project and I started with the most difficult task, reading the RC receiver for a Futaba F-14 transmitter. Let me summarize:

Latest Code(github), here.

Issue: Servo Jitter when using one of the 4 props with a servo, VarSpeedServo.h library is being used.

Facts:

Its not a power issue, servo is powered from a external power supply (5.42v) and has a ferrite core on the cable, common ground with Arduino.
Jitter is not happening when the transmitter is powered off.
Jitter is accentuated when using map function to convert us to angle but when using writeMicroseconds method, jitter is minimal but still exists.

servos[outputnum].write( map(nonISR_output[outputnum],1020,1980,5,175), SPEED);

This map function and speed was introduced to prevent servo jerky movement.

Hardware:

Robbe Futaba F14 transmitter;
Robbe Futaba FP-R118F receiver;
Futaba Multiprop 4+4(8039);
Arduino Nano V3 with Funduino I/O extension board;
Futaba S3003 servo (specs here)

Questions:

Is the code optimized?
Shall I change the approach when reading and writing the pulse width? Maybe using timer interrupts?

Please help, I run out of ideas.

Thank you.

Bogdan

Multiprop4+4.png

BogdanM · February 22, 2021, 8:03am

#include <VarSpeedServo.h>
#include "ThreeStateDoubleSwitch.h"

#define NUM_OUTPUTS         8
#define NUM_SERVOS          4
#define NUM_SWITCHES        4

// ISR variables
byte last_channel_1;
unsigned long timer_1, current_time, receiver_input;
volatile uint8_t current_output = 0;
volatile uint8_t output_ready = 0;

volatile int multi_output[NUM_OUTPUTS] = {0,}; // Array that holds effective RC reading from multiprop;
int nonISR_output[NUM_OUTPUTS] = {0,};        // Array that holds a copy multi_output values, outside ISR 

uint8_t outputA_pins[NUM_OUTPUTS];
uint8_t outputB_pins[NUM_OUTPUTS];

uint8_t SPEED = 28; // servo speed for speed library, value from 0 to 255;

//Servo servos[NUM_SERVOS]; // only 4 servo, 1 per each prop, only one servo used in my experiment

VarSpeedServo servos[NUM_SERVOS];

ThreeStateDoubleSwitch switches[NUM_SWITCHES];

void setup()
{

 //   Serial.begin(115200); 

      PCICR |= (1 << PCIE0);               //Set PCIE0 to enable PCMSK0 scan.
      PCMSK0 |= (1 << PCINT0);             //Set PCINT0 (digital input 8) to trigger an interrupt on state change.


    // Init array of output pins
    // 1-4 - Props
    // 5-8 - 3 Position Switches

    // Primary function array, includes both switched and props

    outputA_pins[0] = 6;    // Prop 1
    outputA_pins[1] = 0;    // Prop 2
    outputA_pins[2] = 0;    // Prop 3
    outputA_pins[3] = 0;    // Prop 4
    outputA_pins[4] = 7;    // Switch 1
    outputA_pins[5] = 0;    // Switch 2
    outputA_pins[6] = 0;    // Switch 3
    outputA_pins[7] = 0;    // Switch 4

    // Secondary function array, includes only switches

    outputB_pins[0] = 0;    // Prop 1 - not used, reserved for 2 switching function
    outputB_pins[1] = 0;    // Prop 2 - not used, reserved for 2 switching function
    outputB_pins[2] = 0;    // Prop 3 - not used, reserved for 2 switching function
    outputB_pins[3] = 0;    // Prop 4 - not used, reserved for 2 switching function
    outputB_pins[4] = 9;    // Switch 1
    outputB_pins[5] = 0;    // Switch 2
    outputB_pins[6] = 0;    // Switch 3
    outputB_pins[7] = 0;    // Switch 4

    // assign output pins

    for( uint8_t outputnum=0; outputnum<NUM_OUTPUTS; outputnum++ )
    {
        if( outputA_pins[outputnum] != 0 )
        {
            pinMode( outputA_pins[outputnum], OUTPUT );
            pinMode( outputB_pins[outputnum], OUTPUT );

        }//if

    }//for

    //set up the array of switch and servo objects, and init switch to LOW and servo to a "centered" position

    for( uint8_t outputnum=0; outputnum<NUM_OUTPUTS; outputnum++ )
    {
        if( outputnum > 3 )
        {
            //non-servo outputs
            if( outputA_pins[outputnum] != 0 )
                digitalWrite( outputA_pins[outputnum], LOW );
            if( outputB_pins[outputnum] != 0 )
                digitalWrite( outputB_pins[outputnum], LOW );

        }//if
        else
        {
            //servo outputs
            multi_output[outputnum] = 1500;
            if( outputA_pins[outputnum] != 0 )
            {
                servos[outputnum].attach( outputA_pins[outputnum], 1020,1980 );
                servos[outputnum].write( map(multi_output[outputnum],1020,1980,5,175),SPEED );

            }//if

        }//else

    }//for


}//setup

void loop()
{

/*
if (output_ready == 1) {
  
for (outputnum = 0; outputnum < 8; outputnum++) {

  Serial.print(outputnum);
  Serial.print("/");
  Serial.print(multi_output[outputnum]);
  Serial.print(" ");
  }
Serial.println();
delay(250);
} 
*/
    //if the interrupt routine flag is set, we have a set of servos to move
    
    if( output_ready == 1 )
    {
      
       noInterrupts();
        // copy ISR_accessed array to working, non-ISR array in protected section
        for( uint8_t outputnum=0; outputnum<8; outputnum++ ){
       
          nonISR_output[outputnum] = multi_output[outputnum];
          
        }//for
      interrupts();

        for (uint8_t outputnum = 0; outputnum < 8; outputnum++)
        {

         
           switch( outputnum )
            {
                case    0:

                    // I only use one Prop to drive a servo, position 1
                    servos[outputnum].write( map(nonISR_output[outputnum],1020,1980,5,175), SPEED);
                    //servos[outputnum].writeMicroseconds(nonISR_output[outputnum]);
                   
                    break;


                //case    1:
                //case    2:
                //case    3:

                case    4:

                    // I only use one switch to light two distinct led circuits, position 5
                    switches[outputnum-4].computeNewState(nonISR_output[outputnum]);
                    
                    //check the on/off state and act
                    digitalWrite(outputA_pins[outputnum], switches[outputnum-4].isUpperSwitchOn() ? HIGH:LOW );
                    digitalWrite(outputB_pins[outputnum], switches[outputnum-4].isLowerSwitchOn() ? HIGH:LOW );
                    
                    break;

                //case    5: 
                //case    6:

                case    7:
                
                    if( (nonISR_output[outputnum] >= 1020) && (nonISR_output[outputnum] <= 1070) )
                    {
                        digitalWrite( outputA_pins[outputnum], HIGH );

                    }//if

                    if( (nonISR_output[outputnum] >= 1940) && (nonISR_output[outputnum] <= 1980) )
                    {
                        digitalWrite( outputB_pins[outputnum], HIGH );

                    }//if
                    if( (nonISR_output[outputnum] >= 1480) && (nonISR_output[outputnum] <= 1540) )
                    {
                        digitalWrite( outputA_pins[outputnum], LOW );
                        digitalWrite( outputB_pins[outputnum], LOW );

                    }//if
                    
                    break;
                    
            }//switch

        }//for

    }//if

}//loop


ISR(PCINT0_vect){
  
  current_time = micros();
  
  //Receiver Channel 6 to D8 
    
  if(PINB & B00000001){                                                     //Is input 8 high?
    if(last_channel_1 == 0){                                                //Input 8 changed from 0 to 1.
      last_channel_1 = 1;                                                   //Remember current input state.
      timer_1 = current_time;                                               //Set timer_1 to current_time.
    }
  }
  else if(last_channel_1 == 1){                                             //Input 8 is not high and changed from 1 to 0.
    last_channel_1 = 0;                                                     //Remember current input state.
    receiver_input = current_time - timer_1;                                //Channel 6 is current_time - timer_1.

      if ((receiver_input > 850)&&(receiver_input <950)){
          current_output = 0;
      
         }
          else {

                if (current_output <8){
          
                  multi_output[current_output]= receiver_input;
                  current_output++;
                  output_ready = 1;
                 } 
    
          }
    }
}

Posted also the code here.

In the absence of an oscilloscope, I run Nick Gammon's(Gammon Forum : Electronics : Microprocessors : Timers and counters) codes("Measuring a duty cycle using the input capture unit") which returned the following results:

signal train pulse duration and frequency:

20:54:25.614 -> Took: 931.69 uS. - Sync
20:54:26.125 -> Took: 1518.56 uS.
20:54:26.639 -> Took: 1059.00 uS.
20:54:27.117 -> Took: 1519.38 uS.
20:54:27.630 -> Took: 1058.94 uS.
20:54:28.145 -> Took: 1518.69 uS.
20:54:28.658 -> Took: 1058.94 uS.
20:54:29.169 -> Took: 1518.38 uS.
20:54:29.647 -> Took: 1058.69 uS.

21:01:12.297 -> Took: 351478 counts. Frequency: 45.52 Hz.
21:01:12.842 -> Took: 351475 counts. Frequency: 45.52 Hz.
21:01:13.350 -> Took: 351477 counts. Frequency: 45.52 Hz.
21:01:13.899 -> Took: 351470 counts. Frequency: 45.52 Hz.
21:01:14.411 -> Took: 351475 counts. Frequency: 45.52 Hz.
21:01:14.956 -> Took: 351479 counts. Frequency: 45.52 Hz.
21:01:15.469 -> Took: 351474 counts. Frequency: 45.52 Hz.
21:01:16.018 -> Took: 351485 counts. Frequency: 45.52 Hz.
21:01:16.532 -> Took: 351477 counts. Frequency: 45.52 Hz.

according the above data, servo(sub- channel) is receiving data only 5(45/9 pulses including the sync one) times per second, hence the need to slow down the servo speed in order to prevent the jerky move.

wildbill · February 22, 2021, 12:58pm

As I said in your other thread, store the mapped value that you're going to set to the servo rather than sending it blindly. Don't set it if it has't changed much.

Also, attach the servo once, not every time the reading changes.

BogdanM · February 22, 2021, 5:23pm

Hi Bill, the did follow your recommendation but I had no positive result, I have increased the value up to 100, its something else. The below piece of code was tried and then dropped.

 noInterrupts();
        // copy ISR_accessed array to working, non-ISR array in protected section
        for( uint8_t idx=0; idx<8; idx++ ){
             comp_output[idx] = nonISR_output[idx];
           nonISR_output[idx] = multi_output[idx];
        }
        interrupts();

if (nonISR_output[outputnum]-comp_output[outputnum]>20 || nonISR_output[outputnum]-comp_output[outputnum]<-20) {
                
                    // I only use one Prop to drive a servo, position 1
                    servos[outputnum].write( map(nonISR_output[outputnum],1020,1980,5,175), 30,false); 
                    }

                    else {

                   // I only use one Prop to drive a servo, position 1
                    servos[outputnum].write( map(comp_output[outputnum],1020,1980,5,175),30,false);
                    }
                    
                    break;

What do you mean by attaching the servo every time? Its attached only once in the setup section.

Thank you.

Bogdan

wildbill · February 22, 2021, 6:37pm

You didn't try what I suggested, but perhaps there is another problem.

Maybe run the same code and ignore what you get from the multi prop. Just send the servo to 45 every time instead of using the map. Does it still jitter?

Also, what does this servo control?

BogdanM · February 23, 2021, 8:50am

Hi, I did send the 45 angle value to the servo every time I get pulse for that sub-channel , jitter is still there, more accentuated with varspeedservo library than with the standard servo library.

I am thinking of using that servo to move the antenna in the conning tower.

wildbill · February 23, 2021, 12:54pm

I think you said that jitter goes away if you turn the multi prop transmitter off.

The servo library uses timer interrupts to do its job. I expect varspeedservo does too. I suspect that your interrupt code to read the multi prop and the servo library code are interfering with each other.

I think servos need to be regularly fed their pulse or they won't keep position. If you miss a pulse because you were reading from the multi prop with interrupts disabled, perhaps that is enough to cause jitter. In fact, you may not even miss a pulse, just delay it; not sure what impact that would have.

Further evidence is that varspeedservo is worse. I assume it does more than the regular servo library so perhaps it spends more time in its interrupt routine.

In my view, your interrupt routine is too big and therefore takes longer than it needs to with interrupts disabled. Most of that code could just run in loop. The interrupt needs to catch an edge, capture the time and set a volatile flag that says whether it was rising or falling. All the other work can be done outside.

You might be able to spend your way out of trouble with a faster micro controller.

BogdanM · February 23, 2021, 1:25pm

wildbill:
I think you said that jitter goes away if you turn the multi prop transmitter off.

"Yes."

The servo library uses timer interrupts to do its job. I expect varspeedservo does too. I suspect that your interrupt code to read the multi prop and the servo library code are interfering with each other.

I think servos need to be regularly fed their pulse or they won't keep position. If you miss a pulse because you were reading from the multi prop with interrupts disabled, perhaps that is enough to cause jitter. In fact, you may not even miss a pulse, just delay it; not sure what impact that would have.

Further evidence is that varspeedservo is worse. I assume it does more than the regular servo library so perhaps it spends more time in its interrupt routine.

In my view, your interrupt routine is too big and therefore takes longer than it needs to with interrupts disabled. Most of that code could just run in loop. The interrupt needs to catch an edge, capture the time and set a volatile flag that says whether it was rising or falling. All the other work can be done outside.

"This is as much I as could, I even used port manipulation and get rid of the enableinterrupt library ."
ISR(PCINT0_vect){

current_time = micros();

//Receiver Channel 6 to D8

if(PINB & B00000001){ //Is input 8 high?
if(last_channel_1 == 0){ //Input 8 changed from 0 to 1.
last_channel_1 = 1; //Remember current input state.
timer_1 = current_time; //Set timer_1 to current_time.
}
}
else if(last_channel_1 == 1){ //Input 8 is not high and changed from 1 to 0.
last_channel_1 = 0; //Remember current input state.
receiver_input = current_time - timer_1; //Channel 6 is current_time - timer_1.

if ((receiver_input > 850)&&(receiver_input <950)){
current_output = 0;

}
else {

if (current_output <8){

multi_output[current_output]= receiver_input;
current_output++;
output_ready = 1;
}

}
}
}





You might be able to spend your way out of trouble with a faster micro controller.

"Do you have any suggestions?"

BogdanM · February 23, 2021, 1:31pm

#define ppmPin 2

const int channelAmount = 8;
volatile unsigned long rawValues[channelAmount];
uint16_t pulse = 0;
volatile byte pulseCounter = 0;


void setup_timer1() {
  //WGM -> Fast PWM
  //Prescaler -> 1/8
  //Compare Output Mode - > non-inverting mode
  //Input capture interrupt enable
  TIMSK1 &= ~( _BV(TOIE1) | _BV(ICIE1) | _BV(OCIE1A) | _BV(OCIE1B));
  TCCR1B &= ~(_BV(ICNC1));
  TCCR1A |= _BV(WGM11) | _BV(WGM10) | _BV(COM1A1) | _BV(COM1B1);
  TCCR1B |= _BV(WGM12) | _BV(WGM13) | _BV(ICES1);
  TCCR1B |= _BV(CS11);
  TCCR1B &= ~( _BV(CS12) | _BV(CS10) );
  TCCR1A &= ~( _BV(COM1A0) | _BV(COM1B0));
  TIMSK1 |= (1 << ICIE1);
  OCR1A = 0xFFFF;
  OCR1B = 0x0;
}

void setup()
{
  setup_timer1();
  Serial.begin(115200);
}

void loop()
{

  for (int i = 0; i < 8; i++)   //display all channels
  {
    Serial.print((rawValues[i] / 2));
    Serial.print("  ");
  }
  Serial.println();
}


ISR(TIMER1_CAPT_vect) {

  // ICR1 - time ISR Triggered
  static unsigned int lastICR; // Input Capture Register - icr at last caputre
  uint16_t pulse;

  pulse = ICR1 - lastICR;
  lastICR = ICR1;

  if (pulse > 5000)
  { //pulse too long, means start of new frame
    pulseCounter = 0;
  }
  else if (pulse > 1000)
  {
    //pulse good, take reading, go to next channel
    if (abs((rawValues[pulseCounter] - pulse)) > 3)
    {
      rawValues[pulseCounter] = pulse;
    }
    if ( pulseCounter < 8)
    {
      pulseCounter++;
    }
  }
  else {
    //too short, nothing to see here
  }

}

Hi, can someone explain me what this code does? Can I adapt it to my needs? Try it, not working...Dead!

wildbill · February 23, 2021, 1:40pm

You're missing the point I think. Your interrupt routine is doing a bunch of necessary calculations, but you don't need to do them there. If you know that there was an interrupt and when and whether it was rising or falling, you can do all the other calcs outside.

I don't think that the timer route you've taken is necessary, plain old attachInterrupt would do the job - if the interrupt routine is reduced to the absolute minimum required.

As to other processors, the Teensy range is fast, as are some of the Adafruit offerings. Note that most of them are 3V3, although some are 5V tolerant and I think the older Teensys may be 5V and still blazingly fast compared to a nano.

BogdanM · February 25, 2021, 11:34am

Wildbill, like this?

External Interrupts
volatile int pwm_value = 0;
volatile int prev_time = 0;
 
void setup() {
  Serial.begin(115200);
  // when pin D2 goes high, call the rising function
  attachInterrupt(0, rising, RISING);
}
 
void loop() { }
 
void rising() {
  attachInterrupt(0, falling, FALLING);
  prev_time = micros();
}
 
void falling() {
  attachInterrupt(0, rising, RISING);
  pwm_value = micros()-prev_time;
  Serial.println(pwm_value);

I will ajust the code and give it a try.

I am a bit reluctant also with this approach, a friend said that the jitter might be from the millis() interrupt and my approach needs to change.

Thank you.
Bogdan

wildbill · February 25, 2021, 12:06pm

It's not quite what I had in mind - that flipping the attached interrupt routine is a bit unorthodox, but it looks plausible. Certainly those functions are nice & short now - try it.

Even at that baud rate though, you may find that serial printing every reading causes some interference. Note too that anything you try to print in an interrupt will simply be queued until interrupts are enabled again - not sure it matters here.

Another thing to consider is that maybe you can just use the rising edge. If the 20mS gap between pulses is accurate, you can just subtract it from the difference between them to get the reading you need.

system · June 25, 2021, 12:07pm

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.