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
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).
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
Post a Comment