| 26 | } |
| 27 | |
| 28 | bool HalTiltSensor::readGyro(float& gx, float& gy, float& gz) const { |
| 29 | Wire.beginTransmission(_i2cAddr); |
| 30 | Wire.write(REG_GX_L); // Start reading at Gyro X Low |
| 31 | if (Wire.endTransmission(false) != 0) { |
| 32 | return false; |
| 33 | } |
| 34 | |
| 35 | Wire.requestFrom(_i2cAddr, (uint8_t)6); |
| 36 | if (Wire.available() < 6) { |
| 37 | return false; |
| 38 | } |
| 39 | |
| 40 | auto readInt16 = [&]() -> int16_t { |
| 41 | const uint8_t lo = Wire.read(); |
| 42 | const uint8_t hi = Wire.read(); |
| 43 | return static_cast<int16_t>((hi << 8) | lo); |
| 44 | }; |
| 45 | |
| 46 | // If Full Scale is ±512 dps, the scale factor is 32768 / 512 = 64 LSB/dps |
| 47 | constexpr float SCALE = 1.0f / 64.0f; |
| 48 | gx = readInt16() * SCALE; |
| 49 | gy = readInt16() * SCALE; |
| 50 | gz = readInt16() * SCALE; |
| 51 | return true; |
| 52 | } |
| 53 | |
| 54 | void HalTiltSensor::begin() { |
| 55 | if (!gpio.deviceIsX3()) { |