Shifted pitch and yaw measurement about gy521

[RobTillaart]

my note to myself above seems to tickle a possible cause.

error ~17 could be 16 + some normal variation ==> bit error?

There is a correction factor 0.96 in the formula, or 4% when looking at it differently.

_roll  = 0.96 * _gax + 0.04 * _aax;

if gax is between 355-360 degrees. that 4% correction is about 360 degrees * 4% = 14.40
if gax is between 0 and 5 degrees, that 4% correction is about 5 * 4% = 0.20
==> this is a difference in the order of magnitude you are observing.

So this could imply hat the normalization in the read() function is incorrect.

So lets remove the normalization (line 173 - 192)

  //  normalize internal vars
  // if (_gax < 0) _gax += 360.0;
  // else if (_gax >= 360.0) _gax -= 360.0;
  // if (_gay < 0) _gay += 360.0;
  // else if (_gay >= 360.0) _gay -= 360.0;
  // if (_gaz < 0) _gaz += 360.0;
  // else if (_gaz >= 360.0) _gaz -= 360.0;

  _yaw   = _gaz;
  _pitch = 0.96 * _gay + 0.04 * _aay;
  _roll  = 0.96 * _gax + 0.04 * _aax;

  //  normalize 
  // if (_pitch < 0) _pitch += 360;
  // else if (_pitch >= 360) _pitch -= 360;
  //  if (_roll  < 0) _roll  += 360;
  // else if (_roll  >= 360) _roll  -= 360;
  // if (_yaw  < 0) _yaw  += 360;
  // else if (_yaw  >= 360) _yaw  -= 360;

  return GY521_OK;
}

Can you test please?

[janq0]

@janq0

Can you test the version below of read()?

By normalizing the _gax and _gay at 375 degrees the math should be right again. Then these values will keep highest precision and not "overflow" see issue #36.

Thanks for your time, appreciated!

int16_t GY521::read()
{
  uint32_t now = millis();
  if (_throttle)
  {
    if ((now - _lastTime) < _throttleTime)
    {
      //  not an error.
      return GY521_THROTTLED;
    }
  }
  _lastTime = now;

  //  Connected ?
  _wire->beginTransmission(_address);
  _wire->write(GY521_ACCEL_XOUT_H);
  if (_wire->endTransmission() != 0)
  {
    _error = GY521_ERROR_WRITE;
    return _error;
  }

  //  Get the data
  int8_t n = _wire->requestFrom(_address, (uint8_t)14);
  if (n != 14)
  {
    _error = GY521_ERROR_READ;
    return _error;
  }
  //  ACCELEROMETER
  _ax = _WireRead2();           //  ACCEL_XOUT_H  ACCEL_XOUT_L
  _ay = _WireRead2();           //  ACCEL_YOUT_H  ACCEL_YOUT_L
  _az = _WireRead2();           //  ACCEL_ZOUT_H  ACCEL_ZOUT_L
  //  TEMPERATURE
  _temperature = _WireRead2();  //  TEMP_OUT_H    TEMP_OUT_L
  //  GYROSCOPE
  _gx = _WireRead2();           //  GYRO_XOUT_H   GYRO_XOUT_L
  _gy = _WireRead2();           //  GYRO_YOUT_H   GYRO_YOUT_L
  _gz = _WireRead2();           //  GYRO_ZOUT_H   GYRO_ZOUT_L

  //  duration interval
  now = micros();
  float duration = (now - _lastMicros) * 1e-6;  // duration in seconds.
  _lastMicros = now;


  //  next lines might be merged per axis. (performance)

  //  Convert raw acceleration to g's
  _ax *= _raw2g;
  _ay *= _raw2g;
  _az *= _raw2g;

  //  Error correct raw acceleration (in g) measurements  // #18 kudos to Merkxic
  _ax += axe;
  _ay += aye;
  _az += aze;

  //  prepare for Pitch Roll Yaw
  float _ax2 = _ax * _ax;
  float _ay2 = _ay * _ay;
  float _az2 = _az * _az;

  _aax = atan(       _ay / sqrt(_ax2 + _az2)) * GY521_RAD2DEGREES;
  _aay = atan(-1.0 * _ax / sqrt(_ay2 + _az2)) * GY521_RAD2DEGREES;
  _aaz = atan(       _az / sqrt(_ax2 + _ay2)) * GY521_RAD2DEGREES;
  //  optimize #22
  //  _aax = atan(_ay / hypot(_ax, _az)) * GY521_RAD2DEGREES;
  //  _aay = atan(-1.0 * _ax / hypot(_ay, _az)) * GY521_RAD2DEGREES;
  //  _aaz = atan(_az / hypot(_ax, _ay)) * GY521_RAD2DEGREES;

  //  Convert to Celsius
  _temperature = _temperature * 0.00294117647 + 36.53;  //  == /340.0  + 36.53;

  //  Convert raw Gyro to degrees/seconds
  _gx *= _raw2dps;
  _gy *= _raw2dps;
  _gz *= _raw2dps;

  //  Error correct raw gyro measurements.
  _gx += gxe;
  _gy += gye;
  _gz += gze;

  //  _gax etc might loose precision after many iterations #36
  _gax += _gx * duration;
  _gay += _gy * duration;
  _gaz += _gz * duration;

  //  normalize
  bool _normalize = true;
  if (_normalize)
  {
    //  correction at 375 due to the factor 0.96 in roll formula
    if (_gax > 375)    _gax -= 375;
    else if (_gax < 0) _gax += 375;
    //  correction at 375 due to the factor 0.96 in pitch formula
    if (_gay > 375)    _gay -= 375;
    else if (_gay < 0) _gay += 375;
    // correction at 360
    if (_gaz > 360)    _gaz -= 360;
    else if (_gaz < 0) _gaz += 360;
  }


  //  Calculate Pitch Roll Yaw
  _yaw   = _gaz;
  _roll  = 0.96 * _gax + 0.04 * _aax;
  _pitch = 0.96 * _gay + 0.04 * _aay;


  if (_normalize)
  {
    if (_pitch > 360)    _pitch -= 360;
    else if (_pitch < 0) _pitch += 360;
    if (_roll > 360)     _roll -= 360;
    else if (_roll < 0)  _roll += 360;
    if (_yaw > 360)      _yaw -= 360;
    else if (_yaw < 0)   _yaw += 360;
  }

  return GY521_OK;
}

Might need to change > to >= as 0 == 360.

I tested this code and it seems to work. The measured values are normalized and the issue is gone.

[RobTillaart]

Thanks for testing,
Will merge a PR later today as its time for a small siesta.
(33°C today)

[RobTillaart]

Thanks for the issue.
Quite busy so it will take a while before I can Investigate..

[RobTillaart]

Q: are the jumps also occurring if you go the other way / from 350ish to 0.xxx?

[RobTillaart]

Notes to myself

• check math (done, but do it again)
• simulate raw sensor to stress math (lot of work)
• error ~17 could be 16 + some normal variation ==> bit error?
• pitch and roll correct with _aax _aay while yaw doesn't. ==> Are the _gaN values smooth or does jump comes from _aaN ?

[RobTillaart]

Q: what are your 6 calibration values?

[RobTillaart]

Scanned several tutorials / libraries and most had identical or similar math.
There was this remark that might fix the issue.
Remark in https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/

Can you verify if the fix below works as I have no setup to test.

Change in GY521.cpp the following lines (181-183) and give it a try.

  _yaw   = _gaz;
  _pitch = 0.96 * _gay + 0.04 * _aay;
  _roll  = 0.96 * _gax + 0.04 * _aax;

into

  _gax = 0.96 * _gax + 0.04 * _aax;
  _gay = 0.96 * _gay + 0.04 * _aay;

  _roll  = _gax;
  _pitch = _gay;
  _yaw   = _gaz;

The difference is that the correction factor 0.96 * _gax + 0.04 * _aax adds up or not in the _gax
(Y -axis idem).

If this fixes your issue I will update the library asap.

[janq0]

Q: what are your 6 calibration values?

The calibration in the example i gave was generated by the GY521_readCalibration_2 as follows:

  sensor.axe = 0.0786060;
  sensor.aye = 0.0127197;
  sensor.aze = -1.0656372;
  sensor.gxe = 1.3613740;
  sensor.gye = 2.9567937;
  sensor.aze = 0.1717557;

Scanned several tutorials / libraries and most had identical or similar math. There was this remark that might fix the issue. Remark in https://howtomechatronics.com/tutorials/arduino/arduino-and-mpu6050-accelerometer-and-gyroscope-tutorial/

Can you verify if the fix below works as I have no setup to test.

Change in GY521.cpp the following lines (181-183) and give it a try.

  _yaw   = _gaz;
  _pitch = 0.96 * _gay + 0.04 * _aay;
  _roll  = 0.96 * _gax + 0.04 * _aax;

into

  _gax = 0.96 * _gax + 0.04 * _aax;
  _gay = 0.96 * _gay + 0.04 * _aay;

  _roll  = _gax;
  _pitch = _gay;
  _yaw   = _gaz;

The difference is that the correction factor 0.96 * _gax + 0.04 * _aax adds up or not in the _gax (Y -axis idem).

If this fixes your issue I will update the library asap.

Changing the lines you specified makes the measurement very imprecise. (Is there some mistake in the code you included?) The values pitch and roll change at about 1 degree per second even though the GY-521 is stationary. Changing the actual pitch and roll of the gyroscope impacted the measured value, but not in a predictable way, which means I wasn't able to tell if the original issue was fixed by this. I made sure that the sensor is property calibrated before testing:

  sensor.axe = 0.0841863441;
  sensor.aye = 0.0117734062;
  sensor.aze = -1.0599359273;
  sensor.gxe = 1.4209155082;
  sensor.gye = 2.9071882247;
  sensor.aze = 0.1559407472;

[RobTillaart]

The calibration in the example i gave was generated by the GY521_readCalibration_2 as follows:
sensor.axe = 0.0786060;
sensor.aye = 0.0127197;
sensor.aze = -1.0656372;
sensor.gxe = 1.3613740;
sensor.gye = 2.9567937;
sensor.aze = 0.1717557;

Looks reasonable, no extremes.

[RobTillaart]

Changing the lines you specified makes the measurement very imprecise. (Is there some mistake in the code you included?) The values pitch and roll change at about 1 degree per second even though the GY-521 is stationary.

The code included was a remark on the website mentioned.

[RobTillaart]

Labelled issue as a bug as normalization is not ok.

Did some math (manual) and this confirmed the normalization is at least at the wrong place.
Needs more investigation.

[janq0]

So lets remove the normalization (line 173 - 192)

  //  normalize internal vars
  // if (_gax < 0) _gax += 360.0;
  // else if (_gax >= 360.0) _gax -= 360.0;
  // if (_gay < 0) _gay += 360.0;
  // else if (_gay >= 360.0) _gay -= 360.0;
  // if (_gaz < 0) _gaz += 360.0;
  // else if (_gaz >= 360.0) _gaz -= 360.0;

  _yaw   = _gaz;
  _pitch = 0.96 * _gay + 0.04 * _aay;
  _roll  = 0.96 * _gax + 0.04 * _aax;

  //  normalize 
  // if (_pitch < 0) _pitch += 360;
  // else if (_pitch >= 360) _pitch -= 360;
  //  if (_roll  < 0) _roll  += 360;
  // else if (_roll  >= 360) _roll  -= 360;
  // if (_yaw  < 0) _yaw  += 360;
  // else if (_yaw  >= 360) _yaw  -= 360;

  return GY521_OK;
}

Can you test please?

Good job! The normalization seems to cause the issue as you pointed out. The measured values are accurate when I comment out the problematic code.

[RobTillaart]

Good to hear the cause seems to be found.
Now I need (time to) find a way to get the normalization again in the code. Without it floats will eventually loose precision.

[RobTillaart]

@janq0

Can you test the version below of read()?

By normalizing the _gax and _gay at 375 degrees the math should be right again.
Then these values will keep highest precision and not "overflow" see issue #36.

Thanks for your time, appreciated!

int16_t GY521::read()
{
  uint32_t now = millis();
  if (_throttle)
  {
    if ((now - _lastTime) < _throttleTime)
    {
      //  not an error.
      return GY521_THROTTLED;
    }
  }
  _lastTime = now;

  //  Connected ?
  _wire->beginTransmission(_address);
  _wire->write(GY521_ACCEL_XOUT_H);
  if (_wire->endTransmission() != 0)
  {
    _error = GY521_ERROR_WRITE;
    return _error;
  }

  //  Get the data
  int8_t n = _wire->requestFrom(_address, (uint8_t)14);
  if (n != 14)
  {
    _error = GY521_ERROR_READ;
    return _error;
  }
  //  ACCELEROMETER
  _ax = _WireRead2();           //  ACCEL_XOUT_H  ACCEL_XOUT_L
  _ay = _WireRead2();           //  ACCEL_YOUT_H  ACCEL_YOUT_L
  _az = _WireRead2();           //  ACCEL_ZOUT_H  ACCEL_ZOUT_L
  //  TEMPERATURE
  _temperature = _WireRead2();  //  TEMP_OUT_H    TEMP_OUT_L
  //  GYROSCOPE
  _gx = _WireRead2();           //  GYRO_XOUT_H   GYRO_XOUT_L
  _gy = _WireRead2();           //  GYRO_YOUT_H   GYRO_YOUT_L
  _gz = _WireRead2();           //  GYRO_ZOUT_H   GYRO_ZOUT_L

  //  duration interval
  now = micros();
  float duration = (now - _lastMicros) * 1e-6;  // duration in seconds.
  _lastMicros = now;


  //  next lines might be merged per axis. (performance)

  //  Convert raw acceleration to g's
  _ax *= _raw2g;
  _ay *= _raw2g;
  _az *= _raw2g;

  //  Error correct raw acceleration (in g) measurements  // #18 kudos to Merkxic
  _ax += axe;
  _ay += aye;
  _az += aze;

  //  prepare for Pitch Roll Yaw
  float _ax2 = _ax * _ax;
  float _ay2 = _ay * _ay;
  float _az2 = _az * _az;

  _aax = atan(       _ay / sqrt(_ax2 + _az2)) * GY521_RAD2DEGREES;
  _aay = atan(-1.0 * _ax / sqrt(_ay2 + _az2)) * GY521_RAD2DEGREES;
  _aaz = atan(       _az / sqrt(_ax2 + _ay2)) * GY521_RAD2DEGREES;
  //  optimize #22
  //  _aax = atan(_ay / hypot(_ax, _az)) * GY521_RAD2DEGREES;
  //  _aay = atan(-1.0 * _ax / hypot(_ay, _az)) * GY521_RAD2DEGREES;
  //  _aaz = atan(_az / hypot(_ax, _ay)) * GY521_RAD2DEGREES;

  //  Convert to Celsius
  _temperature = _temperature * 0.00294117647 + 36.53;  //  == /340.0  + 36.53;

  //  Convert raw Gyro to degrees/seconds
  _gx *= _raw2dps;
  _gy *= _raw2dps;
  _gz *= _raw2dps;

  //  Error correct raw gyro measurements.
  _gx += gxe;
  _gy += gye;
  _gz += gze;

  //  _gax etc might loose precision after many iterations #36
  _gax += _gx * duration;
  _gay += _gy * duration;
  _gaz += _gz * duration;

  //  normalize
  bool _normalize = true;
  if (_normalize)
  {
    //  correction at 375 due to the factor 0.96 in roll formula
    if (_gax > 375)    _gax -= 375;
    else if (_gax < 0) _gax += 375;
    //  correction at 375 due to the factor 0.96 in pitch formula
    if (_gay > 375)    _gay -= 375;
    else if (_gay < 0) _gay += 375;
    // correction at 360
    if (_gaz > 360)    _gaz -= 360;
    else if (_gaz < 0) _gaz += 360;
  }


  //  Calculate Pitch Roll Yaw
  _yaw   = _gaz;
  _roll  = 0.96 * _gax + 0.04 * _aax;
  _pitch = 0.96 * _gay + 0.04 * _aay;


  if (_normalize)
  {
    if (_pitch > 360)    _pitch -= 360;
    else if (_pitch < 0) _pitch += 360;
    if (_roll > 360)     _roll -= 360;
    else if (_roll < 0)  _roll += 360;
    if (_yaw > 360)      _yaw -= 360;
    else if (_yaw < 0)   _yaw += 360;
  }

  return GY521_OK;
}

Might need to change > to >= as 0 == 360.

[RobTillaart]

the line bool _normalize = true; is for testing.
It will be a private variable and will get a setter and getter in the public interface.

[RobTillaart]

Created develop branch for version 0.4.0.
Build is running.

[RobTillaart]

Dev branch build is OK,
Pull Request created
PR builds OK

[RobTillaart]

This issue closed due to the PR (link).
If there are more related questions please reopen / or open a new issue.

Again, thanks for reporting!