add _readRaw()

RobTillaart · Jul 25, 2024 · 071f22b · 071f22b
1 parent e30d950
commit 071f22b
Show file tree

Hide file tree

Showing 3 changed files with 44 additions and 26 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -7,7 +7,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 
 ## [0.6.1] - 2024-07-25
-- explicit initialization of gax, gay and gaz
+- improve initialization of gax, gay and gaz
+- add **_readRaw()** to improve calibrate()
 
 
 ## [0.6.0] - 2024-06-22

diff --git a/GY521.cpp b/GY521.cpp
@@ -88,7 +88,7 @@ void GY521::calibrate(uint16_t times)
   //  summarize (6x) the measurements.
   for (uint16_t i = 0; i < times; i++)
   {
-    read();
+    _readRaw();
     _axe -= getAccelX();
     _aye -= getAccelY();
     _aze -= getAccelZ();
@@ -133,33 +133,12 @@ int16_t GY521::read()
   }
   _lastTime = now;
 
-  //  Connected ?
-  _wire->beginTransmission(_address);
-  _wire->write(GY521_ACCEL_XOUT_H);
-  if (_wire->endTransmission() != 0)
+  int16_t rv = _readRaw();
+  if (rv  != GY521_OK)
   {
-    _error = GY521_ERROR_WRITE;
-    return _error;
+    return rv;
   }
 
-  //  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.
@@ -545,6 +524,43 @@ uint8_t GY521::getRegister(uint8_t reg)
 }
 
 
+///////////////////////////////////////////////////////////////////
+//
+//  PRIVATE
+//
+int16_t GY521::_readRaw()
+{
+  //  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
+
+  return GY521_OK;
+}
+
+
 //  to read register of 2 bytes.
 int16_t GY521::_WireRead2()
 {

diff --git a/GY521.h b/GY521.h
@@ -151,6 +151,7 @@ class GY521
 
   bool     _normalize = true;       //  default true.
 
+  int16_t  _readRaw();
   //  to read register of 2 bytes.
   int16_t  _WireRead2();