Slow Serial printing of data when using complementary filter.

this code wrote yaw,pitch , roll values of quadcopter.the values okay printing of values in serial monitor slow .like if angle 20 degrees made,the serial printing gradually increases 1,9,15 , 20 , becomes stable.current using baud rate of 250000 , seems okay when using lower baud rate , problem reappears.help.

code: [select]


#include<wire.h>

double accelx,accely,accelz,temperature,gyrox,gyroy,gyroz,gyro_x_cal,gyro_y_cal,gyro_z_cal,accel_x_cal,accel_y_cal,accel_z_cal; //these raw data mpu6050.
uint32_t timer; //it's timer, saved big-ass unsigned int.  use save times "micros()" command , subtract present time in microseconds time stored in timer calculate time each loop.
double roll, pitch ,yaw; //these angles in complementary filter
float rollangle,pitchangle;
int cal_int;
void setup() {
  // set mpu 6050:
  serial.begin(250000);
  wire.begin();
  twbr = 12;   
  setupmpu();   
  serial.println("calibrating accelerometer & gyroscope......");
  for(cal_int=1;cal_int<=2000;cal_int++)
  {
   recordregisters();
   gyro_x_cal += gyrox;
   gyro_y_cal  += gyroy ;
   gyro_z_cal += gyroz;

   accel_x_cal +=accelx;
   accel_y_cal +=accely;
   accel_z_cal +=accelz;
  }
  serial.println("calibration done..!!!");
  gyro_x_cal /= 2000;
  gyro_y_cal /= 2000;
  gyro_z_cal /= 2000;

  accel_x_cal /= 2000;
  accel_y_cal /= 2000;
  accel_z_cal /= 2000;
 
 
  //start timer
  timer = micros();
}

void loop() {
//now begins main loop.
  //collect raw data sensor.
  recordregisters();
  gyrox = gyrox / 65.5;
  gyroy = gyroy / 65.5;
  gyroz = gyroz / 65.5;

  accelx = accelx / 4096.0;
  accely = accely / 4096.0;
  accelz= accelz / 4096.0;
 
  double dt = (double)(micros() - timer) / 1000000; //this line 3 things: 1) stops timer, 2)converts timer's output seconds microseconds, 3)casts value double saved "dt".
  timer = micros(); //start timer again can calculate next dt.

  //the next 2 lines calculate orientation of accelerometer relative earth , convert output of atan2 radians degrees
  //we use data correct cumulative errors in orientation gyroscope develops.
  rollangle=atan2(accely,accelz)*180/pi; // formula found on internet
  pitchangle=atan2(accelx,sqrt(accely*accely+accelz*accelz))*180/pi; //formula found on internet

 
 

  //the complementary filter
  //this filter calculates angle based on integrating angular velocity angular displacement.
  //dt, recall, time between gathering data mpu6050.  we'll pretend
  //angular velocity has remained constant on time dt, , multiply angular velocity
  //time displacement.
  //the filter adds small correcting factor accelerometer ("roll" or "pitch"), gyroscope knows way down.
  roll = 0.85 * (roll+ gyrox * dt) + 0.15 * rollangle; // calculate angle using complimentary filter
  pitch = 0.85 * (pitch + gyroy * dt) + 0.15 * pitchangle;
  yaw=gyroz;
 
  serial.print("roll  ");
  serial.print(roll);
  serial.print("   pitch  ");
  serial.print(pitch);
  serial.print("   yaw    ");
  serial.println(yaw);

}

void setupmpu(){
  wire.begintransmission(0b1101000); //this i2c address of mpu (b1101000/b1101001 ac0 low/high datasheet sec. 9.2)
  wire.write(0x6b); //accessing register 6b - power management (sec. 4.28)
  wire.write(0b00000000); //setting sleep register 0. (required; see note on p. 9)
  wire.endtransmission(); 
  wire.begintransmission(0b1101000); //i2c address of mpu
  wire.write(0x1b); //accessing register 1b - gyroscope configuration (sec. 4.4)
  wire.write(0x08); //setting gyro full scale +/- 500deg./s
  wire.endtransmission();
  wire.begintransmission(0b1101000); //i2c address of mpu
  wire.write(0x1c); //accessing register 1c - acccelerometer configuration (sec. 4.5)
  wire.write(0x10); //setting accel +/- 8g
  wire.endtransmission();
 
  wire.begintransmission(0b1101000);                                      //start communication address found during search
  wire.write(0x1a);                                                          //we want write config register (1a hex)
  wire.write(0x03);                                                          //set register bits 00000011 (set digital low pass filter ~43hz)
  wire.endtransmission();                                                    //end transmission gyro   
}
void recordregisters() {
  wire.begintransmission(0b1101000); //i2c address of mpu
  wire.write(0x3b); //starting register accel readings
  wire.endtransmission();
  wire.requestfrom(0b1101000,14); //request accel registers (3b - 40)
  while(wire.available() < 14);
  accelx = wire.read()<<8|wire.read(); //store first 2 bytes accelx
  accely = wire.read()<<8|wire.read(); //store middle 2 bytes accely
  accelz = wire.read()<<8|wire.read(); //store last 2 bytes accelz
  temperature=wire.read()<<8|wire.read();
  gyrox = wire.read()<<8|wire.read(); //store first 2 bytes accelx
  gyroy = wire.read()<<8|wire.read(); //store middle 2 bytes accely
  gyroz = wire.read()<<8|wire.read(); //store last 2 bytes accelz

  if(cal_int == 2000)
  {
    gyrox -= gyro_x_cal;
    gyroy -= gyro_y_cal;
    gyroz -= gyro_z_cal;

    accelx -=accel_x_cal;
    accely -=accel_y_cal;
    accelz -=accel_z_cal;
   
  }
}


not sure if it's issue (it @ low baudrates) there software buffer in hardwareserial code possibly fill faster can emptied (transmitted); once buffer full, code block till there space before place next byte in software buffer , continue , hence slow down code.

measure how long recordregisters() function take

code: [select]


void setup()
{
  // existing code
  ...
  ...

  unsigned long starttime = millis();
  recordregisters()
  serial.println(millis() - starttime);
}

void loop()
{
}

that should give idea. if recordregisters slower time takes send data, it's not problem; else it's problem. can calculate time how long takes send data (10 bits per byte @ 250000 kbits/s).

Arduino Forum > Using Arduino > Microcontrollers > Slow Serial printing of data when using complementary filter.

arduino