/********* Pleasedontcode.com **********

	Pleasedontcode thanks you for automatic code generation! Enjoy your code!

	- Terms and Conditions:
	You have a non-exclusive, revocable, worldwide, royalty-free license
	for personal and commercial use. Attribution is optional; modifications
	are allowed, but you're responsible for code maintenance. We're not
	liable for any loss or damage. For full terms,
	please visit pleasedontcode.com/termsandconditions.

	- Project: "MPU6050 Initialization"
	- Source Code compiled for: Arduino Uno
	- Source Code created on: 2024-06-16 14:26:09

********* Pleasedontcode.com **********/

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* flight controller for brushed motors using radio */
	/* frequency transmitters and receiver like flysky , */
	/* radiolink */
/****** END SYSTEM REQUIREMENTS *****/

/****** DEFINITION OF LIBRARIES *****/
#include 
#include     // https://github.com/LiquidCGS/FastIMU

/****** FUNCTION PROTOTYPES *****/
void setup(void);
void loop(void);

/***** DEFINITION OF DIGITAL INPUT PINS *****/
const uint8_t gyro_MPU6050_Interrupt_PIN_D2 = 2;

/***** DEFINITION OF I2C PINS *****/
const uint8_t gyro_MPU6050_I2C_PIN_SDA_A4 = A4;
const uint8_t gyro_MPU6050_I2C_PIN_SCL_A5 = A5;
const uint8_t gyro_MPU6050_I2C_SLAVE_ADDRESS = 0x68; // 104 in decimal

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
MPU6050 IMU; // Initialize MPU6050 with FastIMU library

/****** GLOBAL VARIABLES *****/
bool dmpReady = false;  // set true if DMP init was successful
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint8_t fifoBuffer[64]; // FIFO storage buffer

Quaternion q;           // [w, x, y, z] quaternion container
float ypr[3];           // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high

void dmpDataReady() {
    mpuInterrupt = true;
}

void setup(void) {
    // put your setup code here, to run once:
    Wire.begin();
    Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties

    Serial.begin(115200);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately

    Serial.println(F("Initializing I2C devices..."));
    calData calib = { 0 }; // Calibration data
    int err = IMU.init(calib, gyro_MPU6050_I2C_SLAVE_ADDRESS);
    if (err != 0) {
        Serial.print(F("Error initializing IMU: "));
        Serial.println(err);
        while (true) { ; }
    }

    pinMode(gyro_MPU6050_Interrupt_PIN_D2, INPUT);

    Serial.println(F("Testing device connections..."));
    Serial.println(IMU.init(calib, gyro_MPU6050_I2C_SLAVE_ADDRESS) == 0 ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (!Serial.available());
    while (Serial.available() && Serial.read());

    Serial.println(F("Initializing DMP..."));
    IMU.setGyroRange(250); // Set gyro range to ±250 degrees/second
    IMU.setAccelRange(2);  // Set accelerometer range to ±2g

    // configure LED for output
    pinMode(LED_BUILTIN, OUTPUT);
}

void loop(void) {
    // wait for MPU interrupt or extra packet(s) available
    IMU.update();

    AccelData accel;
    GyroData gyro;
    MagData mag;

    IMU.getAccel(&accel);
    IMU.getGyro(&gyro);
    if (IMU.hasMagnetometer()) {
        IMU.getMag(&mag);
    }

    Serial.print("Accel: ");
    Serial.print(accel.accelX);
    Serial.print(", ");
    Serial.print(accel.accelY);
    Serial.print(", ");
    Serial.print(accel.accelZ);
    Serial.print(" | Gyro: ");
    Serial.print(gyro.gyroX);
    Serial.print(", ");
    Serial.print(gyro.gyroY);
    Serial.print(", ");
    Serial.print(gyro.gyroZ);
    if (IMU.hasMagnetometer()) {
        Serial.print(" | Mag: ");
        Serial.print(mag.magX);
        Serial.print(", ");
        Serial.print(mag.magY);
        Serial.print(", ");
        Serial.print(mag.magZ);
    }
    Serial.println();

    // blink LED to indicate activity
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
}

/* END CODE */