Showing the changes to interface/src/SerialInterface.cpp (1 of 4 files). Show all
Unified · Split
| @@ -131,43 +131,10 @@ void SerialInterface::initializePort(char* portname, int baud) { · 130 unmodified lines | |||
| 131 | #endif | 131 | #endif |
| 132 | } | 132 | } |
| 133 | 133 | ||
| 134 | // Reset Trailing averages to the current measurement | ||
| 135 | void SerialInterface::resetTrailingAverages() { | ||
| 136 | for (int i = 1; i < NUM_CHANNELS; i++) trailingAverage[i] = lastMeasured[i]; | ||
| 137 | } | ||
| 138 | |||
| 139 | // Render the serial interface channel values onscreen as vertical lines | 134 | // Render the serial interface channel values onscreen as vertical lines |
| 140 | void SerialInterface::renderLevels(int width, int height) { | 135 | void SerialInterface::renderLevels(int width, int height) { |
| 141 | int i; | ||
| 142 | int displayX = 10; | ||
| 143 | const int GAP = 16; | ||
| 144 | char val[40]; | 136 | char val[40]; |
| 145 | if (!USING_INVENSENSE_MPU9150) { | 137 | if (USING_INVENSENSE_MPU9150) { |
| 146 | // Legacy - Maple ADC gyros | ||
| 147 | for(i = 0; i < NUM_CHANNELS; i++) { | ||
| 148 | // Actual value | ||
| 149 | glLineWidth(2.0); | ||
| 150 | glColor4f(1, 1, 1, 1); | ||
| 151 | glBegin(GL_LINES); | ||
| 152 | glVertex2f(displayX, height * 0.95); | ||
| 153 | glVertex2f(displayX, height * (0.25 + 0.75f * getValue(i) / 4096)); | ||
| 154 | glColor4f(1, 0, 0, 1); | ||
| 155 | glVertex2f(displayX - 3, height * (0.25 + 0.75f * getValue(i) / 4096)); | ||
| 156 | glVertex2f(displayX, height * (0.25 + 0.75f * getValue(i) / 4096)); | ||
| 157 | glEnd(); | ||
| 158 | // Trailing Average value | ||
| 159 | glBegin(GL_LINES); | ||
| 160 | glColor4f(1, 1, 1, 1); | ||
| 161 | glVertex2f(displayX, height * (0.25 + 0.75f * getTrailingValue(i) / 4096)); | ||
| 162 | glVertex2f(displayX + 4, height * (0.25 + 0.75f * getTrailingValue(i) / 4096)); | ||
| 163 | glEnd(); | ||
| 164 | |||
| 165 | sprintf(val, "%d", getValue(i)); | ||
| 166 | drawtext(displayX - GAP / 2, (height * 0.95) + 2, 0.08, 90, 1.0, 0, val, 0, 1, 0); | ||
| 167 | |||
| 168 | displayX += GAP; | ||
| 169 | } | ||
| 170 | } else { | ||
| 171 | // For invensense gyros, render as horizontal bars | 138 | // For invensense gyros, render as horizontal bars |
| 172 | const int LEVEL_CORNER_X = 10; | 139 | const int LEVEL_CORNER_X = 10; |
| 173 | const int LEVEL_CORNER_Y = 200; | 140 | const int LEVEL_CORNER_Y = 200; |
| @@ -218,18 +185,6 @@ void SerialInterface::renderLevels(int width, int height) { · 44 unmodified lines | |||
| 218 | glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 30); | 185 | glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 30); |
| 219 | glEnd(); | 186 | glEnd(); |
| 220 | } | 187 | } |
| 221 | |||
| 222 | // Display Serial latency block | ||
| 223 | if (LED) { | ||
| 224 | glColor3f(1,0,0); | ||
| 225 | glBegin(GL_QUADS); { | ||
| 226 | glVertex2f(width - 100, height - 100); | ||
| 227 | glVertex2f(width, height - 100); | ||
| 228 | glVertex2f(width, height); | ||
| 229 | glVertex2f(width - 100, height); | ||
| 230 | } | ||
| 231 | glEnd(); | ||
| 232 | } | ||
| 233 | } | 188 | } |
| 234 | 189 | ||
| 235 | void convertHexToInt(unsigned char* sourceBuffer, int& destinationInt) { | 190 | void convertHexToInt(unsigned char* sourceBuffer, int& destinationInt) { |
| @@ -263,6 +218,8 @@ void SerialInterface::readData() { · 27 unmodified lines | |||
| 263 | convertHexToInt(sensorBuffer + 14, accelXRate); | 218 | convertHexToInt(sensorBuffer + 14, accelXRate); |
| 264 | 219 | ||
| 265 | const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * 9.80665f; | 220 | const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * 9.80665f; |
| 221 | // From MPU-9150 register map, with setting on | ||
| 222 | // highest resolution = +/- 2G | ||
| 266 | 223 | ||
| 267 | _lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND2; | 224 | _lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND2; |
| 268 | _lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND2; | 225 | _lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND2; |
| @@ -296,60 +253,7 @@ void SerialInterface::readData() { · 27 unmodified lines | |||
| 296 | } | 253 | } |
| 297 | 254 | ||
| 298 | totalSamples++; | 255 | totalSamples++; |
| 299 | } else { | 256 | } |
| 300 | // This array sets the rate of trailing averaging for each channel: | ||
| 301 | // If the sensor rate is 100Hz, 0.001 will make the long term average a 10-second average | ||
| 302 | const float AVG_RATE[] = {0.002, 0.002, 0.002, 0.002, 0.002, 0.002}; | ||
| 303 | char bufchar[1]; | ||
| 304 | |||
| 305 | while (read(_serialDescriptor, &bufchar, 1) > 0) { | ||
| 306 | serialBuffer[serialBufferPos] = bufchar[0]; | ||
| 307 | serialBufferPos++; | ||
| 308 | // Have we reached end of a line of input? | ||
| 309 | if ((bufchar[0] == '\n') || (serialBufferPos >= MAX_BUFFER)) { | ||
| 310 | std::string serialLine(serialBuffer, serialBufferPos-1); | ||
| 311 | //printLog("%s\n", serialLine.c_str()); | ||
| 312 | int spot; | ||
| 313 | //int channel = 0; | ||
| 314 | std::string val; | ||
| 315 | for (int i = 0; i < NUM_CHANNELS + 2; i++) { | ||
| 316 | spot = serialLine.find_first_of(" ", 0); | ||
| 317 | if (spot != std::string::npos) { | ||
| 318 | val = serialLine.substr(0,spot); | ||
| 319 | //printLog("%s\n", val.c_str()); | ||
| 320 | if (i < NUM_CHANNELS) lastMeasured[i] = atoi(val.c_str()); | ||
| 321 | else samplesAveraged = atoi(val.c_str()); | ||
| 322 | } else LED = atoi(serialLine.c_str()); | ||
| 323 | serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1); | ||
| 324 | } | ||
| 325 | |||
| 326 | // Update Trailing Averages | ||
| 327 | for (int i = 0; i < NUM_CHANNELS; i++) { | ||
| 328 | if (totalSamples > SAMPLES_TO_DISCARD) { | ||
| 329 | trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] + | ||
| 330 | AVG_RATE[i]*(float)lastMeasured[i]; | ||
| 331 | } else { | ||
| 332 | trailingAverage[i] = (float)lastMeasured[i]; | ||
| 333 | } | ||
| 334 | |||
| 335 | } | ||
| 336 | /* | ||
| 337 | // Use a set of initial samples to compute gravity | ||
| 338 | if (totalSamples < GRAVITY_SAMPLES) { | ||
| 339 | gravity.x += lastMeasured[ACCEL_X]; | ||
| 340 | gravity.y += lastMeasured[ACCEL_Y]; | ||
| 341 | gravity.z += lastMeasured[ACCEL_Z]; | ||
| 342 | } | ||
| 343 | if (totalSamples == GRAVITY_SAMPLES) { | ||
| 344 | gravity = glm::normalize(gravity); | ||
| 345 | printLog("gravity: %f,%f,%f\n", gravity.x, gravity.y, gravity.z); | ||
| 346 | } | ||
| 347 | */ | ||
| 348 | totalSamples++; | ||
| 349 | serialBufferPos = 0; | ||
| 350 | } | ||
| 351 | } | ||
| 352 | } | ||
| 353 | 257 | ||
| 354 | if (initialSamples == totalSamples) { | 258 | if (initialSamples == totalSamples) { |
| 355 | timeval now; | 259 | timeval now; |
| @@ -373,18 +277,6 @@ void SerialInterface::resetSerial() { · 17 unmodified lines | |||
| 373 | 277 | ||
| 374 | gettimeofday(&lastGoodRead, NULL); | 278 | gettimeofday(&lastGoodRead, NULL); |
| 375 | 279 | ||
| 376 | if (!USING_INVENSENSE_MPU9150) { | ||
| 377 | // Clear the measured and average channel data | ||
| 378 | for (int i = 0; i < NUM_CHANNELS; i++) { | ||
| 379 | lastMeasured[i] = 0; | ||
| 380 | trailingAverage[i] = 0.0; | ||
| 381 | } | ||
| 382 | // Clear serial input buffer | ||
| 383 | for (int i = 1; i < MAX_BUFFER; i++) { | ||
| 384 | serialBuffer[i] = ' '; | ||
| 385 | } | ||
| 386 | } | ||
| 387 | |||
| 388 | #endif | 280 | #endif |
| 389 | } | 281 | } |
| 390 | 282 | ||