Servo Jitter - Idea to Solve

Projects General Guidance
1

I am new to this, so please let me know if I'm really off base here. For the past month, I've been reading and studying other people's code and came up with something that works pretty good; however, I am left with a amount of servo jitter. My project will certainly work as is, but I don't see this jitter in commercially available products like APM, Naze32, etc...so I should be able to figure this out.

I see many people asking about jitter and no real solutions. First, I am using quality parts and if the servos are plugged directly into the receiver, there is no jitter. Second, I am powering the servos and Arduino with a 5V BEC, not through my computer's USB port. Third, my connections are through a breadboard and seem to be good. I suspect a soldered up project will have less jitter, but I'd like to get the software as good as I can get it too.

Servo jitter is noticeably more present when plugged into my computer's USB port while debugging output is being monitored; however a small amount of jitter is still present when debugging code is removed and the Arduino is not connected to the computer.

I am performing the receiver inputs using interrupts. You can see my code in the below post as it won't fit here. My question is, why not read the PWM values once at the beginning of each loop? Then process the signal and output to the servos. Then start over and do it again and again. This way, the interrupts won't affect other parts of the code which I'm thinking is causing the jitter.

1047×785 200 KB

2 
#include <EnableInterrupt.h>
#include <Servo.h>
#include <eRCaGuy_Timer2_Counter.h>

#define SERIAL_SPEED 115200
#define SERIAL_DELAY 4     // If Servos attached, use small value

// *****     Debug Output - If defined, enable Serial Monitor     *****
// *****          EXPECT SERVO JITTER IN DEBUG MODE               *****
#define DEBUG_SETUP         // Must be defined if any of the below are defined 
#define DEBUG_RX            // Debug Output for Receiver Input
// #define DEBUG_SERVO         // Debug Output for Servo Output
// #define DEBUG_MIXING        // Debug Output for Signal Processing

// *****     rx INPUT     *****
#define RX_TOTAL_CHANNELS  6

#define rxThr  0
#define rxAil  1
#define rxEle  2
#define rxRud  3
#define rxAux1 4
#define rxAux2 5

#define rxThr_PIN  2
#define rxAil_PIN  3
#define rxEle_PIN  4
#define rxRud_PIN  5
#define rxAux1_PIN 6
#define rxAux2_PIN 7

// *****     rx Servo Output     *****
#define SERVO_TOTAL_CHANNELS 6
#define dtLowRate 0.25
#define dtHighRate 1.0

#define ltAil 0
#define ltTV  1
#define ltESC 2
#define rtESC 3
#define rtTV  4
#define rtAil 5

#define ltAil_PIN  8
#define ltTV_PIN   9
#define ltESC_PIN 10
#define rtESC_PIN 11
#define rtTV_PIN  12
#define rtAil_PIN 13

// *****     rx INPUT     *****
uint16_t rxPulse[RX_TOTAL_CHANNELS];
uint64_t rxPulseStart[RX_TOTAL_CHANNELS];
volatile uint16_t rxPulseTemp[RX_TOTAL_CHANNELS];

// *****     rx Servo Output     *****
volatile uint32_t ltAilPulse, ltTVPulse, ltESCPulse, rtESCPulse, rtTVPulse, rtAilPulse;
volatile float DTRate;

Servo ServoArray[SERVO_TOTAL_CHANNELS];

// *****     rx INPUTS     *****
void rxReadPulse() {
  noInterrupts();
  memcpy(rxPulse, (const void *)rxPulseTemp, sizeof(rxPulseTemp));
  interrupts();
}

void calc_input(uint8_t channel, uint8_t input_pin) {
  if (digitalRead(input_pin) == HIGH) {
    rxPulseStart[channel] = timer2.get_count()/2;
  } else {
    uint16_t rxPulsTotal = (uint16_t)((timer2.get_count()/2) - rxPulseStart[channel]);
    rxPulseTemp[channel] = rxPulsTotal;
  }
}

void calc_Thr()  {
  calc_input(rxThr, rxThr_PIN);
}
void calc_Ail()  {
  calc_input(rxAil, rxAil_PIN);
}
void calc_Ele()  {
  calc_input(rxEle, rxEle_PIN);
}
void calc_Rud()  {
  calc_input(rxRud, rxRud_PIN);
}
void calc_Aux1() {
  calc_input(rxAux1, rxAux1_PIN);
}
void calc_Aux2() {
  calc_input(rxAux2, rxAux2_PIN);
}


// *****     rx Servo Output     *****
double reverse(double val) {
  return (val - 1500) * -1 + 1500;
}

double add(double val1, double val2) {
  double out = (val1 - 1500 + val2 - 1500) + 1500;
  out = (out < 1000) ? 1000 : out;
  out = (out > 2000) ? 2000 : out;
  return out;
}

void mixElevon(void) {
  ltAilPulse = add(rxPulse[rxAil], rxPulse[rxEle]);
  rtAilPulse = add(rxPulse[rxAil], reverse(rxPulse[rxEle]));
}

void mixThrottle(void) {
  float yaw_pct, thr_pct;
  double upMod, dwnMod;
  uint32_t ltESCTmp, rtESCTmp;

  yaw_pct = ((float)rxPulse[rxRud] - 1500) / (float)500;    // Value Between -1 and 1: 0 for Center, -1 full left, 1 full right
  thr_pct = ((float)rxPulse[rxThr] - 1000) / (float)1000;     // Value Between 0 and 1: .5 for Mid Throttle

  if (yaw_pct < (float)-1.0) { yaw_pct = (float)-1.0; }       // Constrain values between -1.0 and 1.0
    else if (yaw_pct > (float)1.0) { yaw_pct = (float)1.0; }
  if (thr_pct < (float)0.0) { yaw_pct = (float)0.0; }         // Constrain values between 0.0 and 1.0
    else if (thr_pct > (float)1.0) { thr_pct = (float)1.0; }  
  
  dwnMod  = abs(yaw_pct) * (rxPulse[rxThr] - 1000) * DTRate;  // Calculate Rudder Pulse Size Constrained by Amount of Throttle
  upMod   = dwnMod * float(1-thr_pct);                        // Up Modifier has less of an effect as throttle increases.

#ifdef DEBUG_MIXING
  Serial.print("MIXING   - rate: "); Serial.print(DTRate);
  Serial.print("  yawP: "); Serial.print(yaw_pct);
  Serial.print("  thrP: "); Serial.print(thr_pct);  
  Serial.print("  uMod: "); Serial.print(upMod);
  Serial.print("  dMod: "); Serial.println(dwnMod);
#endif

  if (yaw_pct < 0) {  // Rudder Left
    ltESCTmp = reverse(rxPulse[rxThr] - dwnMod);
    ltESCPulse = constrain(ltESCTmp, 988, 2012);
    rtESCTmp = rxPulse[rxThr] + upMod;
    rtESCPulse = constrain(rtESCTmp, 988, 2012);
  }
  else {  // Rudder Right
    ltESCTmp = reverse(rxPulse[rxThr] + upMod);
    ltESCPulse = constrain(ltESCTmp, 988, 2012);
    rtESCTmp = rxPulse[rxThr] - dwnMod;
    rtESCPulse = constrain(rtESCTmp, 988, 2012);
  }
}


// SETUP
void setup() {
  // General Setup
#ifdef DEBUG_SETUP
  Serial.begin(SERIAL_SPEED);
  Serial.println(" ");
  Serial.println("     Copyright (C) 2017 - Jim Lander (jamieFL)");  
  Serial.println("     This program comes with ABSOLUTELY NO WARRANTY.  It is free software: you can redistribute it and/or modify ");
  Serial.println("     it under the terms of the GNU General Public License.  For details, see http://www.gnu.org/licenses/");
  Serial.println(" ");
  delay(2000);
#endif

  timer2.setup(); //this MUST be done before the other Timer2_Counter functions work; Note: since this messes up PWM outputs on pins 3 & 11, as well as 
                  //interferes with the tone() library (http://arduino.cc/en/reference/tone), you can always revert Timer2 back to normal by calling 
                  //timer2.unsetup()

  //rx INPUTS
  pinMode(rxThr_PIN, INPUT);
  pinMode(rxAil_PIN, INPUT);
  pinMode(rxEle_PIN, INPUT);
  pinMode(rxRud_PIN, INPUT);
  pinMode(rxAux1_PIN, INPUT);
  pinMode(rxAux2_PIN, INPUT);

  enableInterrupt(rxThr_PIN, calc_Thr, CHANGE);
  enableInterrupt(rxAil_PIN, calc_Ail, CHANGE);
  enableInterrupt(rxEle_PIN, calc_Ele, CHANGE);
  enableInterrupt(rxRud_PIN, calc_Rud, CHANGE);
  enableInterrupt(rxAux1_PIN, calc_Aux1, CHANGE);
  enableInterrupt(rxAux2_PIN, calc_Aux2, CHANGE);

  // Servo OUTPUT
  ServoArray[ltAil].attach(ltAil_PIN, 900, 2100); //  Left Elevon
  ServoArray[ltTV].attach (ltTV_PIN, 900, 2100);  //  Left Thrust Vector Servo
  ServoArray[ltESC].attach(ltESC_PIN, 900, 2100); //  Left ESC
  ServoArray[rtESC].attach(rtESC_PIN, 900, 2100); //  Right ESC
  ServoArray[rtTV].attach (rtTV_PIN, 900, 2100);  //  Right Thrust Vector Servo
  ServoArray[rtAil].attach(rtAil_PIN, 900, 2100); //  Right Elevon
}


// LOOP
void loop() {

  rxReadPulse();  // get current rx INPUTS

#ifdef DEBUG_RX
  Serial.print("RECEIVER - Thr: "); Serial.print(rxPulse[rxThr]);  // Pring rx INPUTS
  Serial.print("  Ail:  "); Serial.print(rxPulse[rxAil]);
  Serial.print("  Ele:  "); Serial.print(rxPulse[rxEle]);
  Serial.print("  Rud:  "); Serial.print(rxPulse[rxRud]);
  Serial.print("  Aux1: "); Serial.print(rxPulse[rxAux1]);
  Serial.print("  Aux2: "); Serial.println(rxPulse[rxAux2]);
#endif  

  // Perform Elevon, Thust Vectoring, and Differential Thrust Mixing
  if (rxPulse[rxAux1] < 1250) {       // MODE 1
    mixElevon();                      // Elevon Mixing: ON
    rtTVPulse = 1500;                 // Thrust Vecoting: OFF
    ltTVPulse = 1500;
    rtESCPulse = rxPulse[rxThr];      // Differential Thrust: OFF
    ltESCPulse = reverse(rxPulse[rxThr]);
  }
  else if (rxPulse[rxAux1] > 1750) {  // MODE 3
    mixElevon();                      // Elevon Mixing: ON
    rtTVPulse = rtAilPulse;           // Thrust Vectoring: ON
    ltTVPulse = ltAilPulse;
    DTRate = dtHighRate;              // Differential Thrust: ON at HIGHT Rate
    mixThrottle();
  }
  else {                              // MODE 2
    mixElevon();                      // Elevon Mixing: ON
    rtTVPulse = 1500;                 // Thrust Vectoring: OFF
    ltTVPulse = 1500;
    DTRate = dtLowRate;               // Differential Thrust: ON at LOW Rate
    mixThrottle();
  }

  ServoArray[ltAil].writeMicroseconds(ltAilPulse);
  ServoArray[ltTV].writeMicroseconds(ltTVPulse);
  ServoArray[ltESC].writeMicroseconds(ltESCPulse);
  ServoArray[rtESC].writeMicroseconds(rtESCPulse);
  ServoArray[rtTV].writeMicroseconds(rtTVPulse);
  ServoArray[rtAil].writeMicroseconds(rtAilPulse);

#ifdef DEBUG_SERVO
  Serial.print("SERVOS   - ltAl: "); Serial.print(ltAilPulse);  // Print Servo OUTPUTS
  Serial.print("  rtAl: "); Serial.print(rtAilPulse);
  Serial.print("  ltTV: "); Serial.print(ltTVPulse);
  Serial.print("  rtTV: "); Serial.print(rtTVPulse);
  Serial.print("  lESC: "); Serial.print(ltESCPulse);
  Serial.print("  rESC: "); Serial.println(rtESCPulse);

  delay(SERIAL_DELAY);
#endif

}
3

Most likely the servo power supply is inadequate, and you should not power the Arduino with the same power supply.

Servos inject electrical noise into the power lines and can damage the Arduino or cause it to reset.

The servo power supply should provide at least 1 ampere per moving servo. USB ports can't provide even 1 ampere.

4

I am powering the whole thing with a 3 amp 5 volt BEC so my power is adequate. I've not heard that the Arduino should not be powered by the same power source that is supplying the servos. I'll have to test that, but I don't see how this is an issue as commercially produced flight controllers like APM and Naze32 work fine with one power source.

5

Manufacturers understand electronic design principles.

In particular they employ experts at "power supply decoupling", which is absolutely required when you run Arduino and servos from the same power source.

6

I'll look into that.

7

Just a quick test...Powering the Arduino separately has no effect.

I've temporarily removed the 5v BEC from Vin so that the Ardino is only powered through the USB port. The Servos and Receiver are powered only through with the BEC.

I'm getting the same jitter.

8

Thank you for the advice to check my power source; however, my real question wasn't to find a problem with my setup, or my code, but in the way I'm reading the PWM signals from the RC receiver. I will look into adding a decoupling capacitor to my project.

My sketch is currently using Interrupts to detect changes in the PWM signal and then measuring the pulse length when it goes high. This seems to be how everyone is doing it. The problem I see is that the very nature of the interrupt can cause occasional erroneous values.

Since I don't need to detect EVERY change in EVERY channel, EVERY millisecond, but only just before a PWM pulse is needed to output to the servos, can't I read the pulse sequentially from each receiver input at the beginning of the loop, process the mixes, then send output to the servos? Then start over?

It might look like this:

  1. Begin loop
  2. enable interrupt
  3. read PWM signal sequentially (one at a time) for each rx channel.
  4. disable interrupt
  5. process mixing
  6. process servo output
  7. End loop

As I said I'm my first post, I've only been at this for a month and nobody does it this way that I can find. Because of this, I'm thinking there must be a reason and I'm interested in learning.