Programming a MPU9250 Gyro

Hi!

I am new to arduino programming (embedded programming) but have previous experience in programming. I want to try to make a drone. I have gathered some code online and tried to put it together to understand it better, now the pwmRight drifts away by time… This is what ive come up with this far:

#include <Servo.h>
#include "mpu9250.h"

Mpu9250 imu(&Wire, 0x68);
int status;

/* Temporary without remote */
float input_throttle = 1200;

/* Valut */
float elapsedTime, time, timePrev;
long loop_timer;

/* Roll PID */
float roll_PID, pid_throttle_L_F, pid_throttle_L_B, pid_throttle_R_F, pid_throttle_R_B, roll_error, roll_previous_error;
float roll_pid_p = 0;
float roll_pid_i = 0;
float roll_pid_d = 0;
/* Roll PID Constants */
double roll_kp = 7; //3.55
double roll_ki = 0.006; //0.003
double roll_kd = 1.2; //2.05
float roll_desired_angle = 0;

/* Pitch PID */
float pitch_PID, pitch_error, pitch_previous_error;
float pitch_pid_p = 0;
float pitch_pid_i = 0;
float pitch_pid_d = 0;
/* Pitch PID Constants */
double pitch_kp = 72; //3.55
double pitch_ki = 0.006; //0.003
double pitch_kd = 1.22; //2.05
float pitch_desired_angle = 0;

/* Yaw PID */
float yaw_PID, yaw_error, yaw_previous_error;
float yaw_pid_p = 0;
float yaw_pid_i = 0;
float yaw_pid_d = 0;
/* Yaw PID Constants */
double yaw_kp = 72; //3.55
double yaw_ki = 0.006; //0.003
double yaw_kd = 1.22; //2.05
float yaw_desired_angle = 0;

void setup() {
  Serial.begin(115200);
  while (!Serial) {}

  if (!imu.Begin()) {
    Serial.println("IMU initialization unsuccessful");
    while (1) {}
  }

  loop_timer = micros();
}

void loop() {
  /* Read the sensor */
  if (imu.Read()) {
    timePrev = time;
    time = millis();
    elapsedTime = (time - timePrev) / 1000;

    /* PID */
    roll_error = roll_desired_angle - (imu.gyro_x_radps() * (180 / 3.14));
    pitch_error = pitch_desired_angle - (imu.gyro_y_radps() * (180 / 3.14));
    yaw_error = pitch_desired_angle - (imu.gyro_z_radps() * (180 / 3.14));

    /* Prospect */
    roll_pid_p = roll_kp * roll_error;
    pitch_pid_p = pitch_kp * pitch_error;
    yaw_pid_p = yaw_kp * yaw_error;

    /* Integral */
    if (-3 < roll_error < 3)
    {
      roll_pid_i = roll_pid_i + (roll_ki * roll_error);
    }
    if (-3 < pitch_error < 3)
    {
      pitch_pid_i = pitch_pid_i + (pitch_ki * pitch_error);
    }
    if (-3 < yaw_error < 3)
    {
      yaw_pid_i = yaw_pid_i + (yaw_ki * yaw_error);
    }

    /* Derivate */
    roll_pid_d = roll_kd * ((roll_error - roll_previous_error) / elapsedTime);
    pitch_pid_d = pitch_kd * ((pitch_error - pitch_previous_error) / elapsedTime);
    yaw_pid_d = yaw_kd * ((yaw_error - yaw_previous_error) / elapsedTime);

    /* PID summery */
    roll_PID = roll_pid_p + roll_pid_i + roll_pid_d;
    pitch_PID = pitch_pid_p + pitch_pid_i + pitch_pid_d;
    yaw_PID = yaw_pid_p + yaw_pid_i + yaw_pid_d;

    /* Regulate PID output for ESCs */
    if (roll_PID < -400) {
      roll_PID = -400;
    }
    if (roll_PID > 400) {
      roll_PID = 400;
    }
    if (pitch_PID < -400) {
      pitch_PID = -400;
    }
    if (pitch_PID > 400) {
      pitch_PID = 400;
    }
    if (yaw_PID < -400) {
      yaw_PID = -400;
    }
    if (yaw_PID > 400) {
      yaw_PID = 400;
    }

    /* Set the throttle PID for each motor */
    pid_throttle_R_F = input_throttle - roll_PID - pitch_PID + yaw_PID;
    pid_throttle_L_F = input_throttle + roll_PID - pitch_PID - yaw_PID;
    pid_throttle_R_B = input_throttle - roll_PID + pitch_PID - yaw_PID;
    pid_throttle_L_B = input_throttle + roll_PID + pitch_PID + yaw_PID;

    /* Regulate throttle for ESCs */
    //Right front
    if (pid_throttle_R_F < 1100)
    {
      pid_throttle_R_F = 1100;
    }
    if (pid_throttle_R_F > 2000)
    {
      pid_throttle_R_F = 2000;
    }

    //Left front
    if (pid_throttle_L_F < 1100)
    {
      pid_throttle_L_F = 1100;
    }
    if (pid_throttle_L_F > 2000)
    {
      pid_throttle_L_F = 2000;
    }

    //Right back
    if (pid_throttle_R_B < 1100)
    {
      pid_throttle_R_B = 1100;
    }
    if (pid_throttle_R_B > 2000)
    {
      pid_throttle_R_B = 2000;
    }

    //Left back
    if (pid_throttle_L_B < 1100)
    {
      pid_throttle_L_B = 1100;
    }
    if (pid_throttle_L_B > 2000)
    {
      pid_throttle_L_B = 2000;
    }

    /* Save Previous Error */
    roll_error = roll_previous_error;
    pitch_error = pitch_previous_error;
    yaw_error = yaw_previous_error;

    /* Display the data */
    Serial.print(imu.gyro_x_radps(), 6);
    Serial.print("\t");
    Serial.print(imu.gyro_y_radps(), 6);
    Serial.print("\t");
    Serial.print(imu.gyro_z_radps(), 6);

    /*
      Serial.print(imu.mag_x_ut(), 6);
      Serial.print("\t");
      Serial.print(imu.mag_y_ut(), 6);
      Serial.print("\t");
      Serial.print(imu.mag_z_ut(), 6);
      Serial.print("\t");
      Serial.println(imu.die_temperature_c(), 6);
    */
  }
  while (micros() - loop_timer < 4000);
  loop_timer = micros();
}

Thanks beforehand!

Best regards Max

Your topic has been moved here as this forum section is more appropriate than where it was originally posted

Terrible code. Look for something that actually has a chance of working.

The most egregious error is that the gyro is not calibrated. The raw gyro values always have offsets that will cause drift, often fatal to the drone.

I changed the topic a bit @jremington, do I want to make this for all my 4 motors, can you help me with that? :slight_smile:

The particular approach can never be made reliable, so there is no point in trying to fix it. Expect the drone to eventually crash, always.

Ok, so what should I do?

I have gathered some code online

Do more on line research and use your best judgement. There are far, far better approaches. Pay attention to comments on web sites that look interesting.

This in not a problem that can be solved by a few dozen lines of code.

Ok thank you

I found this, which may be of interest:

HTH

a7

That was not quite the answer I was looking for, the program is freezing and I don’t know why, can you help me? I find it hard when you are trying to put me off but I want to keep trying…

No, I don’t suppose it was. Imma do you a favor and just not be so unhelpful anymore, please except my sincere apologies.

CU

a7

When I run my updated code I get the error “float time” redecalred as different kind of symbol, anyone have any idea?

Have you posted your updated code ?

From the error message it sounds like you may have a variable named time and a function named time

I found the solution, I accidently had ESP8266 board selected. Now it “should” work, I am still waiting for the mpu9250 and some 3.3v regulators. :slight_smile:

The MPU9250 outputs 3 values, x y z, I have read that you should fool the 9250 to think that it’s always flat with the gyro angle, but what is the entity of the output z like radians/s etc? I am using the bolder flight mpu925 library.