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
2929 const int ZERO_OFFSET = 2048;
3030 const short NO_READ_MAXIMUM_MSECS = 3000;
3131 const short SAMPLES_TO_DISCARD = 100; // Throw out the first few samples
32−const int GRAVITY_SAMPLES = 200; // Use the first samples to compute gravity vector
32+const int GRAVITY_SAMPLES = 60; // Use the first samples to compute gravity vector
3333
3434 const bool USING_INVENSENSE_MPU9150 = 1;
3535
@@ -139,34 +139,36 @@ void SerialInterface::resetTrailingAverages() { · 103 unmodified lines
139139 // Render the serial interface channel values onscreen as vertical lines
140140 void SerialInterface::renderLevels(int width, int height) {
141141 int i;
142− int disp_x = 10;
142+ int displayX = 10;
143143 const int GAP = 16;
144144 char val[40];
145145 if (!USING_INVENSENSE_MPU9150) {
146+ // Legacy - Maple ADC gyros
146147 for(i = 0; i < NUM_CHANNELS; i++) {
147148 // Actual value
148149 glLineWidth(2.0);
149150 glColor4f(1, 1, 1, 1);
150151 glBegin(GL_LINES);
151− glVertex2f(disp_x, height * 0.95);
152− glVertex2f(disp_x, height * (0.25 + 0.75f * getValue(i) / 4096));
152+ glVertex2f(displayX, height * 0.95);
153+ glVertex2f(displayX, height * (0.25 + 0.75f * getValue(i) / 4096));
153154 glColor4f(1, 0, 0, 1);
154− glVertex2f(disp_x - 3, height * (0.25 + 0.75f * getValue(i) / 4096));
155− glVertex2f(disp_x, height * (0.25 + 0.75f * getValue(i) / 4096));
155+ glVertex2f(displayX - 3, height * (0.25 + 0.75f * getValue(i) / 4096));
156+ glVertex2f(displayX, height * (0.25 + 0.75f * getValue(i) / 4096));
156157 glEnd();
157158 // Trailing Average value
158159 glBegin(GL_LINES);
159160 glColor4f(1, 1, 1, 1);
160− glVertex2f(disp_x, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));
161− glVertex2f(disp_x + 4, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));
161+ glVertex2f(displayX, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));
162+ glVertex2f(displayX + 4, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));
162163 glEnd();
163164
164165 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);
166167
167− disp_x += GAP;
168+ displayX += GAP;
168169 }
169170 } else {
171+ // For invensense gyros, render as horizontal bars
170172 const int LEVEL_CORNER_X = 10;
171173 const int LEVEL_CORNER_Y = 200;
172174
@@ -177,18 +179,37 @@ void SerialInterface::renderLevels(int width, int height) { · 4 unmodified lines
177179 drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0);
178180 sprintf(val, "Roll %4.1f", _lastRollRate);
179181 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);
180188
181189 // Draw the levels as horizontal lines
182190 const int LEVEL_CENTER = 150;
191+ const float ACCEL_VIEW_SCALING = 50.f;
183192 glLineWidth(2.0);
184193 glColor4f(1, 1, 1, 1);
185194 glBegin(GL_LINES);
195+ // Gyro rates
186196 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3);
187197 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastYawRate, LEVEL_CORNER_Y - 3);
188198 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12);
189199 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastPitchRate, LEVEL_CORNER_Y + 12);
190200 glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27);
191201 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+
192213 glEnd();
193214 // Draw green vertical centerline
194215 glColor4f(0, 1, 0, 0.5);
@@ -237,15 +258,15 @@ void SerialInterface::readData() { · 42 unmodified lines
237258
238259 int accelXRate, accelYRate, accelZRate;
239260
240− convertHexToInt(sensorBuffer + 6, accelXRate);
261+ convertHexToInt(sensorBuffer + 6, accelZRate);
241262 convertHexToInt(sensorBuffer + 10, accelYRate);
242− convertHexToInt(sensorBuffer + 14, accelZRate);
263+ convertHexToInt(sensorBuffer + 14, accelXRate);
243264
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;
245266
246− _lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND;
247− _lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND;
248− _lastAccelZ = ((float) accelZRate) * LSB_TO_METERS_PER_SECOND;
267+ _lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND2;
268+ _lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND2;
269+ _lastAccelZ = ((float) -accelZRate) * LSB_TO_METERS_PER_SECOND2;
249270
250271 int rollRate, yawRate, pitchRate;
251272
@@ -262,6 +283,18 @@ void SerialInterface::readData() { · 10 unmodified lines
262283 _lastYawRate = ((float) yawRate) * LSB_TO_DEGREES_PER_SECOND;
263284 _lastPitchRate = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND + PITCH_BIAS;
264285
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+
265298 totalSamples++;
266299 } else {
267300 // This array sets the rate of trailing averaging for each channel:
@@ -300,7 +333,7 @@ void SerialInterface::readData() { · 32 unmodified lines
300333 }
301334
302335 }
303−
336+ /*
304337 // Use a set of initial samples to compute gravity
305338 if (totalSamples < GRAVITY_SAMPLES) {
306339 gravity.x += lastMeasured[ACCEL_X];
@@ -311,7 +344,7 @@ void SerialInterface::readData() { · 4 unmodified lines
311344 gravity = glm::normalize(gravity);
312345 printLog("gravity: %f,%f,%f\n", gravity.x, gravity.y, gravity.z);
313346 }
314−
347+ */
315348 totalSamples++;
316349 serialBufferPos = 0;
317350 }
@@ -336,12 +369,11 @@ void SerialInterface::resetSerial() { · 18 unmodified lines
336369 #ifdef __APPLE__
337370 active = false;
338371 totalSamples = 0;
372+ _gravity = glm::vec3(0, 0, 0);
339373
340374 gettimeofday(&lastGoodRead, NULL);
341375
342− if (!USING_INVENSENSE_MPU9150) {
343− gravity = glm::vec3(0, -1, 0);
344−
376+ if (!USING_INVENSENSE_MPU9150) {
345377 // Clear the measured and average channel data
346378 for (int i = 0; i < NUM_CHANNELS; i++) {
347379 lastMeasured[i] = 0;