Showing the changes to interface/src/SerialInterface.cpp (1 of 3 files). Show all
Unified · Split
| @@ -173,12 +173,12 @@ void SerialInterface::renderLevels(int width, int height) { · 172 unmodified lines | ||
| 173 | 173 | const int LEVEL_CORNER_X = 10; |
| 174 | 174 | const int LEVEL_CORNER_Y = 200; |
| 175 | 175 | |
| 176 | − // Draw the text values | |
| 177 | − sprintf(val, "Yaw %d", _lastYaw); | |
| 176 | + // Draw the numeric degree/sec values from the gyros | |
| 177 | + sprintf(val, "Yaw %4.1f", _lastYawRate); | |
| 178 | 178 | drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y, 0.10, 0, 1.0, 1, val, 0, 1, 0); |
| 179 | − sprintf(val, "Pitch %d", _lastPitch); | |
| 179 | + sprintf(val, "Pitch %4.1f", _lastPitchRate); | |
| 180 | 180 | drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0); |
| 181 | − sprintf(val, "Roll %d", _lastRoll); | |
| 181 | + sprintf(val, "Roll %4.1f", _lastRollRate); | |
| 182 | 182 | drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0); |
| 183 | 183 | |
| 184 | 184 | // Draw the levels as horizontal lines |
| @@ -187,11 +187,11 @@ void SerialInterface::renderLevels(int width, int height) { · 2 unmodified lines | ||
| 187 | 187 | glColor4f(1, 1, 1, 1); |
| 188 | 188 | glBegin(GL_LINES); |
| 189 | 189 | glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3); |
| 190 | − glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastYaw, LEVEL_CORNER_Y - 3); | |
| 190 | + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastYawRate, LEVEL_CORNER_Y - 3); | |
| 191 | 191 | glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12); |
| 192 | − glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastPitch, LEVEL_CORNER_Y + 12); | |
| 192 | + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastPitchRate, LEVEL_CORNER_Y + 12); | |
| 193 | 193 | glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27); |
| 194 | − glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastRoll, LEVEL_CORNER_Y + 27); | |
| 194 | + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastRollRate, LEVEL_CORNER_Y + 27); | |
| 195 | 195 | glEnd(); |
| 196 | 196 | // Draw green vertical centerline |
| 197 | 197 | glColor4f(0, 1, 0, 0.5); |
| @@ -238,9 +238,17 @@ void SerialInterface::readData() { · 40 unmodified lines | ||
| 238 | 238 | write(_serialDescriptor, "RD684306\n", 9); |
| 239 | 239 | read(_serialDescriptor, gyroBuffer, 20); |
| 240 | 240 | |
| 241 | − convertHexToInt(gyroBuffer + 6, _lastYaw); | |
| 242 | − convertHexToInt(gyroBuffer + 10, _lastRoll); | |
| 243 | − convertHexToInt(gyroBuffer + 14, _lastPitch); | |
| 241 | + int rollRate, yawRate, pitchRate; | |
| 242 | + | |
| 243 | + convertHexToInt(gyroBuffer + 6, rollRate); | |
| 244 | + convertHexToInt(gyroBuffer + 10, yawRate); | |
| 245 | + convertHexToInt(gyroBuffer + 14, pitchRate); | |
| 246 | + | |
| 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. | |
| 249 | + _lastRollRate = (float) rollRate * LSB_TO_DEGREES_PER_SECOND; | |
| 250 | + _lastYawRate = (float) yawRate * LSB_TO_DEGREES_PER_SECOND; | |
| 251 | + _lastPitchRate = (float) pitchRate * LSB_TO_DEGREES_PER_SECOND; | |
| 244 | 252 | |
| 245 | 253 | totalSamples++; |
| 246 | 254 | } else { |