Hello
I'm building a quadcopter and seem to have an issue where the readings I get from the MPU get interrupted when the XBee receives data from and XBee/Arduino setup. How would I go about to fix this, as the lag between the MPU readings causes the quadcopters motors to discharge?
How would I go about to fix this, as the lag between the MPU readings causes the quadcopters motors to discharge?
Pretty simple, really. Change the code you didn't post. Or the configurations of the XBees, which you also failed to describe.
Whoop!
//Coordinator
int sensorPin = A0; //Input from dial
int sensorValue = 0;
void setup()
{
Serial.begin(9600);
}
void loop()
{
sensorValue = analogRead(sensorPin);
sensorValue = map(sensorValue,0,714,0,500); //New range 0-500
Serial.println(sensorValue);
delay(400);
}
//Quad
#include <Wire.h>
#include <SoftwareSerial.h>
#include <Servo.h>
SoftwareSerial xbee(8,9);
//Vars
int gyro_x, gyro_y, gyro_z;
long acc_x, acc_y, acc_z, acc_total_vector;
int temperature;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
long loop_timer;
int serial_loop_counter;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch_output, angle_roll_output;
float error_pitch,error_roll, previous_pitch_error, previous_roll_error;
float desired_pitch_angle = 0;
float desired_roll_angle = 0;
float pid_p_pitch = 0;
float pid_i_pitch = 0;
float pid_d_pitch = 0;
float pid_p_roll = 0;
float pid_i_roll = 0;
float pid_d_roll = 0;
double kp = 0;//3.0; //max change
double ki = 0;//0.040; //fine tune
double kd = 17;//2.05; //speed of correction deg/sec
float elapsedTime, time, timePrev;
float PID_pitch,PID_roll;
float pwm4,pwm5,pwm6,pwm7;
double throttle = 1001;
Servo esc1;
Servo esc2;
Servo esc3;
Servo esc4;
void setup() {
Wire.begin();
Serial.begin(9600);
xbee.begin(9600);
setup_mpu_6050_registers();
Serial.print("Calibrating gyro");
read_mpu_6050_data();
for (int cal_int = 0; cal_int < 2000 ; cal_int ++){
gyro_x_cal += gyro_x; //Read the raw acc and gyro data from the MPU-6050
gyro_y_cal += gyro_y;
gyro_z_cal += gyro_z;
delay(3); //Delay 3us to simulate the 250Hz program loop
}
gyro_x_cal /= 2000;
gyro_y_cal /= 2000;
gyro_z_cal /= 2000;
delay(3000);
loop_timer = micros(); //Reset the loop timer
esc1.attach(4);
esc1.writeMicroseconds(1001);
esc2.attach(5);
esc2.writeMicroseconds(1001);
esc3.attach(6);
esc3.writeMicroseconds(1001);
esc4.attach(7);
esc4.writeMicroseconds(1001);
delay(4001);
}
void loop(){
int controllerValue = 0;
timePrev = time; // the previous time is stored before the actual time read
time = millis(); // actual time read
elapsedTime = (time - timePrev) / 1000;
read_mpu_6050_data(); //Read the raw acc and gyro data from the MPU-6050
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
//Gyro angle calculations
//0.0000611 = 1 / (250Hz / 65.5)
angle_pitch += gyro_x * 0.0000611;
angle_roll += gyro_y * 0.0000611;
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_xacc_x)+(acc_yacc_y)+(acc_zacc_z)); //Calculate the total accelerometer vector
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_y/acc_total_vector) 57.296; //Calculate the pitch angle
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
//Place the MPU-6050 spirit level and note the values in the following two lines for calibration
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll
if(set_gyro_angles){ //If the IMU is already started
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle maybe change to 0.0004
}
else{
angle_pitch = angle_pitch_acc;
angle_roll = angle_roll_acc;
set_gyro_angles = true;
}
//To dampen the pitch and roll angles a complementary filter is used
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value
Serial.println(angle_pitch_output); //Prints the pitch angle to Serial
if(xbee.available() > 0)
{
controllerValue = xbee.parseInt(); //reads the coordinator
}
}
void read_mpu_6050_data(){ //Subroutine for reading the raw gyro and accelerometer data
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x3B); //Send the requested starting register
Wire.endTransmission(); //End the transmission
Wire.requestFrom(0x68,14); //Request 14 bytes from the MPU-6050
while(Wire.available() < 14); //Wait until all the bytes are received
acc_x = Wire.read()<<8|Wire.read(); //Add the low and high byte to the acc_x variable
acc_y = Wire.read()<<8|Wire.read(); //Add the low and high byte to the acc_y variable
acc_z = Wire.read()<<8|Wire.read(); //Add the low and high byte to the acc_z variable
temperature = Wire.read()<<8|Wire.read(); //Add the low and high byte to the temperature variable
gyro_x = Wire.read()<<8|Wire.read(); //Add the low and high byte to the gyro_x variable
gyro_y = Wire.read()<<8|Wire.read(); //Add the low and high byte to the gyro_y variable
gyro_z = Wire.read()<<8|Wire.read(); //Add the low and high byte to the gyro_z variable
}
void setup_mpu_6050_registers(){
//Activate the MPU-6050
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x6B); //Send the requested starting register
Wire.write(0x00); //Set the requested starting register
Wire.endTransmission(); //End the transmission
//Configure the accelerometer (+/-8g)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1C); //Send the requested starting register
Wire.write(0x10); //Set the requested starting register
Wire.endTransmission(); //End the transmission
//Configure the gyro (500dps full scale)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1B); //Send the requested starting register
Wire.write(0x08); //Set the requested starting register
Wire.endTransmission(); //End the transmission
}
When you run the quad program and open the Serial monitor, the MPU6050 pitch values update extremely slow due to the data being received from the remote XBee. Both XBee's are running default settings from the XCTU setup. The readouts work so that means they are at least communicating and sending data.
When you run the quad program and open the Serial monitor, the MPU6050 pitch values update extremely slow due to the data being received from the remote XBee.
No. It is because of the WAY you receive the data.
if(xbee.available() > 0)
{
controllerValue = xbee.parseInt(); //reads the coordinator
}
parseInt() is a blocking function. It waits for a character to arrive that is not an digit. Sending data at the extremely slow speed of 9600 baud causes a significant delay between characters arriving.
Either pick up the speed or read and store the data as it arrives. Convert the stored data to an int when a non-digit character arrives.
writes a series of arrays, and converts char to int
Ah ha! That works! Thank you very much.