Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Gyroscope.cpp
- Revision:
- 11:d21275e60ebb
- Parent:
- 8:cba37530d480
- Child:
- 12:1632d7391453
diff -r 670bba8a9a50 -r d21275e60ebb Gyroscope.cpp --- a/Gyroscope.cpp Tue Feb 24 10:10:14 2015 +0000 +++ b/Gyroscope.cpp Thu Mar 12 10:16:01 2015 +0000 @@ -56,11 +56,21 @@ write_reg(0x15, 0); // new data interrupt disabled - const int16_t gx = read_reg(0x02) | (read_reg(0x03) << 8); - const int16_t gy = read_reg(0x04) | (read_reg(0x05) << 8); - const int16_t gz = read_reg(0x06) | (read_reg(0x07) << 8); - + union { + uint8_t bytes[6]; + struct { + int16_t z; + int16_t y; + int16_t x; + } val; + } buffer; + + size_t i = sizeof buffer; + while (i --> 0) { + buffer.bytes[i] = read_reg(0x02 + i); + }; + write_reg(0x15, 1 << 7); // new data interrupt enabled - return Vector3(gx, gy, gz) * gyro_resolution; + return Vector3(buffer.val.x, buffer.val.y, buffer.val.z) * gyro_resolution; }