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

Comments

Popular posts from this blog

Flip address is out of range arduino uno r3

Arduino Uno not uploading

Indesign and MathType fonts