Showing the changes to interface/src/SerialInterface.cpp (1 of 2 files). Show all
Unified · Split
| @@ -246,9 +246,12 @@ void SerialInterface::readData() { · 245 unmodified lines | |||
| 246 | 246 | ||
| 247 | // Convert the integer rates to floats | 247 | // Convert the integer rates to floats |
| 248 | const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec. | 248 | const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f; // From MPU-9150 register map, 2000 deg/sec. |
| 249 | const float PITCH_BIAS = 2.0; // Strangely, there is a small DC bias in the | ||
| 250 | // invensense pitch reading. Gravity? | ||
| 251 | |||
| 249 | _lastRollRate = (float) rollRate * LSB_TO_DEGREES_PER_SECOND; | 252 | _lastRollRate = (float) rollRate * LSB_TO_DEGREES_PER_SECOND; |
| 250 | _lastYawRate = (float) yawRate * LSB_TO_DEGREES_PER_SECOND; | 253 | _lastYawRate = (float) yawRate * LSB_TO_DEGREES_PER_SECOND; |
| 251 | _lastPitchRate = (float) pitchRate * LSB_TO_DEGREES_PER_SECOND; | 254 | _lastPitchRate = (float) -pitchRate * LSB_TO_DEGREES_PER_SECOND + PITCH_BIAS; |
| 252 | 255 | ||
| 253 | totalSamples++; | 256 | totalSamples++; |
| 254 | } else { | 257 | } else { |