I2C Sensor read causing jitters in servos

BACKGROUND
I am making a remote controlled plane using an Arduino on the plane side and two on the controller side (one extra Arduino to deal with the lcd and annunciator) connected with serial (later this will be an xbee but I kept accidentally breaking them during testing so I decided to only use them once everything is done). I am adding sensors on the plane side (1 Adafruit/Bosch BNO055 and 1 Adafruit/Bosch BME280, both connected with I2C) for temperature, altitude, roll, pitch, and yaw. I am sending the temperature and altitude readings back to the plane to display on the lcd and the attitude measurements I am using for the plane’s autopilot. The way the serial is connected is through a handshake protocol (one sends one string of data, the other processes and sends back its string of data, the one processes it, sends back, etc.).

THE PROBLEM
I have a function reading all of the sensor values I need (using the sensor library’s built in commands for each value). I call this function just before I send back a string of data to the controller so that It may add a small delay in the processing time for the plane but it will not interfere with the serial communications (it won’t be called when a string of data is incoming). However whenever I call the function the servos for the control surfaces of the plane begin to jitter slightly. This isn’t just when the plane is connected to serial either. Even when it is disconnected and reading sensor values the servos jitter. I tried each individual I2C command (for altitude, 3 altitudes, temp) and the problem wasn’t stemming from just one (each one caused it). And when I comment out the function call it works perfectly (no jitters). I’ve tried everything and have no clue what to do.

-Note: when I add a delay after the sensor read it reduces the jitters but not completely and it also increases the servo movement latency by a lot so I’d rather not use that.

Here’s my code:

#include <SoftwareSerial.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_BNO055.h>
#include <Adafruit_BME280.h>
#include <SPI.h>
#include <Adafruit_Sensor.h>
#include <utility/imumaths.h>

#define BME_SCK 13
#define BME_MISO 12
#define BME_MOSI 11
#define BME_CS 10

#define SEALEVELPRESSURE_HPA (1013.25)
#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 bno = Adafruit_BNO055(55);
Adafruit_BME280 bme; // I2C
#define LSM9DS1_M	0x1E // Would be 0x1C if SDO_M is LOW
#define LSM9DS1_AG	0x6B // Would be 0x6A if SDO_AG is LOW
Servo rudder;
Servo eng;
Servo ail;
Servo ele;
char incomingChar;
String strfinal="";
int throt;
bool autop=false;
bool conFail;
unsigned long ts=0;
int ailval=90;
int eleval=90;
int engval=90;
int rudval=90;
String substr="";
bool transmit=false;
int batper=100;
float voltage;
int cells=3;
float batperfl;
int tempint=70;
float tempFl=70.4;
int alt=200;
float altFl;
float roll;
float pitch;
float yaw;
char incomingWire;
int WireHelp;
String sensStr="";
String subSensStr="";
unsigned long tsTest=0;
SoftwareSerial mySerial(4, 8); // RX, TX
unsigned long tsSens=0;

void setup() {
  //mySerial.begin(4800);
Serial.begin(9600);
rudder.attach(3);
rudder.write(90);
eng.attach(9);
eng.write(0);
ail.attach(5);
ail.write(90);
ele.attach(6);
ele.write(90);
pinMode(2,OUTPUT);
digitalWrite(2,LOW);
if (!bme.begin()) {
    while (1);
  }
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    while(1);
  }
  delay(1000);
  bno.setExtCrystalUse(true);
}


void loop() {
  /*if(millis()-tsTest>30){
  Serial.println(altFl);
  tsTest=millis();}*/
  volt();
  if(transmit==true){
    getSensors();
    Serial.print(alt);//alt
    Serial.print('a');
    Serial.print(tempint);//temp
    Serial.print('b');
    Serial.print(batper);
    Serial.print('!');
    transmit=false;
  }
  /*if(conFail==true){
    if(millis()-tsSens>75){
   getSensors(); 
 tsSens=millis();}
  }*/
  if(Serial.available()>0){
    incomingChar=(char)Serial.read();
    if(incomingChar=='!'){
      lib();
      strfinal="";
      ts=millis();
      conFail=false;
      digitalWrite(2,LOW);
      //rollnshit();
      transmit=true;}
      else{
        strfinal+=incomingChar;}
}
rudder.write(rudval);
eng.write(engval);
ail.write(ailval);
ele.write(eleval);
if(millis()-ts>300){
 conFail=true;
digitalWrite(2,HIGH); 
}
}
void lib(){
  if(strfinal.charAt(strfinal.length()-1)=='1'){
    autop=true;}
  if(strfinal.charAt(strfinal.length()-1)=='2'){
    autop=false;}
  substr=strfinal.substring(strfinal.indexOf('c')+1,strfinal.length()-1);
  eleval=substr.toInt();
  substr=strfinal.substring(strfinal.indexOf('b')+1,strfinal.indexOf('c'));
  ailval=substr.toInt();
  substr=strfinal.substring(strfinal.indexOf('a')+1,strfinal.indexOf('b'));
  engval=substr.toInt();
  substr=strfinal.substring(0,strfinal.indexOf('a'));
  rudval=substr.toInt();
}
void volt(){
  voltage=(analogRead(3)/204.6)*(10.06+(4.65+0.98))/(4.65+0.98);
  if((voltage>5.4)&&(voltage<9.15)){
    batperfl=(voltage-6.6)/1.8*100;
  }
  if(voltage>=9.15){
    batperfl=(voltage-9.9)/2.7*100;}
  batper=round(batperfl);
  if(batper<0){
    batper=0;}
}
void getSensors(){
 //sensors_event_t event;
  //bno.getEvent(&event);
  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
  roll=euler.z();
  pitch=euler.y();
  yaw=euler.x();
 tempFl= bme.readTemperature();
 tempFl=((tempFl*9/5)+32);
altFl= bme.readAltitude(SEALEVELPRESSURE_HPA);
altFl=(altFl*3.2808); 
tempint=round(tempFl);
alt=round(altFl);
}

PlaneCode.ino (3.47 KB)

Hello,
How are you driving the servo motors?

With Arduino Servo Library (They are connected to PWM pins on the arduino)

@mureytasroc,

Were you able to track down the issue? I'm in a similar predicament with my project.

thanks.