| 193 | |
| 194 | |
| 195 | int16_t GY521::readAccel() |
| 196 | { |
| 197 | uint32_t now = millis(); |
| 198 | if (_throttle) |
| 199 | { |
| 200 | if ((now - _lastTime) < _throttleTime) |
| 201 | { |
| 202 | // not an error. |
| 203 | return GY521_THROTTLED; |
| 204 | } |
| 205 | } |
| 206 | _lastTime = now; |
| 207 | |
| 208 | // Connected ? |
| 209 | _wire->beginTransmission(_address); |
| 210 | _wire->write(GY521_ACCEL_XOUT_H); |
| 211 | if (_wire->endTransmission() != 0) |
| 212 | { |
| 213 | _error = GY521_ERROR_WRITE; |
| 214 | return _error; |
| 215 | } |
| 216 | |
| 217 | // Get the data |
| 218 | int8_t n = _wire->requestFrom(_address, (uint8_t)6); |
| 219 | if (n != 6) |
| 220 | { |
| 221 | _error = GY521_ERROR_READ; |
| 222 | return _error; |
| 223 | } |
| 224 | // ACCELEROMETER |
| 225 | _ax = _WireRead2(); // ACCEL_XOUT_H ACCEL_XOUT_L |
| 226 | _ay = _WireRead2(); // ACCEL_YOUT_H ACCEL_YOUT_L |
| 227 | _az = _WireRead2(); // ACCEL_ZOUT_H ACCEL_ZOUT_L |
| 228 | |
| 229 | // next lines might be merged per axis. |
| 230 | |
| 231 | // Convert raw acceleration to g's |
| 232 | _ax *= _raw2g; |
| 233 | _ay *= _raw2g; |
| 234 | _az *= _raw2g; |
| 235 | |
| 236 | // Error correct raw acceleration (in g) measurements // #18 kudos to Merkxic |
| 237 | _ax += axe; |
| 238 | _ay += aye; |
| 239 | _az += aze; |
| 240 | |
| 241 | // prepare for Pitch Roll Yaw |
| 242 | float _ax2 = _ax * _ax; |
| 243 | float _ay2 = _ay * _ay; |
| 244 | float _az2 = _az * _az; |
| 245 | |
| 246 | _aax = atan( _ay / sqrt(_ax2 + _az2)) * GY521_RAD2DEGREES; |
| 247 | _aay = atan(-1.0 * _ax / sqrt(_ay2 + _az2)) * GY521_RAD2DEGREES; |
| 248 | _aaz = atan( _az / sqrt(_ax2 + _ay2)) * GY521_RAD2DEGREES; |
| 249 | // optimize #22 |
| 250 | // _aax = atan(_ay / hypot(_ax, _az)) * GY521_RAD2DEGREES; |
| 251 | // _aay = atan(-1.0 * _ax / hypot(_ay, _az)) * GY521_RAD2DEGREES; |
| 252 | // _aaz = atan(_az / hypot(_ax, _ay)) * GY521_RAD2DEGREES; |