Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: Gyroscope.cpp
- Revision:
- 12:1632d7391453
- Parent:
- 11:d21275e60ebb
- Child:
- 16:3e2468d4f4c1
--- a/Gyroscope.cpp Thu Mar 12 10:16:01 2015 +0000 +++ b/Gyroscope.cpp Fri Mar 13 09:12:56 2015 +0000 @@ -55,22 +55,16 @@ const float gyro_resolution = 0.0610351563; // deg/sec write_reg(0x15, 0); // new data interrupt disabled - - 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); - }; - + + uint8_t buffer[6]; + for (size_t i = 0; i < 6; i++) + buffer[i] = read_reg(0x02 + i); + + const int16_t x = buffer[1] << 8 | buffer[0]; + const int16_t y = buffer[3] << 8 | buffer[2]; + const int16_t z = buffer[5] << 8 | buffer[4]; + write_reg(0x15, 1 << 7); // new data interrupt enabled - return Vector3(buffer.val.x, buffer.val.y, buffer.val.z) * gyro_resolution; + return Vector3(x, y, -z) * gyro_resolution; }