Skip to content

Commit

Permalink
Fix #20 add multi-Wire support (#23)
Browse files Browse the repository at this point in the history 
* Fix #20 add multi-Wire support
  • Loading branch information
@RobTillaart
RobTillaart authored Jul 5, 2021
1 parent b705d0f commit 3f148d7
Show file tree
Hide file tree
Showing 7 changed files with 145 additions and 64 deletions.
80 changes: 46 additions & 34 deletions GY521.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
//
// FILE: GY521.cpp
// AUTHOR: Rob Tillaart
// VERSION: 0.3.1
// VERSION: 0.3.2
// PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor
// URL: https://github.com/RobTillaart/GY521
//
Expand All @@ -11,14 +11,15 @@
// 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 math for Teensy
// 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
// 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


#include "GY521.h"
Expand All @@ -35,33 +36,34 @@
//
// PUBLIC
//
GY521::GY521(uint8_t address)
GY521::GY521(uint8_t address, TwoWire *wire)
{
_address = address;
_wire = wire;
reset();
}


#if defined (ESP8266) || defined(ESP32)
bool GY521::begin(uint8_t sda, uint8_t scl)
{
Wire.begin(sda, scl);
_wire->begin(sda, scl);
return isConnected();
}
#endif


bool GY521::begin()
{
Wire.begin();
_wire->begin();
return isConnected();
}


bool GY521::isConnected()
{
Wire.beginTransmission(_address);
return (Wire.endTransmission() == 0);
_wire->beginTransmission(_address);
return (_wire->endTransmission() == 0);
}

void GY521::reset()
Expand All @@ -79,10 +81,10 @@ void GY521::reset()

bool GY521::wakeup()
{
Wire.beginTransmission(_address);
Wire.write(GY521_PWR_MGMT_1);
Wire.write(GY521_WAKEUP);
return (Wire.endTransmission() == 0);
_wire->beginTransmission(_address);
_wire->write(GY521_PWR_MGMT_1);
_wire->write(GY521_WAKEUP);
return (_wire->endTransmission() == 0);
}


Expand All @@ -92,18 +94,27 @@ int16_t GY521::read()
{
if ((millis() - _lastTime) < _throttleTime)
{
// not an error.
return GY521_THROTTLED;
}
}

// Connected ?
Wire.beginTransmission(_address);
Wire.write(GY521_ACCEL_XOUT_H);
if (Wire.endTransmission() != 0) return GY521_ERROR_WRITE;
_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) return GY521_ERROR_READ;
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
Expand Down Expand Up @@ -142,19 +153,19 @@ int16_t GY521::read()
_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;
_yaw = _gaz;
_pitch = 0.96 * _gay + 0.04 * _aay;
_roll = 0.96 * _gax + 0.04 * _aax;
_roll = 0.96 * _gax + 0.04 * _aax;

return GY521_OK;
}
Expand Down Expand Up @@ -236,11 +247,11 @@ uint8_t GY521::getGyroSensitivity()

uint8_t GY521::setRegister(uint8_t reg, uint8_t value)
{
Wire.beginTransmission(_address);
Wire.write(reg);
Wire.write(value);
_wire->beginTransmission(_address);
_wire->write(reg);
_wire->write(value);
// no need to do anything if not connected.
if (Wire.endTransmission() != 0)
if (_wire->endTransmission() != 0)
{
_error = GY521_ERROR_WRITE;
return _error;
Expand All @@ -251,31 +262,32 @@ uint8_t GY521::setRegister(uint8_t reg, uint8_t value)

uint8_t GY521::getRegister(uint8_t reg)
{
Wire.beginTransmission(_address);
Wire.write(reg);
if (Wire.endTransmission() != 0)
_wire->beginTransmission(_address);
_wire->write(reg);
if (_wire->endTransmission() != 0)
{
_error = GY521_ERROR_WRITE;
return _error;
}
uint8_t n = Wire.requestFrom(_address, (uint8_t) 1);
if (n != 1)
uint8_t n = _wire->requestFrom(_address, (uint8_t) 1);
if (n != 1)
{
_error = GY521_ERROR_READ;
return _error;
}
uint8_t val = Wire.read();
uint8_t val = _wire->read();
return val;
}


// to read register of 2 bytes.
int16_t GY521::_WireRead2()
{
int16_t tmp = Wire.read();
int16_t tmp = _wire->read();
tmp <<= 8;
tmp |= Wire.read();
tmp |= _wire->read();
return tmp;
}


// -- END OF FILE --
27 changes: 19 additions & 8 deletions GY521.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
//
// FILE: GY521.h
// AUTHOR: Rob Tillaart
// VERSION: 0.3.1
// VERSION: 0.3.2
// PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor
// URL: https://github.com/RobTillaart/GY521
//
Expand All @@ -15,7 +15,7 @@
#include "Wire.h"


#define GY521_LIB_VERSION (F("0.3.1"))
#define GY521_LIB_VERSION (F("0.3.2"))


#ifndef GY521_THROTTLE_TIME
Expand All @@ -34,7 +34,8 @@
class GY521
{
public:
GY521(uint8_t address = 0x69); // 0x68 or 0x69
GY521(uint8_t address = 0x69, TwoWire *wire = &Wire); // 0x68 or 0x69


#if defined (ESP8266) || defined(ESP32)
bool begin(uint8_t sda, uint8_t scl);
Expand All @@ -43,14 +44,16 @@ class GY521
bool isConnected();
void reset();


bool wakeup();
// throttle to force delay between reads.
void setThrottle(bool throttle = true) { _throttle = throttle; };
bool getThrottle() { return _throttle; };
// 0..65535 millis == roughly 1 minute.
// 0..65535 (max milliseconds == roughly 1 minute.
void setThrottleTime(uint16_t ti ) { _throttleTime = ti; };
uint16_t getThrottleTime() { return _throttleTime; };


// returns GY521_OK or one of the error codes above.
int16_t read();

Expand All @@ -60,8 +63,8 @@ class GY521
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; };
float getAccelY() { return _ay; };
Expand All @@ -77,16 +80,21 @@ class GY521
float getRoll() { return _roll; };
float getYaw() { return _yaw; };


// last time sensor is actually read.
uint32_t lastTime() { return _lastTime; };


// generic worker to get access to all functionality
uint8_t setRegister(uint8_t reg, uint8_t value);
uint8_t getRegister(uint8_t reg);


// get last error and reset error to OK.
int16_t getError() { return _error; _error = GY521_OK; };

// callibration errors

// calibration errors
float axe = 0, aye = 0, aze = 0; // accelerometer errors
float gxe = 0, gye = 0, gze = 0; // gyro errors

Expand All @@ -110,9 +118,12 @@ class GY521
float _pitch, _roll, _yaw; // used by user

float _temperature = 0;

// to read register of 2 bytes.
int16_t _WireRead2();

TwoWire* _wire;
};


// -- END OF FILE --
2 changes: 1 addition & 1 deletion GY521_registers.h
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
//
// FILE: GY521_registers.h
// AUTHOR: Rob Tillaart
// VERSION: 0.3.0
// VERSION: 0.3.2
// PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor
// URL: https://github.com/RobTillaart/GY521
//
Expand Down
Loading

0 comments on commit 3f148d7

Please sign in to comment.