There's plenty of posts with this combination, but none seem to have a similar issue.
The code:
#include <SPI.h>
#include <I2Cdev.h>
#include "RF24.h"
#include <nRF24L01.h>
#include<Wire.h>
const int orangeLed = 3;
const int greenLed = 4;
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;
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;
RF24 myRadio (8, 7);
byte addresses[][6] = {"0"};
struct package
{
float angle_roll_output;
float angle_pitch_output;
};
typedef struct package Package;
Package data;
void setup() {
pinMode(orangeLed, OUTPUT);
pinMode(greenLed, OUTPUT);
Wire.begin(); //Start I2C as master
Serial.begin(1000000); //Use only for debugging
setup_mpu_6050_registers(); //Setup the registers of the MPU-6050 (500dfs / +/-8g) and start the gyro
for (int cal_int = 0; cal_int < 2000 ; cal_int ++) { //Run this code 2000 times
if (cal_int % 125 == 0) {
Serial.print(".");
digitalWrite(orangeLed, HIGH);
delay(100);
digitalWrite(orangeLed, LOW);
}
read_mpu_6050_data(); //Read the raw acc and gyro data from the MPU-6050
gyro_x_cal += gyro_x; //Add the gyro x-axis offset to the gyro_x_cal variable
gyro_y_cal += gyro_y; //Add the gyro y-axis offset to the gyro_y_cal variable
gyro_z_cal += gyro_z; //Add the gyro z-axis offset to the gyro_z_cal variable
delay(3);
}
gyro_x_cal /= 2000; //Divide the gyro_x_cal variable by 2000 to get the avarage offset
gyro_y_cal /= 2000; //Divide the gyro_y_cal variable by 2000 to get the avarage offset
gyro_z_cal /= 2000; //Divide the gyro_z_cal variable by 2000 to get the avarage offset
myRadio.begin();
myRadio.setChannel(115);
myRadio.setPALevel(RF24_PA_MAX);
myRadio.setDataRate( RF24_2MBPS ) ;
myRadio.openWritingPipe( addresses[0]);
digitalWrite(greenLed, HIGH);
}
void loop() {
read_mpu_6050_data(); //Read the raw acc and gyro data from the MPU-6050
gyro_x -= gyro_x_cal; //Subtract the offset calibration value from the raw gyro_x value
gyro_y -= gyro_y_cal; //Subtract the offset calibration value from the raw gyro_y value
gyro_z -= gyro_z_cal; //Subtract the offset calibration value from the raw gyro_z value
//Gyro angle calculations
//0.0000611 = 1 / (250Hz / 65.5)
angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable
angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_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
}
else { //At first start
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
set_gyro_angles = true; //Set the IMU started flag
}
//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.print ("Roll: ");
Serial.print (angle_roll_output);
Serial.print (" Pitch: ");
Serial.println (angle_pitch_output);
//myRadio.write(&data, sizeof(data));
}
Both parts finally work perfectly when tested separately, and with this code, the GY-521 gives very precise and quick values. But, as soon as I remove the comment dashes from this code:
//myRadio.write(&data, sizeof(data)); (Wish the lines were numbered)
suddenly the data values change very slowly, for example, when I turn the transmitter, "Pitch" goes to 80 but after removing the comment, it takes more than a minute to reach the same 80.
I imagine it just can't transmit the data that fast, or maybe something with clock speeds?
Best I could do is set these lines to max:
myRadio.setPALevel(RF24_PA_MAX);
- myRadio.setDataRate( RF24_2MBPS ) ;*
What other things could I do/change for faster transfer rate?
Thank you!