alex/GY-63_MS5611
1
0
Fork 0
mirror of https://github.com/RobTillaart/Arduino.git synced 2024-10-03 18:09:02 -04:00
Code Issues Actions Packages Projects Releases Wiki Activity
4c15fc1036
GY-63_MS5611/libraries/GY521/GY521.cpp
rob tillaart 9706b6a922 0.3.8 GY521
2022-10-08 15:41:16 +02:00

471 lines
9.7 KiB
C++
Raw Blame History

//
// FILE: GY521.cpp
// AUTHOR: Rob Tillaart
// VERSION: 0.3.8
// PURPOSE: Arduino library for I2C GY521 accelerometer-gyroscope sensor
// URL: https://github.com/RobTillaart/GY521
#include "GY521.h"
// keep register names in sync with BIG MPU6050 lib
#include "GY521_registers.h"
// COMMANDS
#define GY521_WAKEUP 0x00
/////////////////////////////////////////////////////
//
// PUBLIC
//
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);
if (isConnected())
{
return wakeup();
}
return false;
}
#endif
bool GY521::begin()
{
_wire->begin();
if (isConnected())
{
return wakeup();
}
return false;
}
bool GY521::isConnected()
{
_wire->beginTransmission(_address);
return (_wire->endTransmission() == 0);
}
void GY521::reset()
{
setThrottleTime(GY521_THROTTLE_TIME);
_ax = _ay = _az = 0;
_aax = _aay = _aaz = 0;
_gx = _gy = _gz = 0;
_pitch = 0;
_roll = 0;
_yaw = 0;
}
bool GY521::wakeup()
{
_wire->beginTransmission(_address);
_wire->write(GY521_PWR_MGMT_1);
_wire->write(GY521_WAKEUP);
return (_wire->endTransmission() == 0);
}
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.
// 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 += _gx * duration;
_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;
}
int16_t GY521::readAccel()
{
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)6);
if (n != 6)
{
_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
// next lines might be merged per axis.
// 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;
return GY521_OK;
}
int16_t GY521::readGyro()
{
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_GYRO_XOUT_H);
if (_wire->endTransmission() != 0)
{
_error = GY521_ERROR_WRITE;
return _error;
}
// 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
// duration interval
now = micros();
float duration = (now - _lastMicros) * 1e-6; // duration in seconds.
_lastMicros = now;
// next lines might be merged per axis.
// 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;
// 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;
}
int16_t GY521::readTemperature()
{
// DO NOT THROTTLE
_wire->beginTransmission(_address);
_wire->write(GY521_TEMP_OUT_H);
if (_wire->endTransmission() != 0)
{
_error = GY521_ERROR_WRITE;
return _error;
}
// 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
return GY521_OK;
}
bool GY521::setAccelSensitivity(uint8_t as)
{
_afs = as;
if (_afs > 3) _afs = 3;
uint8_t val = getRegister(GY521_ACCEL_CONFIG);
if (_error != 0)
{
return false;
}
// no need to write same value
if (((val >> 3) & 3) != _afs)
{
val &= 0xE7;
val |= (_afs << 3);
if (setRegister(GY521_ACCEL_CONFIG, val) != GY521_OK)
{
return false;
}
}
// calculate conversion factor. // 4 possible values => lookup table?
_raw2g = (1 << _afs) * GY521_RAW2G;
return true;
}
uint8_t GY521::getAccelSensitivity()
{
uint8_t val = getRegister(GY521_ACCEL_CONFIG);
if (_error != GY521_OK)
{
return _error; // return and propagate error (best thing to do)
}
_afs = (val >> 3) & 3;
return _afs;
}
bool GY521::setGyroSensitivity(uint8_t gs)
{
_gfs = gs;
if (_gfs > 3) _gfs = 3;
uint8_t val = getRegister(GY521_GYRO_CONFIG);
if (_error != 0)
{
return false;
}
// no need to write same value
if (((val >> 3) & 3) != _gfs)
{
val &= 0xE7;
val |= (_gfs << 3);
if (setRegister(GY521_GYRO_CONFIG, val) != GY521_OK)
{
return false;
}
}
// calculate conversion factor..
// 4 possible values => lookup table?
_raw2dps = (1 << _gfs) * GY521_RAW2DPS;
return true;
}
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)
}
_gfs = (val >> 3) & 3;
return _gfs;
}
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)
{
_error = GY521_ERROR_WRITE;
return _error;
}
return GY521_OK;
}
uint8_t GY521::getRegister(uint8_t reg)
{
_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)
{
_error = GY521_ERROR_READ;
return _error;
}
uint8_t val = _wire->read();
return val;
}
// to read register of 2 bytes.
int16_t GY521::_WireRead2()
{
int16_t tmp = _wire->read();
tmp <<= 8;
tmp |= _wire->read();
return tmp;
}
// -- END OF FILE --
Reference in New Issue View Git Blame Copy Permalink