diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index cca18f76cc..012d821256 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -150,29 +150,65 @@ void SerialInterface::renderLevels(int width, int height) { int i; int disp_x = 10; const int GAP = 16; - char val[10]; - for(i = 0; i < NUM_CHANNELS; i++) { - // Actual value + char val[40]; + if (!USING_INVENSENSE_MPU9150) { + for(i = 0; i < NUM_CHANNELS; i++) + { + + // Actual value + glLineWidth(2.0); + glColor4f(1, 1, 1, 1); + glBegin(GL_LINES); + glVertex2f(disp_x, height*0.95); + glVertex2f(disp_x, height*(0.25 + 0.75f*getValue(i)/4096)); + glColor4f(1, 0, 0, 1); + glVertex2f(disp_x - 3, height*(0.25 + 0.75f*getValue(i)/4096)); + glVertex2f(disp_x, height*(0.25 + 0.75f*getValue(i)/4096)); + glEnd(); + // Trailing Average value + glBegin(GL_LINES); + glColor4f(1, 1, 1, 1); + glVertex2f(disp_x, height*(0.25 + 0.75f*getTrailingValue(i)/4096)); + glVertex2f(disp_x + 4, height*(0.25 + 0.75f*getTrailingValue(i)/4096)); + glEnd(); + + sprintf(val, "%d", getValue(i)); + drawtext(disp_x-GAP/2, (height*0.95)+2, 0.08, 90, 1.0, 0, val, 0, 1, 0); + + disp_x += GAP; + } + } else { + const int LEVEL_CORNER_X = 10; + const int LEVEL_CORNER_Y = 200; + + // Draw the text values + sprintf(val, "Yaw %d", _lastYaw); + drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y, 0.10, 0, 1.0, 1, val, 0, 1, 0); + sprintf(val, "Pitch %d", _lastPitch); + drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0); + sprintf(val, "Roll %d", _lastRoll); + drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0); + + // Draw the levels as horizontal lines + const int LEVEL_CENTER = 150; glLineWidth(2.0); glColor4f(1, 1, 1, 1); glBegin(GL_LINES); - glVertex2f(disp_x, height*0.95); - glVertex2f(disp_x, height*(0.25 + 0.75f*getValue(i)/4096)); - glColor4f(1, 0, 0, 1); - glVertex2f(disp_x - 3, height*(0.25 + 0.75f*getValue(i)/4096)); - glVertex2f(disp_x, height*(0.25 + 0.75f*getValue(i)/4096)); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastYaw, LEVEL_CORNER_Y - 3); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastPitch, LEVEL_CORNER_Y + 12); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastRoll, LEVEL_CORNER_Y + 27); glEnd(); - // Trailing Average value + // Draw green vertical centerline + glColor4f(0, 1, 0, 0.5); glBegin(GL_LINES); - glColor4f(1, 1, 1, 1); - glVertex2f(disp_x, height*(0.25 + 0.75f*getTrailingValue(i)/4096)); - glVertex2f(disp_x + 4, height*(0.25 + 0.75f*getTrailingValue(i)/4096)); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 6); + glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 30); glEnd(); - sprintf(val, "%d", getValue(i)); - drawtext(disp_x-GAP/2, (height*0.95)+2, 0.08, 90, 1.0, 0, val, 0, 1, 0); - - disp_x += GAP; + } // Display Serial latency block if (LED) { @@ -204,7 +240,7 @@ void SerialInterface::readData() { int initialSamples = totalSamples; - if (USING_INVENSENSE_MPU9150) { + if (USING_INVENSENSE_MPU9150) { unsigned char gyroBuffer[20]; // ask the invensense for raw gyro data