0.3.8 GY521

This commit is contained in:
rob tillaart 2022-10-08 15:41:16 +02:00
parent 066b680ec7
commit 9706b6a922
9 changed files with 255 additions and 125 deletions

View File

@ -1,3 +1,18 @@
platforms:
rpipico:
board: rp2040:rp2040:rpipico
package: rp2040:rp2040
gcc:
features:
defines:
- ARDUINO_ARCH_RP2040
warnings:
flags:
packages:
rp2040:rp2040:
url: https://github.com/earlephilhower/arduino-pico/releases/download/global/package_rp2040_index.json
compile:
# Choosing to run compilation tests on 2 different Arduino platforms
platforms:
@ -8,4 +23,5 @@ compile:
- m4
- esp32
# - esp8266
# - mega2560
# - mega2560
- rpipico

View File

@ -0,0 +1,98 @@
# Change Log GY521
All notable changes to this project will be documented in this file.
The format is based on [Keep a Changelog](http://keepachangelog.com/)
and this project adheres to [Semantic Versioning](http://semver.org/).
## [0.3.8] - 2022-10-07
- added CHANGELOG.md
- fix #36 - limit values of pitch() roll() yaw() to 0..360 range.
- rename three constants, move to .h.
- add constants check to unit test.
- updated keywords
- updated build to include RP2040 pico
- updated documentation a bit.
## [0.3.7] - 2022-07-26
- add partial reads readAccel(), readGyro() and readTemperature()
- rename + add GY521_LIB_VERSION to examples.
## [0.3.6] - 2021-12-18
- update library.json, license, minor edits
## [0.3.5] - 2021-10-20
- update build-CI, badges
- fix #28 add wakeup to begin().
## [0.3.4] - 2021-07-12
- fix #24 improve precision
## [0.3.3] - 2021-07-05
- fix #22 improve maths
## [0.3.2] - 2021-07-05
- fix #20 support multiWire
## [0.3.1] - 2021-06-13
- added more unit test
- some initialization
## [0.3.0] - 2021-04-07
- fix #18 acceleration error correction (kudo's to Merkxic)
## [0.2.3] - 2021-01-26
- align version numbers (oops)
## [0.2.2] - 2021-01-24
- add interface part to readme.md
- add GY521_registers.h
## [0.2.1] - 2020-12-24
- add Arduino-CI build
- add unit tests
## [0.2.0] - 2020-11-03
- improve error handling
## [0.1.5] - 2020-09-29
- fix #6 fix maths for Teensy
## [0.1.4] - 2020-09-29
- fix #5 missing ;
## [0.1.3] - 2020-08-07
- fix ESP support
- add pitch roll yaw demo
## [0.1.2] - 2020-08-06
- fix setAccelSensitivity
- add getters
## [0.1.1] - 2020-07-09
- refactor
- initial release
## [0.1.0] - 2017-11-20
- initial version

View File

@ -1,46 +1,20 @@
//
// FILE: GY521.cpp
// AUTHOR: Rob Tillaart
// VERSION: 0.3.7
// VERSION: 0.3.8
// PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor
// URL: https://github.com/RobTillaart/GY521
//
// HISTORY:
// 0.1.0 2017-11-20 initial version
// 0.1.1 2020-07-09 refactor + initial release
// 0.1.2 2020-08-06 fix setAccelSensitivity + add getters
// 0.1.3 2020-08-07 fix ESP support + pitch roll yaw demo
// 0.1.4 2020-09-29 fix #5 missing ;
// 0.1.5 2020-09-29 fix #6 fix maths for Teensy
//
// 0.2.0 2020-11-03 improve error handling
// 0.2.1 2020-12-24 Arduino-CI + unit tests
// 0.2.2 2021-01-24 add interface part to readme.md
// add GY521_registers.h
// 0.2.3 2021-01-26 align version numbers (oops)
//
// 0.3.0 2021-04-07 fix #18 acceleration error correction (kudo's to Merkxic)
// 0.3.1 2021-06-13 added more unit test + some initialization
// 0.3.2 2021-07-05 fix #20 support multiWire
// 0.3.3 2021-07-05 fix #22 improve maths
// 0.3.4 2021-07-12 fix #24 improve precision
// 0.3.5 2021-10-20 update build-CI, badges + #28 add wakeup to begin().
// 0.3.6 2021-12-18 update library.json, license, minor edits
// 0.3.7 2022-07-26 add partial reads readAccel(),
// readGyro() and readTemperature()
// rename + add GY521_LIB_VERSION to examples.
#include "GY521.h"
// keep register names in sync with BIG MPU6050 lib
// keep register names in sync with BIG MPU6050 lib
#include "GY521_registers.h"
// COMMANDS
#define GY521_WAKEUP 0x00
#define RAD2DEGREES (180.0 / PI)
#define RAW2DPS_FACTOR (1.0 / 131.0)
#define RAW2G_FACTOR (1.0 / 16384.0)
/////////////////////////////////////////////////////
@ -85,6 +59,7 @@ bool GY521::isConnected()
return (_wire->endTransmission() == 0);
}
void GY521::reset()
{
setThrottleTime(GY521_THROTTLE_TIME);
@ -114,13 +89,13 @@ int16_t GY521::read()
{
if ((now - _lastTime) < _throttleTime)
{
// not an error.
// not an error.
return GY521_THROTTLED;
}
}
_lastTime = now;
// Connected ?
// Connected ?
_wire->beginTransmission(_address);
_wire->write(GY521_ACCEL_XOUT_H);
if (_wire->endTransmission() != 0)
@ -129,64 +104,64 @@ int16_t GY521::read()
return _error;
}
// Get the data
// 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
// 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
// duration interval
now = micros();
float duration = (now - _lastMicros) * 1e-6; // duration in seconds.
_lastMicros = now;
// next lines might be merged per axis.
// next lines might be merged per axis.
// Convert raw acceleration to g's
// Convert raw acceleration to g's
_ax *= _raw2g;
_ay *= _raw2g;
_az *= _raw2g;
// Error correct raw acceleration (in g) measurements // #18 kudos to Merkxic
// Error correct raw acceleration (in g) measurements // #18 kudos to Merkxic
_ax += axe;
_ay += aye;
_az += aze;
// prepare for Pitch Roll Yaw
// prepare for Pitch Roll Yaw
float _ax2 = _ax * _ax;
float _ay2 = _ay * _ay;
float _az2 = _az * _az;
_aax = atan( _ay / sqrt(_ax2 + _az2)) * RAD2DEGREES;
_aay = atan(-1.0 * _ax / sqrt(_ay2 + _az2)) * RAD2DEGREES;
_aaz = atan( _az / sqrt(_ax2 + _ay2)) * RAD2DEGREES;
// optimize #22
// _aax = atan(_ay / hypot(_ax, _az)) * RAD2DEGREES;
// _aay = atan(-1.0 * _ax / hypot(_ay, _az)) * RAD2DEGREES;
// _aaz = atan(_az / hypot(_ax, _ay)) * RAD2DEGREES;
_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
// Convert to Celsius
_temperature = _temperature * 0.00294117647 + 36.53; // == /340.0 + 36.53;
// Convert raw Gyro to degrees/seconds
// Convert raw Gyro to degrees/seconds
_gx *= _raw2dps;
_gy *= _raw2dps;
_gz *= _raw2dps;
// Error correct raw gyro measurements.
// Error correct raw gyro measurements.
_gx += gxe;
_gy += gye;
_gz += gze;
@ -195,10 +170,24 @@ int16_t GY521::read()
_gay += _gy * duration;
_gaz += _gz * duration;
// 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 external vars.
if (_pitch < 0) _pitch += 360;
else if (_pitch >= 360) _pitch -= 360;
if (_roll < 0) _roll += 360;
else if (_roll >= 360) _roll -= 360;
return GY521_OK;
}
@ -216,7 +205,7 @@ int16_t GY521::readAccel()
}
_lastTime = now;
// Connected ?
// Connected ?
_wire->beginTransmission(_address);
_wire->write(GY521_ACCEL_XOUT_H);
if (_wire->endTransmission() != 0)
@ -232,35 +221,35 @@ int16_t GY521::readAccel()
_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
// 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
// next lines might be merged per axis.
// next lines might be merged per axis.
// Convert raw acceleration to g's
// Convert raw acceleration to g's
_ax *= _raw2g;
_ay *= _raw2g;
_az *= _raw2g;
// Error correct raw acceleration (in g) measurements // #18 kudos to Merkxic
// Error correct raw acceleration (in g) measurements // #18 kudos to Merkxic
_ax += axe;
_ay += aye;
_az += aze;
// prepare for Pitch Roll Yaw
// prepare for Pitch Roll Yaw
float _ax2 = _ax * _ax;
float _ay2 = _ay * _ay;
float _az2 = _az * _az;
_aax = atan( _ay / sqrt(_ax2 + _az2)) * RAD2DEGREES;
_aay = atan(-1.0 * _ax / sqrt(_ay2 + _az2)) * RAD2DEGREES;
_aaz = atan( _az / sqrt(_ax2 + _ay2)) * RAD2DEGREES;
// optimize #22
// _aax = atan(_ay / hypot(_ax, _az)) * RAD2DEGREES;
// _aay = atan(-1.0 * _ax / hypot(_ay, _az)) * RAD2DEGREES;
// _aaz = atan(_az / hypot(_ax, _ay)) * RAD2DEGREES;
_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;
return GY521_OK;
}
@ -279,7 +268,7 @@ int16_t GY521::readGyro()
}
_lastTime = now;
// Connected ?
// Connected ?
_wire->beginTransmission(_address);
_wire->write(GY521_GYRO_XOUT_H);
if (_wire->endTransmission() != 0)
@ -288,31 +277,31 @@ int16_t GY521::readGyro()
return _error;
}
// Get the data
// Get the data
int8_t n = _wire->requestFrom(_address, (uint8_t)6);
if (n != 6)
{
_error = GY521_ERROR_READ;
return _error;
}
// 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
// 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
// duration interval
now = micros();
float duration = (now - _lastMicros) * 1e-6; // duration in seconds.
float duration = (now - _lastMicros) * 1e-6; // duration in seconds.
_lastMicros = now;
// next lines might be merged per axis.
// next lines might be merged per axis.
// Convert raw Gyro to degrees/seconds
// Convert raw Gyro to degrees/seconds
_gx *= _raw2dps;
_gy *= _raw2dps;
_gz *= _raw2dps;
// Error correct raw gyro measurements.
// Error correct raw gyro measurements.
_gx += gxe;
_gy += gye;
_gz += gze;
@ -321,6 +310,14 @@ int16_t GY521::readGyro()
_gay += _gy * duration;
_gaz += _gz * duration;
// 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;
return GY521_OK;
}
@ -336,15 +333,15 @@ int16_t GY521::readTemperature()
return _error;
}
// Get the data
// Get the data
int8_t n = _wire->requestFrom(_address, (uint8_t)2);
if (n != 2)
{
_error = GY521_ERROR_READ;
return _error;
}
// TEMPERATURE
_temperature = _WireRead2(); // TEMP_OUT_H TEMP_OUT_L
// TEMPERATURE
_temperature = _WireRead2(); // TEMP_OUT_H TEMP_OUT_L
return GY521_OK;
}
@ -369,7 +366,7 @@ bool GY521::setAccelSensitivity(uint8_t as)
}
}
// calculate conversion factor. // 4 possible values => lookup table?
_raw2g = (1 << _afs) * RAW2G_FACTOR;
_raw2g = (1 << _afs) * GY521_RAW2G;
return true;
}
@ -395,7 +392,7 @@ bool GY521::setGyroSensitivity(uint8_t gs)
{
return false;
}
// no need to write same value
// no need to write same value
if (((val >> 3) & 3) != _gfs)
{
val &= 0xE7;
@ -405,8 +402,9 @@ bool GY521::setGyroSensitivity(uint8_t gs)
return false;
}
}
// calculate conversion factor.. // 4 possible values => lookup table?
_raw2dps = (1 << _gfs) * RAW2DPS_FACTOR;
// calculate conversion factor..
// 4 possible values => lookup table?
_raw2dps = (1 << _gfs) * GY521_RAW2DPS;
return true;
}
@ -416,7 +414,7 @@ uint8_t GY521::getGyroSensitivity()
uint8_t val = getRegister(GY521_GYRO_CONFIG);
if (_error != GY521_OK)
{
return _error; // return and propagate error (best thing to do)
return _error; // return and propagate error (best thing to do)
}
_gfs = (val >> 3) & 3;
return _gfs;
@ -428,7 +426,7 @@ uint8_t GY521::setRegister(uint8_t reg, uint8_t value)
_wire->beginTransmission(_address);
_wire->write(reg);
_wire->write(value);
// no need to do anything if not connected.
// no need to do anything if not connected.
if (_wire->endTransmission() != 0)
{
_error = GY521_ERROR_WRITE;
@ -458,7 +456,7 @@ uint8_t GY521::getRegister(uint8_t reg)
}
// to read register of 2 bytes.
// to read register of 2 bytes.
int16_t GY521::_WireRead2()
{
int16_t tmp = _wire->read();

View File

@ -2,22 +2,20 @@
//
// FILE: GY521.h
// AUTHOR: Rob Tillaart
// VERSION: 0.3.7
// VERSION: 0.3.8
// PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor
// URL: https://github.com/RobTillaart/GY521
//
// HISTORY:
// see GY521.cpp file
//
#include "Arduino.h"
#include "Wire.h"
#define GY521_LIB_VERSION (F("0.3.7"))
#define GY521_LIB_VERSION (F("0.3.8"))
// THROTTLE TIMING
#ifndef GY521_THROTTLE_TIME
#define GY521_THROTTLE_TIME 10 // milliseconds
#endif
@ -31,6 +29,12 @@
#define GY521_ERROR_NOT_CONNECTED -3
// CONVERSION CONSTANTS
#define GY521_RAD2DEGREES (180.0 / PI)
#define GY521_RAW2DPS (1.0 / 131.0)
#define GY521_RAW2G (1.0 / 16384.0)
class GY521
{
public:
@ -69,10 +73,10 @@ public:
// SET BEFORE READ
// as = 0,1,2,3 ==> 2g 4g 8g 16g
bool setAccelSensitivity(uint8_t as);
uint8_t getAccelSensitivity(); // returns 0,1,2,3
uint8_t getAccelSensitivity(); // returns 0,1,2,3
// gs = 0,1,2,3 ==> 250, 500, 1000, 2000 degrees/second
bool setGyroSensitivity(uint8_t gs);
uint8_t getGyroSensitivity(); // returns 0,1,2,3
uint8_t getGyroSensitivity(); // returns 0,1,2,3
// CALL AFTER READ
float getAccelX() { return _ax; };
@ -104,32 +108,32 @@ public:
// calibration errors
float axe = 0, aye = 0, aze = 0; // accelerometer errors
float gxe = 0, gye = 0, gze = 0; // gyro errors
float axe = 0, aye = 0, aze = 0; // accelerometer errors
float gxe = 0, gye = 0, gze = 0; // gyro errors
private:
uint8_t _address; // I2C address
bool _throttle = true; // to prevent reading too fast
uint8_t _address; // I2C address
bool _throttle = true; // to prevent reading too fast
uint16_t _throttleTime = GY521_THROTTLE_TIME;
uint32_t _lastTime = 0; // to measure duration for math & throttle
uint32_t _lastMicros = 0; // to measure duration for math & throttle
int16_t _error = GY521_OK; // initially everything is OK
uint32_t _lastTime = 0; // to measure duration for math & throttle
uint32_t _lastMicros = 0; // to measure duration for math & throttle
int16_t _error = GY521_OK; // initially everything is OK
uint8_t _afs = 0; // sensitivity factor
float _raw2g = 1.0/16384.0; // raw data to gravity g's
float _ax, _ay, _az; // accelerometer raw
float _aax, _aay, _aaz; // accelerometer processed
uint8_t _afs = 0; // sensitivity factor
float _raw2g = GY521_RAW2G; // raw data to gravity g's
float _ax, _ay, _az; // accelerometer raw
float _aax, _aay, _aaz; // accelerometer processed
uint8_t _gfs = 0;
float _raw2dps = 1.0/131.0;
float _gx, _gy, _gz; // gyro raw
float _gax, _gay, _gaz; // gyro processed.
float _pitch, _roll, _yaw; // used by user
float _raw2dps = GY521_RAW2DPS;
float _gx, _gy, _gz; // gyro raw
float _gax, _gay, _gaz; // gyro processed.
float _pitch, _roll, _yaw; // used by user
float _temperature = 0;
// to read register of 2 bytes.
// to read register of 2 bytes.
int16_t _WireRead2();
TwoWire* _wire;

View File

@ -102,6 +102,11 @@ Note: for pitch roll yaw you need to call **read()**.
returns status = GY521_OK on success.
- **uint32_t lastTime()** last time sensor is actually read. In milliseconds. Not updated by readTemperature().
Since version 0.3.8 the **read()** and **readGyro()** function is updated to keep the range of **getPitch()**,
**getRoll()** and **getYaw()** in the range 0..360 degrees. (Issue #36).
Problem is that with continuous roatation in a same direction internal variables will overflow and new
movements (angles) will get lost as insignificant digits.
#### Call after read
@ -117,9 +122,9 @@ Note that multiple calls will return the same value. One must explicitly call **
- **float getGyroX()** idem
- **float getGyroY()** idem
- **float getGyroZ()** idem
- **float getPitch()** idem
- **float getRoll()** idem
- **float getYaw()** idem
- **float getPitch()** idem. Returns 0.00 - 359.99.
- **float getRoll()** idem. Returns 0.00 - 359.99.
- **float getYaw()** idem. Returns 0.00 - 359.99.
### Register access

View File

@ -53,4 +53,9 @@ GY521_OK LITERAL1
GY521_THROTTLED LITERAL1
GY521_ERROR_READ LITERAL1
GY521_ERROR_WRITE LITERAL1
GY521_ERROR_NOT_CONNECTED LITERAL1
GY521_ERROR_NOT_CONNECTED LITERAL1
GY521_RAD2DEGREES LITERAL1
GY521_RAW2DPS LITERAL1
GY521_RAW2G LITERAL1

View File

@ -15,7 +15,7 @@
"type": "git",
"url": "https://github.com/RobTillaart/GY521.git"
},
"version": "0.3.7",
"version": "0.3.8",
"license": "MIT",
"frameworks": "arduino",
"platforms": "*",

View File

@ -1,5 +1,5 @@
name=GY521
version=0.3.7
version=0.3.8
author=Rob Tillaart <rob.tillaart@gmail.com>
maintainer=Rob Tillaart <rob.tillaart@gmail.com>
sentence=Arduino library for GY521 angle measurement

View File

@ -105,6 +105,10 @@ unittest(test_constants)
assertEqual(GY521_ERROR_NOT_CONNECTED, -3);
assertEqual(GY521_THROTTLE_TIME, 10);
assertEqualFloat(GY521_RAD2DEGREES, 180.0 / PI, 0.001);
assertEqualFloat(GY521_RAW2DPS, 1.0 / 131.0, 0.001);
assertEqualFloat(GY521_RAW2G, 1.0 / 16384.0, 0.000001);
}