History

Added acceleration reading, gravity baseline, and line displays to invensense

Philip Rosedale committed 13 years ago

Commit dbdd4161 parent d2cccce2 files · +55 −23Browse files

Showing the changes to interface/src/SerialInterface.cpp (1 of 2 files). Show all

Unified · Split

interface/src/SerialInterface.cpp+53−21
@@ -29,7 +29,7 @@ int serialBufferPos = 0; · 28 unmodified lines
29const int ZERO_OFFSET = 2048;29const int ZERO_OFFSET = 2048;
30const short NO_READ_MAXIMUM_MSECS = 3000;30const short NO_READ_MAXIMUM_MSECS = 3000;
31const short SAMPLES_TO_DISCARD = 100; // Throw out the first few samples31const short SAMPLES_TO_DISCARD = 100; // Throw out the first few samples
32const int GRAVITY_SAMPLES = 200; // Use the first samples to compute gravity vector32const int GRAVITY_SAMPLES = 60; // Use the first samples to compute gravity vector
33 33
34const bool USING_INVENSENSE_MPU9150 = 1;34const bool USING_INVENSENSE_MPU9150 = 1;
35 35
@@ -139,34 +139,36 @@ void SerialInterface::resetTrailingAverages() { · 103 unmodified lines
139// Render the serial interface channel values onscreen as vertical lines139// Render the serial interface channel values onscreen as vertical lines
140void SerialInterface::renderLevels(int width, int height) {140void SerialInterface::renderLevels(int width, int height) {
141 int i;141 int i;
142 int disp_x = 10;142 int displayX = 10;
143 const int GAP = 16;143 const int GAP = 16;
144 char val[40];144 char val[40];
145 if (!USING_INVENSENSE_MPU9150) {145 if (!USING_INVENSENSE_MPU9150) {
146 // Legacy - Maple ADC gyros
146 for(i = 0; i < NUM_CHANNELS; i++) {147 for(i = 0; i < NUM_CHANNELS; i++) {
147 // Actual value148 // Actual value
148 glLineWidth(2.0);149 glLineWidth(2.0);
149 glColor4f(1, 1, 1, 1);150 glColor4f(1, 1, 1, 1);
150 glBegin(GL_LINES);151 glBegin(GL_LINES);
151 glVertex2f(disp_x, height * 0.95);152 glVertex2f(displayX, height * 0.95);
152 glVertex2f(disp_x, height * (0.25 + 0.75f * getValue(i) / 4096));153 glVertex2f(displayX, height * (0.25 + 0.75f * getValue(i) / 4096));
153 glColor4f(1, 0, 0, 1);154 glColor4f(1, 0, 0, 1);
154 glVertex2f(disp_x - 3, height * (0.25 + 0.75f * getValue(i) / 4096));155 glVertex2f(displayX - 3, height * (0.25 + 0.75f * getValue(i) / 4096));
155 glVertex2f(disp_x, height * (0.25 + 0.75f * getValue(i) / 4096));156 glVertex2f(displayX, height * (0.25 + 0.75f * getValue(i) / 4096));
156 glEnd();157 glEnd();
157 // Trailing Average value158 // Trailing Average value
158 glBegin(GL_LINES);159 glBegin(GL_LINES);
159 glColor4f(1, 1, 1, 1);160 glColor4f(1, 1, 1, 1);
160 glVertex2f(disp_x, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));161 glVertex2f(displayX, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));
161 glVertex2f(disp_x + 4, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));162 glVertex2f(displayX + 4, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));
162 glEnd();163 glEnd();
163 164
164 sprintf(val, "%d", getValue(i));165 sprintf(val, "%d", getValue(i));
165 drawtext(disp_x - GAP / 2, (height * 0.95) + 2, 0.08, 90, 1.0, 0, val, 0, 1, 0);166 drawtext(displayX - GAP / 2, (height * 0.95) + 2, 0.08, 90, 1.0, 0, val, 0, 1, 0);
166 167
167 disp_x += GAP;168 displayX += GAP;
168 }169 }
169 } else {170 } else {
171 // For invensense gyros, render as horizontal bars
170 const int LEVEL_CORNER_X = 10;172 const int LEVEL_CORNER_X = 10;
171 const int LEVEL_CORNER_Y = 200;173 const int LEVEL_CORNER_Y = 200;
172 174
@@ -177,18 +179,37 @@ void SerialInterface::renderLevels(int width, int height) { · 4 unmodified lines
177 drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0);179 drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0);
178 sprintf(val, "Roll %4.1f", _lastRollRate);180 sprintf(val, "Roll %4.1f", _lastRollRate);
179 drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0);181 drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0);
182 sprintf(val, "X %4.3f", _lastAccelX);
183 drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 45, 0.10, 0, 1.0, 1, val, 0, 1, 0);
184 sprintf(val, "Y %4.3f", _lastAccelY);
185 drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 60, 0.10, 0, 1.0, 1, val, 0, 1, 0);
186 sprintf(val, "Z %4.3f", _lastAccelZ);
187 drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 75, 0.10, 0, 1.0, 1, val, 0, 1, 0);
180 188
181 // Draw the levels as horizontal lines 189 // Draw the levels as horizontal lines
182 const int LEVEL_CENTER = 150;190 const int LEVEL_CENTER = 150;
191 const float ACCEL_VIEW_SCALING = 50.f;
183 glLineWidth(2.0);192 glLineWidth(2.0);
184 glColor4f(1, 1, 1, 1);193 glColor4f(1, 1, 1, 1);
185 glBegin(GL_LINES);194 glBegin(GL_LINES);
195 // Gyro rates
186 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3);196 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3);
187 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastYawRate, LEVEL_CORNER_Y - 3);197 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastYawRate, LEVEL_CORNER_Y - 3);
188 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12);198 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12);
189 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastPitchRate, LEVEL_CORNER_Y + 12);199 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastPitchRate, LEVEL_CORNER_Y + 12);
190 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27);200 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27);
191 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastRollRate, LEVEL_CORNER_Y + 27);201 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastRollRate, LEVEL_CORNER_Y + 27);
202 // Acceleration
203 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 42);
204 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelX - _gravity.x)* ACCEL_VIEW_SCALING),
205 LEVEL_CORNER_Y + 42);
206 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 57);
207 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelY - _gravity.y) * ACCEL_VIEW_SCALING),
208 LEVEL_CORNER_Y + 57);
209 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 72);
210 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelZ - _gravity.z) * ACCEL_VIEW_SCALING),
211 LEVEL_CORNER_Y + 72);
212
192 glEnd();213 glEnd();
193 // Draw green vertical centerline214 // Draw green vertical centerline
194 glColor4f(0, 1, 0, 0.5);215 glColor4f(0, 1, 0, 0.5);
@@ -237,15 +258,15 @@ void SerialInterface::readData() { · 42 unmodified lines
237 258
238 int accelXRate, accelYRate, accelZRate;259 int accelXRate, accelYRate, accelZRate;
239 260
240 convertHexToInt(sensorBuffer + 6, accelXRate); 261 convertHexToInt(sensorBuffer + 6, accelZRate);
241 convertHexToInt(sensorBuffer + 10, accelYRate);262 convertHexToInt(sensorBuffer + 10, accelYRate);
242 convertHexToInt(sensorBuffer + 14, accelZRate);263 convertHexToInt(sensorBuffer + 14, accelXRate);
243 264
244 const float LSB_TO_METERS_PER_SECOND = 1.f / 16384.f;265 const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * 9.80665f;
245 266
246 _lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND;267 _lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND2;
247 _lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND;268 _lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND2;
248 _lastAccelZ = ((float) accelZRate) * LSB_TO_METERS_PER_SECOND;269 _lastAccelZ = ((float) -accelZRate) * LSB_TO_METERS_PER_SECOND2;
249 270
250 int rollRate, yawRate, pitchRate;271 int rollRate, yawRate, pitchRate;
251 272
@@ -262,6 +283,18 @@ void SerialInterface::readData() { · 10 unmodified lines
262 _lastYawRate = ((float) yawRate) * LSB_TO_DEGREES_PER_SECOND;283 _lastYawRate = ((float) yawRate) * LSB_TO_DEGREES_PER_SECOND;
263 _lastPitchRate = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND + PITCH_BIAS;284 _lastPitchRate = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND + PITCH_BIAS;
264 285
286 // Accumulate an initial reading for gravity
287 // Use a set of initial samples to compute gravity
288 if (totalSamples < GRAVITY_SAMPLES) {
289 _gravity.x += _lastAccelX;
290 _gravity.y += _lastAccelY;
291 _gravity.z += _lastAccelZ;
292 }
293 if (totalSamples == GRAVITY_SAMPLES) {
294 _gravity /= (float) totalSamples;
295 printLog("Gravity: %f\n", glm::length(_gravity));
296 }
297
265 totalSamples++;298 totalSamples++;
266 } else {299 } else {
267 // This array sets the rate of trailing averaging for each channel:300 // This array sets the rate of trailing averaging for each channel:
@@ -300,7 +333,7 @@ void SerialInterface::readData() { · 32 unmodified lines
300 }333 }
301 334
302 }335 }
303 336 /*
304 // Use a set of initial samples to compute gravity337 // Use a set of initial samples to compute gravity
305 if (totalSamples < GRAVITY_SAMPLES) {338 if (totalSamples < GRAVITY_SAMPLES) {
306 gravity.x += lastMeasured[ACCEL_X];339 gravity.x += lastMeasured[ACCEL_X];
@@ -311,7 +344,7 @@ void SerialInterface::readData() { · 4 unmodified lines
311 gravity = glm::normalize(gravity);344 gravity = glm::normalize(gravity);
312 printLog("gravity: %f,%f,%f\n", gravity.x, gravity.y, gravity.z);345 printLog("gravity: %f,%f,%f\n", gravity.x, gravity.y, gravity.z);
313 }346 }
314 347 */
315 totalSamples++;348 totalSamples++;
316 serialBufferPos = 0;349 serialBufferPos = 0;
317 }350 }
@@ -336,12 +369,11 @@ void SerialInterface::resetSerial() { · 18 unmodified lines
336#ifdef __APPLE__369#ifdef __APPLE__
337 active = false;370 active = false;
338 totalSamples = 0;371 totalSamples = 0;
372 _gravity = glm::vec3(0, 0, 0);
339 373
340 gettimeofday(&lastGoodRead, NULL);374 gettimeofday(&lastGoodRead, NULL);
341 375
342 if (!USING_INVENSENSE_MPU9150) {376 if (!USING_INVENSENSE_MPU9150) {
343 gravity = glm::vec3(0, -1, 0);
344
345 // Clear the measured and average channel data377 // Clear the measured and average channel data
346 for (int i = 0; i < NUM_CHANNELS; i++) {378 for (int i = 0; i < NUM_CHANNELS; i++) {
347 lastMeasured[i] = 0;379 lastMeasured[i] = 0;