| 256 | |
| 257 | |
| 258 | int16_t GY521::readGyro() |
| 259 | { |
| 260 | uint32_t now = millis(); |
| 261 | if (_throttle) |
| 262 | { |
| 263 | if ((now - _lastTime) < _throttleTime) |
| 264 | { |
| 265 | // not an error. |
| 266 | return GY521_THROTTLED; |
| 267 | } |
| 268 | } |
| 269 | _lastTime = now; |
| 270 | |
| 271 | // Connected ? |
| 272 | _wire->beginTransmission(_address); |
| 273 | _wire->write(GY521_GYRO_XOUT_H); |
| 274 | if (_wire->endTransmission() != 0) |
| 275 | { |
| 276 | _error = GY521_ERROR_WRITE; |
| 277 | return _error; |
| 278 | } |
| 279 | |
| 280 | // Get the data |
| 281 | int8_t n = _wire->requestFrom(_address, (uint8_t)6); |
| 282 | if (n != 6) |
| 283 | { |
| 284 | _error = GY521_ERROR_READ; |
| 285 | return _error; |
| 286 | } |
| 287 | // GYROSCOPE |
| 288 | _gx = _WireRead2(); // GYRO_XOUT_H GYRO_XOUT_L |
| 289 | _gy = _WireRead2(); // GYRO_YOUT_H GYRO_YOUT_L |
| 290 | _gz = _WireRead2(); // GYRO_ZOUT_H GYRO_ZOUT_L |
| 291 | |
| 292 | // duration interval |
| 293 | now = micros(); |
| 294 | float duration = (now - _lastMicros) * 1e-6; // duration in seconds. |
| 295 | _lastMicros = now; |
| 296 | |
| 297 | // next lines might be merged per axis. |
| 298 | |
| 299 | // Convert raw Gyro to degrees/seconds |
| 300 | _gx *= _raw2dps; |
| 301 | _gy *= _raw2dps; |
| 302 | _gz *= _raw2dps; |
| 303 | |
| 304 | // Error correct raw gyro measurements. |
| 305 | _gx += gxe; |
| 306 | _gy += gye; |
| 307 | _gz += gze; |
| 308 | |
| 309 | _gax += _gx * duration; |
| 310 | _gay += _gy * duration; |
| 311 | _gaz += _gz * duration; |
| 312 | |
| 313 | // normalize internal vars |
| 314 | if (_gax < 0) _gax += 360.0; |
| 315 | else if (_gax >= 360.0) _gax -= 360.0; |