From f14d4380097f4fdca75d328276837b1b8e5c3d26 Mon Sep 17 00:00:00 2001 From: rob tillaart Date: Tue, 14 Jul 2020 16:02:40 +0200 Subject: [PATCH] 0.1.1 experimental version --- GY521.cpp | 163 +++++++++++++++++++ GY521.h | 91 +++++++++++ LICENSE | 2 +- README.md | 41 ++++- examples/readCalibration/readCalibration.ino | 110 +++++++++++++ examples/test1/test1.ino | 79 +++++++++ keywords.txt | 41 +++++ library.json | 21 +++ library.properties | 11 ++ 9 files changed, 557 insertions(+), 2 deletions(-) create mode 100644 GY521.cpp create mode 100644 GY521.h create mode 100644 examples/readCalibration/readCalibration.ino create mode 100644 examples/test1/test1.ino create mode 100644 keywords.txt create mode 100644 library.json create mode 100644 library.properties diff --git a/GY521.cpp b/GY521.cpp new file mode 100644 index 0000000..546ae96 --- /dev/null +++ b/GY521.cpp @@ -0,0 +1,163 @@ +// +// FILE: GY521.cpp +// AUTHOR: Rob Tillaart +// VERSION: 0.1.1 +// 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 + +#include "Wire.h" +#include "GY521.h" + +// keep names in sync with BIG MPU6050 lib +#define GY521_GYRO_CONFIG 0x1B +#define GY521_ACCEL_CONFIG 0x1C + +#define GY521_ACCEL_XOUT_H 0x3B +#define GY521_TEMP_OUT_H 0x41 +#define GY521_GYRO_XOUT_H 0x43 + +#define GY521_PWR_MGMT_1 0x6B +#define GY521_WAKEUP 0x00 + +#define RAD2DEGREES (180.0 / PI) + + +///////////////////////////////////////////////////// +// +// PUBLIC +// +GY521::GY521(uint8_t address) +{ + _address = 0x69; + if (address == 0x68) _address = 0x68; +} + +bool GY521::wakeup() +{ + Wire.beginTransmission(_address); + Wire.write(GY521_PWR_MGMT_1); + Wire.write(GY521_WAKEUP); + return Wire.endTransmission() == 0; +} + + +int GY521::read() +{ + if (_throttle) + { + if ((millis() - _lastTime) < GY521_THROTTLE_TIME) return GY521_THROTTLED; + } + + // Connected ? + Wire.beginTransmission(_address); + Wire.write(GY521_ACCEL_XOUT_H); + if (Wire.endTransmission() != 0) return GY521_ERROR_READ; + + // Get the data + Wire.requestFrom(_address, (uint8_t)14); + // ACCELEROMETER + _ax = ( ( ((int)Wire.read()) << 8) | Wire.read() ); // ACCEL_XOUT_H ACCEL_XOUT_L + _ay = ( ( ((int)Wire.read()) << 8) | Wire.read() ); // ACCEL_YOUT_H ACCEL_YOUT_L + _az = ( ( ((int)Wire.read()) << 8) | Wire.read() ); // ACCEL_ZOUT_H ACCEL_ZOUT_L + // TEMPERATURE + _temperature = ( ((int)Wire.read()) << 8) | Wire.read(); // TEMP_OUT_H TEMP_OUT_L + // GYROSCOPE + _gx = (( ((int)Wire.read()) << 8) | Wire.read()); // GYRO_XOUT_H GYRO_XOUT_L + _gy = (( ((int)Wire.read()) << 8) | Wire.read()); // GYRO_YOUT_H GYRO_YOUT_L + _gz = (( ((int)Wire.read()) << 8) | Wire.read()); // GYRO_ZOUT_H GYRO_ZOUT_L + + // time interval + uint32_t now = millis(); + float duration = (now - _lastTime) * 0.001; // time in seconds. + _lastTime = now; + + // Convert raw acceleration to g's + _ax *= _raw2g; + _ay *= _raw2g; + _az *= _raw2g; + + // prepare for Pitch Roll Yaw + _aax = atan(_ay / hypot(_ax, _az)) * RAD2DEGREES; + _aay = atan(-1.0 * _ax / hypot(_ay, _az)) * RAD2DEGREES; + _aaz = atan(_az / hypot(_ax, _ay)) * RAD2DEGREES; + + // Error correct the angles + _aax += axe; + _aay += aye; + _aaz += aze; + + // Convert to Celsius + _temperature = (_temperature + 12412.0) * 0.00294117647; // == /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 += _gx * duration; + _gay += _gy * duration; + _gaz += _gz * duration; + + _yaw = _gaz; + _pitch = 0.96 * _gay + 0.04 * _aay; + _roll = 0.96 * _gax + 0.04 * _aax; + + return GY521_OK; +} + +void GY521::setAccelSensitivity(uint8_t as) +{ + if (as > 3) as = 3; + uint8_t val = getRegister(GY521_ACCEL_CONFIG); + val &= 0xE7; + val |= (as << 3); + setRegister(GY521_ACCEL_CONFIG, val); + // calculate conversion factor. + _raw2g = 16384.0; + for (uint8_t i = 0; i < _afs; i++) _raw2g *= 0.5; + _raw2g = 1.0 / _raw2g; +} + +void GY521::setGyroSensitivity(uint8_t gs) +{ + _gfs = gs; + if (_gfs > 3) _gfs = 3; + uint8_t val = getRegister(GY521_GYRO_CONFIG); + val &= 0xE7; + val |= (_gfs << 3); + setRegister(GY521_GYRO_CONFIG, val); + _raw2dps = 131.0; + for (uint8_t i = 0; i < _gfs; i++) _raw2dps *= 0.5; + _raw2dps = 1.0 / _raw2dps; +} + +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. + if (Wire.endTransmission() != 0) return GY521_ERROR_WRITE; + return GY521_OK; +} + +uint8_t GY521::getRegister(uint8_t reg) +{ + Wire.beginTransmission(_address); + Wire.write(reg); + if (Wire.endTransmission() != 0) return GY521_ERROR_WRITE; + Wire.requestFrom(_address, (uint8_t) 1); + uint8_t val = Wire.read(); + return val; +} + +// END OF FILE diff --git a/GY521.h b/GY521.h new file mode 100644 index 0000000..75cd4b5 --- /dev/null +++ b/GY521.h @@ -0,0 +1,91 @@ +#pragma once +// +// FILE: GY521.h +// AUTHOR: Rob Tillaart +// VERSION: 0.1.1 +// 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.1.1")) + +#ifndef GY521_THROTTLE_TIME +#define GY521_THROTTLE_TIME 10 // milliseconds +#endif + +// ERRORCODES getRaw() +#define GY521_OK 0 +#define GY521_THROTTLED 1 +#define GY521_ERROR_READ -1 +#define GY521_ERROR_WRITE -2 + + +class GY521 +{ +public: + GY521(uint8_t address = 0x69); // 0x68 or 0x69 + + bool wakeup(); + // throttle to force delay between reads. + void setThrottle(bool throttle = true) { _throttle = throttle; }; + bool getThrottle() { return _throttle; }; + int read(); + + // as = 0,1,2,3 ==> 2g 4g 8g 16g + void setAccelSensitivity(uint8_t as); + float getAccelX() { return _ax; }; + float getAccelY() { return _ay; }; + float getAccelZ() { return _az; }; + float getAngleX() { return _aax; }; + float getAngleY() { return _aay; }; + float getAngleZ() { return _aaz; }; + + float getTemperature() { return _temperature; }; + + // gs = 0,1,2,3 ==> 250, 500, 1000, 2000 degrees/second + void setGyroSensitivity(uint8_t gs); + float getGyroX() { return _gx; }; + float getGyroY() { return _gy; }; + float getGyroZ() { return _gz; }; + + float getPitch() { return _pitch; }; + float getRoll() { return _roll; }; + float getYaw() { return _yaw; }; + + uint32_t lastTime() { return _lastTime; }; + + // generic worker to get access to all fucntionality + uint8_t setRegister(uint8_t reg, uint8_t value); + uint8_t getRegister(uint8_t reg); + + // callibration 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 + uint32_t _lastTime = 0; // to measure duration for math & throttle + + uint8_t _afs = 0; // sensitivity factor + float _raw2g = 1/16384.0; // 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/131; + float _gx, _gy, _gz; // gyro raw + float _gax, _gay, _gaz; // gyro processed. + float _pitch, _roll, _yaw; // used by user + + float _temperature = 0; +}; + + +// END OF FILE \ No newline at end of file diff --git a/LICENSE b/LICENSE index 3d51ae3..ed401f2 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ MIT License -Copyright (c) 2020 Rob Tillaart +Copyright (c) 2017-2020 Rob Tillaart Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/README.md b/README.md index 51bcfdd..c2f06cc 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,41 @@ # GY521 -Arduino library for GY521 accelerometer- gyroscope a.k.a. MCU-6050 + +Arduino library for I2C GY521 accelerometer-gyroscope sensor a.k.a. MCU-6050 + +## Description + +Experimental library for GY521 a.k.a. MCU-6050 + +Library is work in progress, in fact extracted and extended from an old project. +It needs to be tested a lot more. + +It has two examples +- calibration example to determine the offsets needed +- example to read values. + +## Calibration (short version for now) + +1. load and run calibration example + it shows a header containing 6 numbers and 10 lines og 8 numbers +1. wait until the middle 6 of the longer lines stabilize (should be all 0) +1. copy the 6 numbers above the axe aye aze as these are the numbers needed. + + +## Future + +- test test and test ...(ESP too) +- pitch roll yaw example +- better documentation +- look for math optimizations (atan/hypot) +- calibrate function in the lib ? +- ESP wire support (other pins) +- ... + +## documents + +- check details - MPU-6000-Register-Map1.pdf + +## Operation + +See examples, use with care + diff --git a/examples/readCalibration/readCalibration.ino b/examples/readCalibration/readCalibration.ino new file mode 100644 index 0000000..a1111d1 --- /dev/null +++ b/examples/readCalibration/readCalibration.ino @@ -0,0 +1,110 @@ +// +// FILE: readCalibration.ino +// AUTHOR: Rob Tillaart +// VERSION: 0.1.0 +// PURPOSE: read the calibration values / errors for a flat sensor. +// DATE: 2020-07-14 + +#include "GY521.h" + +GY521 sensor(0x69); + +uint32_t counter = 0; + +float ax, ay, az; +float gx, gy, gz; +float t; + +void setup() +{ + Serial.begin(115200); + Serial.println(__FILE__); + + Wire.begin(); + delay(100); + if (sensor.wakeup() == false) + { + Serial.println("Could not conect to GY521"); + } + // adjust when needed. + sensor.setAccelSensitivity(0); // 2g + sensor.setGyroSensitivity(0); // 250 degrees/s + sensor.setThrottle(false); + + // set all callibration errors to zero + sensor.axe = 0; + sensor.aye = 0; + sensor.aze = 0; + sensor.gxe = 0; + sensor.gye = 0; + sensor.gze = 0; + + Serial.println("\n\nReading calibration numbers..."); +} + +void loop() +{ + ax = ay = az = 0; + gx = gy = gz = 0; + t = 0; + for (int i = 0; i < 20; i++) + { + sensor.read(); + ax -= sensor.getAngleX(); + ay -= sensor.getAngleY(); + az -= sensor.getAngleZ(); + gx -= sensor.getGyroX(); + gy -= sensor.getGyroY(); + gz -= sensor.getGyroZ(); + t += sensor.getTemperature(); + } + + if (counter % 10 == 0) + { + Serial.println("\n\tACCELEROMETER\t\tGYROSCOPE\t\tTEMPERATURE"); + Serial.print('\t'); + Serial.print(sensor.axe, 3); + Serial.print('\t'); + Serial.print(sensor.aye, 3); + Serial.print('\t'); + Serial.print(sensor.aze, 3); + Serial.print('\t'); + Serial.print(sensor.gxe, 3); + Serial.print('\t'); + Serial.print(sensor.gye, 3); + Serial.print('\t'); + Serial.print(sensor.gze, 3); + Serial.print('\n'); + Serial.println("\taxe\taye\taze\tgxe\tgye\tgze\tT"); + } + + Serial.print(counter); + Serial.print('\t'); + Serial.print(ax * 0.05, 3); + Serial.print('\t'); + Serial.print(ay * 0.05, 3); + Serial.print('\t'); + Serial.print(az * 0.05, 3); + Serial.print('\t'); + Serial.print(gx * 0.05, 3); + Serial.print('\t'); + Serial.print(gy * 0.05, 3); + Serial.print('\t'); + Serial.print(gz * 0.05, 3); + Serial.print('\t'); + Serial.print(t * 0.05, 2); + Serial.println(); + + // adjust calibration errors so table should get all zero's. + sensor.axe += ax * 0.05; + sensor.aye += ay * 0.05; + sensor.aze += az * 0.05; + sensor.gxe += gx * 0.05; + sensor.gye += gy * 0.05; + sensor.gze += gz * 0.05; + + counter++; + delay(100); +} + +// -- END OF FILE -- diff --git a/examples/test1/test1.ino b/examples/test1/test1.ino new file mode 100644 index 0000000..08449e9 --- /dev/null +++ b/examples/test1/test1.ino @@ -0,0 +1,79 @@ +// +// FILE: test1.ino +// AUTHOR: Rob Tillaart +// VERSION: 0.1.0 +// PURPOSE: minimal demo +// DATE: 2020-07-01 + +#include "GY521.h" + +GY521 sensor(0x69); + +uint32_t counter = 0; + +void setup() +{ + Serial.begin(115200); + Serial.println(__FILE__); + + Wire.begin(); + + delay(100); + if (sensor.wakeup() == false) + { + Serial.println("Could not conect to GY521"); + } + sensor.setAccelSensitivity(0); // 2g + sensor.setGyroSensitivity(0); // 250 degrees/s + + sensor.setThrottle(); + Serial.println("start..."); + + // set callibration values from calibration sketch. + sensor.axe = 0; + sensor.aye = 0; + sensor.aze = 0; + sensor.gxe = 0; + sensor.gye = 0; + sensor.gze = 0; +} + +void loop() +{ + sensor.read(); + int ax = sensor.getAngleX(); + int ay = sensor.getAngleY(); + int az = sensor.getAngleZ(); + int gx = sensor.getGyroX(); + int gy = sensor.getGyroY(); + int gz = sensor.getGyroZ(); + int t = sensor.getTemperature(); + + if (counter % 10 == 0) + { + Serial.println("\n\tACCELEROMETER\t\tGYROSCOPE\t\tTEMPERATURE"); + Serial.println("\tax\tay\taz\tgx\tgy\tgz\tT"); + } + + Serial.print(counter); + Serial.print('\t'); + Serial.print(ax); + Serial.print('\t'); + Serial.print(ay); + Serial.print('\t'); + Serial.print(az); + Serial.print('\t'); + Serial.print(gx); + Serial.print('\t'); + Serial.print(gy); + Serial.print('\t'); + Serial.print(gz); + Serial.print('\t'); + Serial.print(t); + Serial.println(); + + counter++; + delay(1000); +} + +// -- END OF FILE -- diff --git a/keywords.txt b/keywords.txt new file mode 100644 index 0000000..3f96662 --- /dev/null +++ b/keywords.txt @@ -0,0 +1,41 @@ +# Syntax Coloring Map for GY521 + + +# Datatypes (KEYWORD1) +GY521 KEYWORD1 + + +# Methods and Functions (KEYWORD2) +wakeup KEYWORD2 +read KEYWORD2 +setThrottle KEYWORD2 +getThrottle KEYWORD2 + +setAccelSensitivity KEYWORD2 +getAccelX KEYWORD2 +getAccelY KEYWORD2 +getAccelZ KEYWORD2 +getAngleX KEYWORD2 +getAngleY KEYWORD2 +getAngleZ KEYWORD2 +getTemperature KEYWORD2 + +setGyroSensitivity KEYWORD2 +getGyroX KEYWORD2 +getGyroY KEYWORD2 +getGyroZ KEYWORD2 +getPitch KEYWORD2 +getRoll KEYWORD2 +getYaw KEYWORD2 + +lastTime KEYWORD2 +setRegister KEYWORD2 +getRegister KEYWORD2 + +# Constants (LITERAL1) +GY521_LIB_VERSION LITERAL1 +GY521_THROTTLE_TIME LITERAL1 +GY521_OK LITERAL1 +GY521_THROTTLED LITERAL1 +GY521_ERROR_READ LITERAL1 +GY521_ERROR_WRITE LITERAL1 \ No newline at end of file diff --git a/library.json b/library.json new file mode 100644 index 0000000..c1276f9 --- /dev/null +++ b/library.json @@ -0,0 +1,21 @@ +{ + "name": "GY521", + "keywords": "GY521,Angle,Acceleration,gyro", + "description": "Arduino library for GY521 angle measurement.", + "authors": + [ + { + "name": "Rob Tillaart", + "email": "Rob.Tillaart@gmail.com", + "maintainer": true + } + ], + "repository": + { + "type": "git", + "url": "https://github.com/RobTillaart/GY521.git" + }, + "version":"0.1.1", + "frameworks": "arduino", + "platforms": "*" +} diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..06a18c1 --- /dev/null +++ b/library.properties @@ -0,0 +1,11 @@ +name=GY521 +version=0.1.1 +author=Rob Tillaart +maintainer=Rob Tillaart +sentence=Arduino library for GY521 angle measurement +paragraph= +category=Sensors +url=https://github.com/RobTillaart/GY521 +architectures=* +includes=GY521.h +depends= \ No newline at end of file