History

Improved SerialInterface: Added gravity vector get function, closer to coding standards, better rendering of levels.

Philip Rosedale committed 13 years ago

Commit 5e5a7e01 parent b3e99512 files · +58 −42Browse files

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

Unified · Split

interface/src/SerialInterface.cpp+54−41
@@ -6,12 +6,12 @@ · 5 unmodified lines
66 //
77 // Channels are received in the following order (integer 0-4096 based on voltage 0-3.3v)
88 //
9−// AIN 15: Pitch Gyro (nodding your head 'yes')
10−// AIN 16: Yaw Gyro (shaking your head 'no')
11−// AIN 17: Roll Gyro (looking quizzical, tilting your head)
12−// AIN 18: Lateral acceleration (moving from side-to-side in front of your monitor)
13−// AIN 19: Up/Down acceleration (sitting up/ducking in front of your monitor)
14−// AIN 20: Forward/Back acceleration (Toward or away from your monitor)
9+// 0 - AIN 15: Pitch Gyro (nodding your head 'yes')
10+// 1 - AIN 16: Yaw Gyro (shaking your head 'no')
11+// 2 - AIN 17: Roll Gyro (looking quizzical, tilting your head)
12+// 3 - AIN 18: Lateral acceleration (moving from side-to-side in front of your monitor)
13+// 4 - AIN 19: Up/Down acceleration (sitting up/ducking in front of your monitor)
14+// 5 - AIN 20: Forward/Back acceleration (Toward or away from your monitor)
1515 //
1616
1717 #include "SerialInterface.h"
@@ -21,14 +21,15 @@ · 3 unmodified lines
2121 #include <sys/time.h>
2222 #endif
2323
24−int serial_fd;
24+int serialFd;
2525 const int MAX_BUFFER = 255;
26−char serial_buffer[MAX_BUFFER];
27−int serial_buffer_pos = 0;
26+char serialBuffer[MAX_BUFFER];
27+int serialBufferPos = 0;
2828
2929 const int ZERO_OFFSET = 2048;
3030 const short NO_READ_MAXIMUM_MSECS = 3000;
31−const short SAMPLES_TO_DISCARD = 100;
31+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
3233
3334 void SerialInterface::pair() {
3435
@@ -48,7 +49,7 @@ void SerialInterface::pair() { · 13 unmodified lines
4849 char *serialPortname = new char[100];
4950 sprintf(serialPortname, "/dev/%s", entry->d_name);
5051
51− init(serialPortname, 115200);
52+ initializePort(serialPortname, 115200);
5253
5354 delete [] serialPortname;
5455 }
@@ -60,18 +61,20 @@ void SerialInterface::pair() { · 5 unmodified lines
6061
6162 }
6263
63−// Init the serial port to the specified values
64−int SerialInterface::init(char* portname, int baud)
64+// Connect to the serial port
65+int SerialInterface::initializePort(char* portname, int baud)
6566 {
6667 #ifdef __APPLE__
67− serial_fd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
68+ serialFd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY);
6869
69− printf("Attemping to open serial interface: %s\n", portname);
70−
71− if (serial_fd == -1) return -1; // Failed to open port
70+ printf("Opening SerialUSB %s: ", portname);
7271
72+ if (serialFd == -1) {
73+ printf("Failed.\n");
74+ return -1; // Failed to open port
75+ }
7376 struct termios options;
74− tcgetattr(serial_fd,&options);
77+ tcgetattr(serialFd,&options);
7578 switch(baud)
7679 {
7780 case 9600: cfsetispeed(&options,B9600);
@@ -95,10 +98,10 @@ int SerialInterface::init(char* portname, int baud) · 17 unmodified lines
9598 options.c_cflag &= ~CSTOPB;
9699 options.c_cflag &= ~CSIZE;
97100 options.c_cflag |= CS8;
98− tcsetattr(serial_fd,TCSANOW,&options);
101+ tcsetattr(serialFd,TCSANOW,&options);
99102
100103
101− printf("Serial interface opened!\n");
104+ printf("Connected.\n");
102105 resetSerial();
103106 active = true;
104107 #endif
@@ -126,22 +129,17 @@ void SerialInterface::renderLevels(int width, int height) { · 21 unmodified lines
126129 glBegin(GL_LINES);
127130 glVertex2f(disp_x, height*0.95);
128131 glVertex2f(disp_x, height*(0.25 + 0.75f*getValue(i)/4096));
132+ glColor4f(1, 0, 0, 1);
133+ glVertex2f(disp_x - 3, height*(0.25 + 0.75f*getValue(i)/4096));
134+ glVertex2f(disp_x, height*(0.25 + 0.75f*getValue(i)/4096));
129135 glEnd();
130136 // Trailing Average value
131− glColor4f(1, 1, 0, 1);
132137 glBegin(GL_LINES);
133− glVertex2f(disp_x + 2, height*0.95);
134− glVertex2f(disp_x + 2, height*(0.25 + 0.75f*getTrailingValue(i)/4096));
138+ glColor4f(1, 1, 1, 1);
139+ glVertex2f(disp_x, height*(0.25 + 0.75f*getTrailingValue(i)/4096));
140+ glVertex2f(disp_x + 4, height*(0.25 + 0.75f*getTrailingValue(i)/4096));
135141 glEnd();
136142
137− /*
138− glColor3f(1,0,0);
139− glBegin(GL_LINES);
140− glLineWidth(4.0);
141− glVertex2f(disp_x - 10, height*0.5 - getValue(i)/4096);
142− glVertex2f(disp_x + 10, height*0.5 - getValue(i)/4096);
143− glEnd();
144− */
145143 sprintf(val, "%d", getValue(i));
146144 drawtext(disp_x-GAP/2, (height*0.95)+2, 0.08, 90, 1.0, 0, val, 0, 1, 0);
147145
@@ -162,20 +160,20 @@ void SerialInterface::renderLevels(int width, int height) { · 14 unmodified lines
162160 }
163161 void SerialInterface::readData() {
164162 #ifdef __APPLE__
165− // This array sets the rate of trailing averaging for each channel.
163+ // This array sets the rate of trailing averaging for each channel:
166164 // If the sensor rate is 100Hz, 0.001 will make the long term average a 10-second average
167− const float AVG_RATE[] = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01};
165+ const float AVG_RATE[] = {0.002, 0.002, 0.002, 0.002, 0.002, 0.002};
168166 char bufchar[1];
169167
170168 int initialSamples = totalSamples;
171169
172− while (read(serial_fd, &bufchar, 1) > 0) {
170+ while (read(serialFd, &bufchar, 1) > 0) {
173171 //std::cout << bufchar[0];
174− serial_buffer[serial_buffer_pos] = bufchar[0];
175− serial_buffer_pos++;
172+ serialBuffer[serialBufferPos] = bufchar[0];
173+ serialBufferPos++;
176174 // Have we reached end of a line of input?
177− if ((bufchar[0] == '\n') || (serial_buffer_pos >= MAX_BUFFER)) {
178− std::string serialLine(serial_buffer, serial_buffer_pos-1);
175+ if ((bufchar[0] == '\n') || (serialBufferPos >= MAX_BUFFER)) {
176+ std::string serialLine(serialBuffer, serialBufferPos-1);
179177 //std::cout << serialLine << "\n";
180178 int spot;
181179 //int channel = 0;
@@ -191,6 +189,7 @@ void SerialInterface::readData() { · 9 unmodified lines
191189 serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1);
192190 }
193191
192+ // Update Trailing Averages
194193 for (int i = 0; i < NUM_CHANNELS; i++) {
195194 if (totalSamples > SAMPLES_TO_DISCARD) {
196195 trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] +
@@ -200,8 +199,21 @@ void SerialInterface::readData() { · 3 unmodified lines
200199 }
201200
202201 }
202+
203+ // Use a set of initial samples to compute gravity
204+ if (totalSamples < GRAVITY_SAMPLES) {
205+ gravity.x += lastMeasured[ACCEL_X];
206+ gravity.y += lastMeasured[ACCEL_Y];
207+ gravity.z += lastMeasured[ACCEL_Z];
208+ }
209+ if (totalSamples == GRAVITY_SAMPLES) {
210+ gravity = glm::normalize(gravity);
211+ std::cout << "gravity: " << gravity.x << "," <<
212+ gravity.y << "," << gravity.z << "\n";
213+ }
214+
203215 totalSamples++;
204− serial_buffer_pos = 0;
216+ serialBufferPos = 0;
205217 }
206218 }
207219
@@ -210,7 +222,7 @@ void SerialInterface::readData() { · 2 unmodified lines
210222 gettimeofday(&now, NULL);
211223
212224 if (diffclock(&lastGoodRead, &now) > NO_READ_MAXIMUM_MSECS) {
213− std::cout << "No data coming over serial. Shutting down SerialInterface.\n";
225+ std::cout << "No data - Shutting down SerialInterface.\n";
214226 resetSerial();
215227 }
216228 } else {
@@ -223,6 +235,7 @@ void SerialInterface::resetSerial() { · 6 unmodified lines
223235 #ifdef __APPLE__
224236 active = false;
225237 totalSamples = 0;
238+ gravity = glm::vec3(0,-1,0);
226239
227240 gettimeofday(&lastGoodRead, NULL);
228241
@@ -233,7 +246,7 @@ void SerialInterface::resetSerial() { · 4 unmodified lines
233246 }
234247 // Clear serial input buffer
235248 for (int i = 1; i < MAX_BUFFER; i++) {
236− serial_buffer[i] = ' ';
249+ serialBuffer[i] = ' ';
237250 }
238251 #endif
239252 }