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 | const int LEVEL_CORNER_X = 10; | 173 | const int LEVEL_CORNER_X = 10; |
| 174 | const int LEVEL_CORNER_Y = 200; | 174 | const int LEVEL_CORNER_Y = 200; |
| 175 | 175 | ||
| 176 | // Draw the text values | 176 | // Draw the numeric degree/sec values from the gyros |
| 177 | sprintf(val, "Yaw %d", _lastYaw); | 177 | sprintf(val, "Yaw %4.1f", _lastYawRate); |
| 178 | drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y, 0.10, 0, 1.0, 1, val, 0, 1, 0); | 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 | drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0); | 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 | drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0); | 182 | drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0); |
| 183 | 183 | ||
| 184 | // Draw the levels as horizontal lines | 184 | // Draw the levels as horizontal lines |
| @@ -187,11 +187,11 @@ void SerialInterface::renderLevels(int width, int height) { · 2 unmodified lines | |||
| 187 | glColor4f(1, 1, 1, 1); | 187 | glColor4f(1, 1, 1, 1); |
| 188 | glBegin(GL_LINES); | 188 | glBegin(GL_LINES); |
| 189 | glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3); | 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 | glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12); | 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 | glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27); | 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 | glEnd(); | 195 | glEnd(); |
| 196 | // Draw green vertical centerline | 196 | // Draw green vertical centerline |
| 197 | glColor4f(0, 1, 0, 0.5); | 197 | glColor4f(0, 1, 0, 0.5); |
| @@ -238,9 +238,17 @@ void SerialInterface::readData() { · 40 unmodified lines | |||
| 238 | write(_serialDescriptor, "RD684306\n", 9); | 238 | write(_serialDescriptor, "RD684306\n", 9); |
| 239 | read(_serialDescriptor, gyroBuffer, 20); | 239 | read(_serialDescriptor, gyroBuffer, 20); |
| 240 | 240 | ||
| 241 | convertHexToInt(gyroBuffer + 6, _lastYaw); | 241 | int rollRate, yawRate, pitchRate; |
| 242 | convertHexToInt(gyroBuffer + 10, _lastRoll); | 242 | |
| 243 | convertHexToInt(gyroBuffer + 14, _lastPitch); | 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 | totalSamples++; | 253 | totalSamples++; |
| 246 | } else { | 254 | } else { |