# Controlling Servo with MPU6050

**URL:** <https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306>\
**Category:** Sensors\
**Created:** [December 2, 2020, 3:11pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306 "2020-12-02T15:11:22Z")\
**Posts on this page:** 20\
**Page:** 2

<div class="post-metadata">

**Author:** ![buavts](https://avatars.discourse-cdn.com/v4/letter/b/8e8cbc/32.png) [@buavts](https://forum.arduino.cc/u/buavts)\
**Post date:** [December 3, 2020, 5:34pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/21 "2020-12-03T17:34:47Z")

</div>

I found this sketch/example online (actually in the Arduino project hub) and I am trying to read the data coming from the mpu6050 module. But no matter what I do the Serial Monitor always says "Could not find a valid MPU6050 sensor, check wiring!" I can't get the data! Please help!

Here is the code:

```cpp
#include <MPU6050.h>

#include <MPU6050.h>

#include <Wire.h>

/*
    MPU6050 Triple Axis Gyroscope & Accelerometer. Simple Gyroscope Example.
    Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html
    GIT: https://github.com/jarzebski/Arduino-MPU6050
    Web: http://www.jarzebski.pl
    (c) 2014 by Korneliusz Jarzebski
*/

#include <Wire.h>

 MPU6050 MPU;

void setup() 
{
  Serial.begin(115200);

  // Initialize MPU6050
  Serial.println("Initialize MPU6050");
  while(!MPU.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
  { Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
    delay(500);
  }
  
  // If you want, you can set gyroscope offsets
  MPU.setGyroOffsetX(155);
  MPU.setGyroOffsetY(15);
  MPU.setGyroOffsetZ(15);
  
  // Calibrate gyroscope. The calibration must be at rest.
  // If you don't want calibrate, comment this line.
 // MPU.calibrateGyro();

  // Set threshold sensivty. Default 3.
  // If you don't want use threshold, comment this line or set 0.
  MPU.setThreshold(3);
  
  // Check settings
  checkSettings();
}

void checkSettings()
{
  Serial.println();
  
  Serial.print(" * Sleep Mode:        ");
  Serial.println(MPU.getSleepEnabled() ? "Enabled" : "Disabled");
  
  Serial.print(" * Clock Source:      ");
  switch(MPU.getClockSource())
  {
    case MPU6050_CLOCK_KEEP_RESET:    Serial.println("Stops the clock and keeps the timing generator in reset"); break;
    case MPU6050_CLOCK_EXTERNAL_19MHZ: Serial.println("PLL with external 19.2MHz reference"); break;
    case MPU6050_CLOCK_EXTERNAL_32KHZ: Serial.println("PLL with external 32.768kHz reference"); break;
    case MPU6050_CLOCK_PLL_ZGYRO:      Serial.println("PLL with Z axis gyroscope reference"); break;
    case MPU6050_CLOCK_PLL_YGYRO:      Serial.println("PLL with Y axis gyroscope reference"); break;
    case MPU6050_CLOCK_PLL_XGYRO:      Serial.println("PLL with X axis gyroscope reference"); break;
    case MPU6050_CLOCK_INTERNAL_8MHZ:  Serial.println("Internal 8MHz oscillator"); break;
  }
  
  Serial.print(" * Gyroscope:        ");
  switch(MPU.getScale())
  {
    case MPU6050_SCALE_2000DPS:        Serial.println("2000 dps"); break;
    case MPU6050_SCALE_1000DPS:        Serial.println("1000 dps"); break;
    case MPU6050_SCALE_500DPS:        Serial.println("500 dps"); break;
    case MPU6050_SCALE_250DPS:        Serial.println("250 dps"); break;
  } 
  
  Serial.print(" * Gyroscope offsets: ");
  Serial.print(MPU.getGyroOffsetX());
  Serial.print(" / ");
  Serial.print(MPU.getGyroOffsetY());
  Serial.print(" / ");
  Serial.println(MPU.getGyroOffsetZ());
  
  Serial.println();
}

void loop()
{
  Vector rawGyro = MPU.readRawGyro();
  Vector normGyro = MPU.readNormalizeGyro();

  Serial.print(" Xraw = ");
  Serial.print(rawGyro.XAxis);
  Serial.print(" Yraw = ");
  Serial.print(rawGyro.YAxis);
  Serial.print(" Zraw = ");
  Serial.println(rawGyro.ZAxis);

  Serial.print(" Xnorm = ");
  Serial.print(normGyro.XAxis);
  Serial.print(" Ynorm = ");
  Serial.print(normGyro.YAxis);
  Serial.print(" Znorm = ");
  Serial.println(normGyro.ZAxis);
  
  delay(10);
}

```

---

<div class="post-metadata">

**Author:** ![anon94801460](https://avatars.discourse-cdn.com/v4/letter/a/278dde/32.png) [@anon94801460](https://forum.arduino.cc/u/anon94801460)\
**Post date:** [December 3, 2020, 7:09pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/22 "2020-12-03T19:09:59Z")

</div>

You probably have a wiring problem. Use the I2C address scanner program to verify I2C communications.

---

<div class="post-metadata">

**Author:** ![buavts](https://avatars.discourse-cdn.com/v4/letter/b/8e8cbc/32.png) [@buavts](https://forum.arduino.cc/u/buavts)\
**Post date:** [December 3, 2020, 7:56pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/23 "2020-12-03T19:56:18Z")

</div>

Ok I scanned and there where no bad connections (it even said my address, which is 0x68). In the code above there is no way to change the address. Right?

---

<div class="post-metadata">

**Author:** ![anon94801460](https://avatars.discourse-cdn.com/v4/letter/a/278dde/32.png) [@anon94801460](https://forum.arduino.cc/u/anon94801460)\
**Post date:** [December 3, 2020, 9:06pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/24 "2020-12-03T21:06:42Z")

</div>

What does the library documentation say about device address?

Although there are many MPU-6050 libraries, you don't need one. This works:

```cpp
// MPU-6050 Short Example Sketch
// By Arduino User JohnChi
// August 17, 2014
// Public Domain
#include<Wire.h>
const int MPU_addr = 0x68; // I2C address of the MPU-6050
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);    // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);
}

void loop() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  int t = Wire.read();
  AcX = (t << 8) | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  t = Wire.read();
  AcY = (t << 8) | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  t = Wire.read();
  AcZ = (t << 8) | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  t = Wire.read();
  Tmp = (t << 8) | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  t = Wire.read();
  GyX = (t << 8) | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  t = Wire.read();
  GyY = (t << 8) | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  t = Wire.read();
  GyZ = (t << 8) | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  Serial.print("AcX = "); Serial.print(AcX);
  Serial.print(" | AcY = "); Serial.print(AcY);
  Serial.print(" | AcZ = "); Serial.print(AcZ);
  Serial.print(" | Tmp = "); Serial.print(Tmp / 340.00 + 36.53); //equation for temperature in degrees C from datasheet
  Serial.print(" | GyX = "); Serial.print(GyX);
  Serial.print(" | GyY = "); Serial.print(GyY);
  Serial.print(" | GyZ = "); Serial.println(GyZ);
  delay(333);
}

```

---

<div class="post-metadata">

**Author:** ![buavts](https://avatars.discourse-cdn.com/v4/letter/b/8e8cbc/32.png) [@buavts](https://forum.arduino.cc/u/buavts)\
**Post date:** [December 3, 2020, 9:20pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/25 "2020-12-03T21:20:45Z")

</div>

The problem with your example is that in the serial monitor the Gyro values don't stay constant when I tilt the module because it's measuring the rate, so it only changes for a second. How do I get the Gyro values to become constant values (like the accelerometer) when I tilt?

---

<div class="post-metadata">

**Author:** ![anon94801460](https://avatars.discourse-cdn.com/v4/letter/a/278dde/32.png) [@anon94801460](https://forum.arduino.cc/u/anon94801460)\
**Post date:** [December 3, 2020, 9:37pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/26 "2020-12-03T21:37:06Z")

</div>

The gyro measures the rates of rotation, not 3D orientation angles. And there are rate offsets that need to be removed by calibration, upon power up.

What do **you** want to measure?

---

<div class="post-metadata">

**Author:** ![buavts](https://avatars.discourse-cdn.com/v4/letter/b/8e8cbc/32.png) [@buavts](https://forum.arduino.cc/u/buavts)\
**Post date:** [December 3, 2020, 9:48pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/27 "2020-12-03T21:48:37Z")

</div>

First of all I want to say thanks. You have been the best help to me!

I am doing a project where the MPU6050 and an Arduino Nano control a servo that controls the flaps on a foam glider. Since the glider is already accelerating I can't use the accelerometer data and instead use the gyro but I can't figure out how to change the raw gyro data to actually move a servo.

---

<div class="post-metadata">

**Author:** ![anon94801460](https://avatars.discourse-cdn.com/v4/letter/a/278dde/32.png) [@anon94801460](https://forum.arduino.cc/u/anon94801460)\
**Post date:** [December 3, 2020, 10:06pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/28 "2020-12-03T22:06:40Z")

</div>

Do you want to use the sensor to keep the glider flying level?

If so, to do that well is challenging, but there are several tutorials on line. Search for something like "mpu-6050 flight stabilizer" or "drone stabilizer".

Beware of Instructables. Many are simply cobbled-together nonsense, that happened to work once.

---

<div class="post-metadata">

**Author:** ![Idahowalker](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/idahowalker/32/238858_2.png) [@Idahowalker](https://forum.arduino.cc/u/Idahowalker)\
**Post date:** [December 3, 2020, 10:12pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/29 "2020-12-03T22:12:12Z")

</div>

You will NOT be able to use the data from a RATE gyro to determine IMU angle. Understand?

And yet, when I go to the homie built drone sites, they manage to do it. I found the Boulderflight library to be a favorite of DIY drone peoples.

---

<div class="post-metadata">

**Author:** ![anon94801460](https://avatars.discourse-cdn.com/v4/letter/a/278dde/32.png) [@anon94801460](https://forum.arduino.cc/u/anon94801460)\
**Post date:** [December 3, 2020, 10:14pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/30 "2020-12-03T22:14:04Z")

</div>

What are you talking about? **ALL 3D orientation filter codes use the gyro rate data.**

---

<div class="post-metadata">

**Author:** ![Idahowalker](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/idahowalker/32/238858_2.png) [@Idahowalker](https://forum.arduino.cc/u/Idahowalker)\
**Post date:** [December 3, 2020, 10:15pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/31 "2020-12-03T22:15:46Z")

</div>

> jremington:  
> What are you talking about? **ALL 3D orientation filter codes use the gyro rate data.**

OK, just using the gyros tell me the tilt angle of a MPU6050; just the gyros. That's what buavts wants just gyros.

Using gyro data is completely different than only using gyro data.

---

<div class="post-metadata">

**Author:** ![buavts](https://avatars.discourse-cdn.com/v4/letter/b/8e8cbc/32.png) [@buavts](https://forum.arduino.cc/u/buavts)\
**Post date:** [December 3, 2020, 10:18pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/32 "2020-12-03T22:18:29Z")

</div>

Yes I do want to keep the glider level.

AND

Why will I not be able to determine IMU angle with gyro. I know it can be done, people online have done it with the MPU6050 but I can't access there files because I have to become a patron supporter which needs money.

---

<div class="post-metadata">

**Author:** ![Idahowalker](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/idahowalker/32/238858_2.png) [@Idahowalker](https://forum.arduino.cc/u/Idahowalker)\
**Post date:** [December 3, 2020, 10:24pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/33 "2020-12-03T22:24:52Z")

</div>

With gyro and accelerometer data

---

<div class="post-metadata">

**Author:** ![anon94801460](https://avatars.discourse-cdn.com/v4/letter/a/278dde/32.png) [@anon94801460](https://forum.arduino.cc/u/anon94801460)\
**Post date:** [December 3, 2020, 10:25pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/34 "2020-12-03T22:25:43Z")

</div>

For full 3D orientation, most people use a 9DOF sensor, because the MPU-6050 can't define yaw (angle from North).

However, for pitch and roll stabilization with your MPU-6050. you can start with this code, which uses the Mahony filter to determine the 3D orientation. Yaw will drift.

Be sure to calibrate the rate gyro as every one is different. It is not so important to calibrate the accelerometer, but it helps.

This code just prints out the orientation angles.

```cpp
//
// MPU-6050 Mahony IMU  (yaw angle is relative to starting orientation)
// last update 7/9/2020 SJR
//

#include "Wire.h"

// AD0 low = 0x68 (default for Sparkfun module)
// AD0 high = 0x69
int MPU_addr = 0x68;

// vvvvvvvvvvvvvvvvvv  VERY VERY IMPORTANT vvvvvvvvvvvvvvvvvvvvvvvvvvvvv
//These are the previously determined offsets and scale factors for accelerometer and gyro for
// a particular example of an MPU-6050. They are not correct for other examples.
//The IMU code will NOT work well or at all if these are not correct

float A_cal[6] = {265.0, -80.0, -700.0, 0.994, 1.000, 1.014}; // 0..2 offset xyz, 3..5 scale xyz
float G_off[3] = { -499.5, -17.7, -82.0}; //raw offsets, determined for gyro at rest
#define gscale ((250./32768.0)*(PI/180.0))  //gyro default 250 LSB per d/s -> rad/s

// ^^^^^^^^^^^^^^^^^^^ VERY VERY IMPORTANT ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

// GLOBALLY DECLARED, required for Mahony filter
// vector to hold quaternion
float q[4] = {1.0, 0.0, 0.0, 0.0};

// Free parameters in the Mahony filter and fusion scheme,
// Kp for proportional feedback, Ki for integral
float Kp = 30.0;
float Ki = 0.0;

// with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.
// with MPU-6050, some instability observed at Kp=100 Now set to 30.

// char s[60]; //snprintf buffer, if needed

// globals for AHRS loop timing
unsigned long now_ms, last_ms = 0; //millis() timers

// print interval
unsigned long print_ms = 200; //print angles every "print_ms" milliseconds
float yaw, pitch, roll; //Euler angle output

void setup() {

  Wire.begin();
  Serial.begin(9600);
  Serial.println("starting");

  // initialize sensor
  // defaults for gyro and accel sensitivity are 250 dps and +/- 2 g
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);    // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

}

// AHRS loop

void loop()
{
  static int i = 0;
  static float deltat = 0;  //loop time in seconds
  static unsigned long now = 0, last = 0; //micros() timers

  //raw data
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  int16_t Tmp; //temperature

  //scaled data as vector
  float Axyz[3];
  float Gxyz[3];

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers
  int t = Wire.read() << 8;
  ax = t | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  t = Wire.read() << 8;
  ay = t | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  t = Wire.read() << 8;
  az = t | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  t = Wire.read() << 8;
  Tmp = t | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  t = Wire.read() << 8;
  gx = t | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  t = Wire.read() << 8;
  gy = t | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  t = Wire.read() << 8;
  gz = t | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  Axyz[0] = (float) ax;
  Axyz[1] = (float) ay;
  Axyz[2] = (float) az;

  //apply offsets and scale factors from Magneto
  for (int i = 0; i < 3; i++) Axyz[i] = (Axyz[i] - A_cal[i]) * A_cal[i + 3];

  Gxyz[0] = ((float) gx - G_off[0]) * gscale; //250 LSB(d/s) default to radians/s
  Gxyz[1] = ((float) gy - G_off[1]) * gscale;
  Gxyz[2] = ((float) gz - G_off[2]) * gscale;

  //  snprintf(s,sizeof(s),"mpu raw %d,%d,%d,%d,%d,%d",ax,ay,az,gx,gy,gz);
  //  Serial.println(s);

  now = micros();
  deltat = (now - last) * 1.0e-6; //seconds since last update
  last = now;

  Mahony_update(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], deltat);

  // Compute Tait-Bryan angles.
  // In this coordinate system, the positive z-axis is down toward Earth.
  // Yaw is the angle between Sensor x-axis and Earth magnetic North
  // (or true North if corrected for local declination, looking down on the sensor
  // positive yaw is counterclockwise, which is not conventional for NED navigation.
  // Pitch is angle between sensor x-axis and Earth ground plane, toward the
  // Earth is positive, up toward the sky is negative. Roll is angle between
  // sensor y-axis and Earth ground plane, y-axis up is positive roll. These
  // arise from the definition of the homogeneous rotation matrix constructed
  // from quaternions. Tait-Bryan angles as well as Euler angles are
  // non-commutative; that is, the get the correct orientation the rotations
  // must be applied in the correct order which for this configuration is yaw,
  // pitch, and then roll.
  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
  // which has additional links.

  roll  = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
  pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
  //conventional yaw increases clockwise from North. Not that the MPU-6050 knows where North is.
  yaw  = -atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3]));
  // to degrees
  yaw  *= 180.0 / PI;
  if (yaw < 0) yaw += 360.0; //compass circle
  pitch *= 180.0 / PI;
  roll *= 180.0 / PI;

  now_ms = millis(); //time to print?
  if (now_ms - last_ms >= print_ms) {
    last_ms = now_ms;
    // print angles for serial plotter...
    //  Serial.print("ypr ");
    Serial.print(yaw, 0);
    Serial.print(", ");
    Serial.print(pitch, 0);
    Serial.print(", ");
    Serial.println(roll, 0);
  }
}
//--------------------------------------------------------------------------------------------------
// Mahony scheme uses proportional and integral filtering on
// the error between estimated reference vector (gravity) and measured one.
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date      Author      Notes
// 29/09/2011 SOH Madgwick    Initial release
// 02/10/2011 SOH Madgwick  Optimised for reduced CPU load
// last update 07/09/2020 SJR minor edits
//--------------------------------------------------------------------------------------------------
// IMU algorithm update

void Mahony_update(float ax, float ay, float az, float gx, float gy, float gz, float deltat) {
  float recipNorm;
  float vx, vy, vz;
  float ex, ey, ez;  //error terms
  float qa, qb, qc;
  static float ix = 0.0, iy = 0.0, iz = 0.0;  //integral feedback terms
  float tmp;

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  tmp = ax * ax + ay * ay + az * az;
  if (tmp > 0.0)
  {

    // Normalise accelerometer (assumed to measure the direction of gravity in body frame)
    recipNorm = 1.0 / sqrt(tmp);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Estimated direction of gravity in the body frame (factor of two divided out)
    vx = q[1] * q[3] - q[0] * q[2];
    vy = q[0] * q[1] + q[2] * q[3];
    vz = q[0] * q[0] - 0.5f + q[3] * q[3];

    // Error is cross product between estimated and measured direction of gravity in body frame
    // (half the actual magnitude)
    ex = (ay * vz - az * vy);
    ey = (az * vx - ax * vz);
    ez = (ax * vy - ay * vx);

    // Compute and apply to gyro term the integral feedback, if enabled
    if (Ki > 0.0f) {
      ix += Ki * ex * deltat;  // integral error scaled by Ki
      iy += Ki * ey * deltat;
      iz += Ki * ez * deltat;
      gx += ix;  // apply integral feedback
      gy += iy;
      gz += iz;
    }

    // Apply proportional feedback to gyro term
    gx += Kp * ex;
    gy += Kp * ey;
    gz += Kp * ez;
  }

  // Integrate rate of change of quaternion, q cross gyro term
  deltat = 0.5 * deltat;
  gx *= deltat;  // pre-multiply common factors
  gy *= deltat;
  gz *= deltat;
  qa = q[0];
  qb = q[1];
  qc = q[2];
  q[0] += (-qb * gx - qc * gy - q[3] * gz);
  q[1] += (qa * gx + qc * gz - q[3] * gy);
  q[2] += (qa * gy - qb * gz + q[3] * gx);
  q[3] += (qa * gz + qb * gy - qc * gx);

  // renormalise quaternion
  recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
  q[0] = q[0] * recipNorm;
  q[1] = q[1] * recipNorm;
  q[2] = q[2] * recipNorm;
  q[3] = q[3] * recipNorm;
}

```

---

<div class="post-metadata">

**Author:** ![Idahowalker](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/idahowalker/32/238858_2.png) [@Idahowalker](https://forum.arduino.cc/u/Idahowalker)\
**Post date:** [December 3, 2020, 10:28pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/35 "2020-12-03T22:28:29Z")

</div>

> jremington:  
> For full 3D orientation, most people use a 9DOF sensor, because the MPU-6050 can't define yaw (angle from North). However, for pitch and roll stabilization, with your MPU-6050 you can try this code, which uses the Mahony filter. Yaw will drift.
> 
> Be sure to calibrate the rate gyro as every one is different. It is not so important to calibrate the accelerometer, but it helps.
> 
> ```arduino
> //
> 
> ```

// MPU-6050 Mahony IMU&nbsp; (yaw angle is relative to starting orientation)  
// last update 7/9/2020 SJR  
//

#include "Wire.h"

// AD0 low = 0x68 (default for Sparkfun module)  
// AD0 high = 0x69  
int MPU\_addr = 0x68;

// vvvvvvvvvvvvvvvvvv&nbsp; VERY VERY IMPORTANT vvvvvvvvvvvvvvvvvvvvvvvvvvvvv  
//These are the previously determined offsets and scale factors for accelerometer and gyro for  
// a particular example of an MPU-6050. They are not correct for other examples.  
//The IMU code will NOT work well or at all if these are not correct

float A\_cal[6] = {265.0, -80.0, -700.0, 0.994, 1.000, 1.014}; // 0..2 offset xyz, 3..5 scale xyz  
float G\_off[3] = { -499.5, -17.7, -82.0}; //raw offsets, determined for gyro at rest  
#define gscale ((250./32768.0)\*(PI/180.0))&nbsp; //gyro default 250 LSB per d/s -\> rad/s

// ^^^^^^^^^^^^^^^^^^^ VERY VERY IMPORTANT ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

// GLOBALLY DECLARED, required for Mahony filter  
// vector to hold quaternion  
float q[4] = {1.0, 0.0, 0.0, 0.0};

// Free parameters in the Mahony filter and fusion scheme,  
// Kp for proportional feedback, Ki for integral  
float Kp = 30.0;  
float Ki = 0.0;

// with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required.  
// with MPU-6050, some instability observed at Kp=100 Now set to 30.

// char s[60]; //snprintf buffer, if needed

// globals for AHRS loop timing  
unsigned long now\_ms, last\_ms = 0; //millis() timers

// print interval  
unsigned long print\_ms = 200; //print angles every "print\_ms" milliseconds  
float yaw, pitch, roll; //Euler angle output

void setup() {

Wire.begin();  
&nbsp; Serial.begin(9600);  
&nbsp; Serial.println("starting");

// initialize sensor  
&nbsp; // defaults for gyro and accel sensitivity are 250 dps and +/- 2 g  
&nbsp; Wire.beginTransmission(MPU\_addr);  
&nbsp; Wire.write(0x6B);&nbsp; // PWR\_MGMT\_1 register  
&nbsp; Wire.write(0);&nbsp; &nbsp; // set to zero (wakes up the MPU-6050)  
&nbsp; Wire.endTransmission(true);

}

// AHRS loop

void loop()  
{  
&nbsp; static int i = 0;  
&nbsp; static float deltat = 0;&nbsp; //loop time in seconds  
&nbsp; static unsigned long now = 0, last = 0; //micros() timers

//raw data  
&nbsp; int16\_t ax, ay, az;  
&nbsp; int16\_t gx, gy, gz;  
&nbsp; int16\_t Tmp; //temperature

//scaled data as vector  
&nbsp; float Axyz[3];  
&nbsp; float Gxyz[3];

Wire.beginTransmission(MPU\_addr);  
&nbsp; Wire.write(0x3B);&nbsp; // starting with register 0x3B (ACCEL\_XOUT\_H)  
&nbsp; Wire.endTransmission(false);  
&nbsp; Wire.requestFrom(MPU\_addr, 14, true); // request a total of 14 registers  
&nbsp; int t = Wire.read() \<\< 8;  
&nbsp; ax = t | Wire.read(); // 0x3B (ACCEL\_XOUT\_H) & 0x3C (ACCEL\_XOUT\_L)  
&nbsp; t = Wire.read() \<\< 8;  
&nbsp; ay = t | Wire.read(); // 0x3D (ACCEL\_YOUT\_H) & 0x3E (ACCEL\_YOUT\_L)  
&nbsp; t = Wire.read() \<\< 8;  
&nbsp; az = t | Wire.read(); // 0x3F (ACCEL\_ZOUT\_H) & 0x40 (ACCEL\_ZOUT\_L)  
&nbsp; t = Wire.read() \<\< 8;  
&nbsp; Tmp = t | Wire.read(); // 0x41 (TEMP\_OUT\_H) & 0x42 (TEMP\_OUT\_L)  
&nbsp; t = Wire.read() \<\< 8;  
&nbsp; gx = t | Wire.read(); // 0x43 (GYRO\_XOUT\_H) & 0x44 (GYRO\_XOUT\_L)  
&nbsp; t = Wire.read() \<\< 8;  
&nbsp; gy = t | Wire.read(); // 0x45 (GYRO\_YOUT\_H) & 0x46 (GYRO\_YOUT\_L)  
&nbsp; t = Wire.read() \<\< 8;  
&nbsp; gz = t | Wire.read(); // 0x47 (GYRO\_ZOUT\_H) & 0x48 (GYRO\_ZOUT\_L)

Axyz[0] = (float) ax;  
&nbsp; Axyz[1] = (float) ay;  
&nbsp; Axyz[2] = (float) az;

//apply offsets and scale factors from Magneto  
&nbsp; for (int i = 0; i \< 3; i++) Axyz[i] = (Axyz[i] - A\_cal[i]) \* A\_cal[i + 3];

Gxyz[0] = ((float) gx - G\_off[0]) \* gscale; //250 LSB(d/s) default to radians/s  
&nbsp; Gxyz[1] = ((float) gy - G\_off[1]) \* gscale;  
&nbsp; Gxyz[2] = ((float) gz - G\_off[2]) \* gscale;

//&nbsp; snprintf(s,sizeof(s),"mpu raw %d,%d,%d,%d,%d,%d",ax,ay,az,gx,gy,gz);  
&nbsp; //&nbsp; Serial.println(s);

now = micros();  
&nbsp; deltat = (now - last) \* 1.0e-6; //seconds since last update  
&nbsp; last = now;

Mahony\_update(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], deltat);

// Compute Tait-Bryan angles.  
&nbsp; // In this coordinate system, the positive z-axis is down toward Earth.  
&nbsp; // Yaw is the angle between Sensor x-axis and Earth magnetic North  
&nbsp; // (or true North if corrected for local declination, looking down on the sensor  
&nbsp; // positive yaw is counterclockwise, which is not conventional for NED navigation.  
&nbsp; // Pitch is angle between sensor x-axis and Earth ground plane, toward the  
&nbsp; // Earth is positive, up toward the sky is negative. Roll is angle between  
&nbsp; // sensor y-axis and Earth ground plane, y-axis up is positive roll. These  
&nbsp; /

> ```arduino
> 
> ```

I've already pointed this ^^^^^ out, in a duplicate post [Controlling Servo with MPU6050 - #21 by buavts - Sensors - Arduino Forum](https://forum.arduino.cc/index.php?topic=716692.msg4814547#msg4814547), to the OP.  
The OP just wants to use gyro data so your thingy is not wanted.

---

<div class="post-metadata">

**Author:** ![buavts](https://avatars.discourse-cdn.com/v4/letter/b/8e8cbc/32.png) [@buavts](https://forum.arduino.cc/u/buavts)\
**Post date:** [December 3, 2020, 10:36pm UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/36 "2020-12-03T22:36:15Z")

</div>

Ok this is what I want. I want to control servo with the gyro data from the MPU6050 (I know this is posable, just don't know how to do it).

jremington your code worked great but I think it's using the accelerometer and not the gyro.

---

<div class="post-metadata">

**Author:** ![anon94801460](https://avatars.discourse-cdn.com/v4/letter/a/278dde/32.png) [@anon94801460](https://forum.arduino.cc/u/anon94801460)\
**Post date:** [December 4, 2020, 12:18am UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/37 "2020-12-04T00:18:47Z")

</div>

I assure you, the code uses the gyro rate data. The accelerometer defines "down", so pitch and roll are measured with respect to horizontal.

It is possible to use just the gyro, but then all three angles will drift, not just yaw. After a while, your glider will be upside down and happy.

You can try ignoring the accelerometer data by changing these lines:

```cpp
  tmp = ax * ax + ay * ay + az * az;
  if (tmp > 0.0)

```

to this:

```cpp
//  tmp = ax * ax + ay * ay + az * az;
// Ignore accelerometer data
  tmp = 0.0;
  if(tmp > 0.0)

```

But it seems rather silly to ignore half the sensor data.

---

<div class="post-metadata">

**Author:** ![Coding\_Badly](https://avatars.discourse-cdn.com/v4/letter/c/f14d63/32.png) [@Coding\_Badly](https://forum.arduino.cc/u/Coding_Badly)\
**Post date:** [December 4, 2020, 12:33am UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/38 "2020-12-04T00:33:30Z")

</div>

Stop cross-posting.

---

<div class="post-metadata">

**Author:** ![dyso](https://avatars.discourse-cdn.com/v4/letter/d/a9a28c/32.png) [@dyso](https://forum.arduino.cc/u/dyso)\
**Post date:** [December 4, 2020, 11:34am UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/39 "2020-12-04T11:34:48Z")

</div>

> jremington:  
> It is not so important to calibrate the accelerometer, but it helps.

Are you sure that it is not so important to calibrate the accelerometer? I mean, my LSM303 has the weirdest offset.

 ![Acceleration2.jpg](https://europe1.discourse-cdn.com/arduino/original/4X/9/a/f/9afd7d8f8a87621119e2ab314c7e66409f7fef9e.jpeg)

 ![Acceleration2.jpg](https://europe1.discourse-cdn.com/arduino/original/4X/9/a/f/9afd7d8f8a87621119e2ab314c7e66409f7fef9e.jpeg)

---

<div class="post-metadata">

**Author:** ![dyso](https://avatars.discourse-cdn.com/v4/letter/d/a9a28c/32.png) [@dyso](https://forum.arduino.cc/u/dyso)\
**Post date:** [December 4, 2020, 11:51am UTC](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/40 "2020-12-04T11:51:50Z")

</div>

> buavts:  
> Why will I not be able to determine IMU angle with gyro. I know it can be done, people online have done it with the MPU6050 but I can't access there files because I have to become a patron supporter which needs money.

It can be done but if you really want proof to why we don't do this, you can calculate this.  
Step 1: The gyro measures angular velocity. Let's say degrees/second to imagine it better.  
Step 2: Convert angular velocity in just angles. That means we need to integrate this. If we integrate, that means we need to add a constant. That means:  
angular\_position = angular\_velocity(°/s) \* dt + last\_angular\_position  
dt = the time between the samples  
The problem is that the noise you produce will create a new angular\_position that gets added onto the constant. I don't know if this is clear enough. However, this video is great to understand the problem and the idea why we fuse the accelerometer with the gyroscope:

[![](https://europe1.discourse-cdn.com/arduino/original/4X/7/9/f/79f2b20059161b44b93be1eb3d868e7b0aab1f2c.jpeg "Understanding Sensor Fusion and Tracking, Part 2: Fusing a Mag, Accel, & Gyro Estimate") ](https://www.youtube.com/watch?v=0rlvvYgmTvI&t=775s)

[Previous page](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306.md?page=1)

[Next page](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306.md?page=3)