Showing the changes to interface/src/SerialInterface.cpp (1 of 351 files). Show all
Unified · Split
| @@ -0,0 +1,173 @@ | |||
| 1 | // | ||
| 2 | // SerialInterface.cpp | ||
| 3 | // 2012 by Philip Rosedale for High Fidelity Inc. | ||
| 4 | // | ||
| 5 | // Read interface data from the gyros/accelerometer board using SerialUSB | ||
| 6 | // | ||
| 7 | // Channels are received in the following order (integer 0-4096 based on voltage 0-3.3v) | ||
| 8 | // | ||
| 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) | ||
| 15 | // | ||
| 16 | |||
| 17 | #include "SerialInterface.h" | ||
| 18 | |||
| 19 | int serial_fd; | ||
| 20 | const int MAX_BUFFER = 100; | ||
| 21 | char serial_buffer[MAX_BUFFER]; | ||
| 22 | int serial_buffer_pos = 0; | ||
| 23 | int samples_total = 0; | ||
| 24 | |||
| 25 | const int ZERO_OFFSET = 2048; | ||
| 26 | |||
| 27 | // Init the serial port to the specified values | ||
| 28 | int SerialInterface::init(char * portname, int baud) | ||
| 29 | { | ||
| 30 | serial_fd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY); | ||
| 31 | |||
| 32 | if (serial_fd == -1) return -1; // Failed to open port | ||
| 33 | |||
| 34 | struct termios options; | ||
| 35 | tcgetattr(serial_fd,&options); | ||
| 36 | switch(baud) | ||
| 37 | { | ||
| 38 | case 9600: cfsetispeed(&options,B9600); | ||
| 39 | cfsetospeed(&options,B9600); | ||
| 40 | break; | ||
| 41 | case 19200: cfsetispeed(&options,B19200); | ||
| 42 | cfsetospeed(&options,B19200); | ||
| 43 | break; | ||
| 44 | case 38400: cfsetispeed(&options,B38400); | ||
| 45 | cfsetospeed(&options,B38400); | ||
| 46 | break; | ||
| 47 | case 115200: cfsetispeed(&options,B115200); | ||
| 48 | cfsetospeed(&options,B115200); | ||
| 49 | break; | ||
| 50 | default:cfsetispeed(&options,B9600); | ||
| 51 | cfsetospeed(&options,B9600); | ||
| 52 | break; | ||
| 53 | } | ||
| 54 | options.c_cflag |= (CLOCAL | CREAD); | ||
| 55 | options.c_cflag &= ~PARENB; | ||
| 56 | options.c_cflag &= ~CSTOPB; | ||
| 57 | options.c_cflag &= ~CSIZE; | ||
| 58 | options.c_cflag |= CS8; | ||
| 59 | tcsetattr(serial_fd,TCSANOW,&options); | ||
| 60 | |||
| 61 | // Clear the measured and average channel data | ||
| 62 | for (int i = 1; i < NUM_CHANNELS; i++) | ||
| 63 | { | ||
| 64 | lastMeasured[i] = 0; | ||
| 65 | trailingAverage[i] = 0.0; | ||
| 66 | } | ||
| 67 | |||
| 68 | return 0; // Init serial port was a success | ||
| 69 | } | ||
| 70 | |||
| 71 | // Reset Trailing averages to the current measurement | ||
| 72 | void SerialInterface::resetTrailingAverages() { | ||
| 73 | for (int i = 1; i < NUM_CHANNELS; i++) trailingAverage[i] = lastMeasured[i]; | ||
| 74 | } | ||
| 75 | |||
| 76 | // Render the serial interface channel values onscreen as vertical lines | ||
| 77 | void SerialInterface::renderLevels(int width, int height) { | ||
| 78 | int i; | ||
| 79 | int disp_x = 10; | ||
| 80 | const int GAP = 16; | ||
| 81 | char val[10]; | ||
| 82 | for(i = 0; i < NUM_CHANNELS; i++) | ||
| 83 | { | ||
| 84 | |||
| 85 | // Actual value | ||
| 86 | glLineWidth(2.0); | ||
| 87 | glColor4f(1, 1, 1, 1); | ||
| 88 | glBegin(GL_LINES); | ||
| 89 | glVertex2f(disp_x, height*0.95); | ||
| 90 | glVertex2f(disp_x, height*(0.25 + 0.75f*getValue(i)/4096)); | ||
| 91 | glEnd(); | ||
| 92 | // Trailing Average value | ||
| 93 | glColor4f(1, 1, 0, 1); | ||
| 94 | glBegin(GL_LINES); | ||
| 95 | glVertex2f(disp_x + 2, height*0.95); | ||
| 96 | glVertex2f(disp_x + 2, height*(0.25 + 0.75f*getTrailingValue(i)/4096)); | ||
| 97 | glEnd(); | ||
| 98 | |||
| 99 | /* | ||
| 100 | glColor3f(1,0,0); | ||
| 101 | glBegin(GL_LINES); | ||
| 102 | glLineWidth(4.0); | ||
| 103 | glVertex2f(disp_x - 10, height*0.5 - getValue(i)/4096); | ||
| 104 | glVertex2f(disp_x + 10, height*0.5 - getValue(i)/4096); | ||
| 105 | glEnd(); | ||
| 106 | */ | ||
| 107 | sprintf(val, "%d", getValue(i)); | ||
| 108 | drawtext(disp_x-GAP/2, (height*0.95)+2, 0.08, 90, 1.0, 0, val, 0, 1, 0); | ||
| 109 | |||
| 110 | disp_x += GAP; | ||
| 111 | } | ||
| 112 | // Display Serial latency block | ||
| 113 | if (LED) { | ||
| 114 | glColor3f(1,0,0); | ||
| 115 | glBegin(GL_QUADS); { | ||
| 116 | glVertex2f(width - 100, height - 100); | ||
| 117 | glVertex2f(width, height - 100); | ||
| 118 | glVertex2f(width, height); | ||
| 119 | glVertex2f(width - 100, height); | ||
| 120 | } | ||
| 121 | glEnd(); | ||
| 122 | } | ||
| 123 | |||
| 124 | } | ||
| 125 | void SerialInterface::readData() { | ||
| 126 | // This array sets the rate of trailing averaging for each channel. | ||
| 127 | // If the sensor rate is 100Hz, 0.001 will make the long term average a 10-second average | ||
| 128 | const float AVG_RATE[] = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01}; | ||
| 129 | char bufchar[1]; | ||
| 130 | while (read(serial_fd, bufchar, 1) > 0) | ||
| 131 | { | ||
| 132 | serial_buffer[serial_buffer_pos] = bufchar[0]; | ||
| 133 | serial_buffer_pos++; | ||
| 134 | // Have we reached end of a line of input? | ||
| 135 | if ((bufchar[0] == '\n') || (serial_buffer_pos >= MAX_BUFFER)) | ||
| 136 | { | ||
| 137 | // At end - Extract value from string to variables | ||
| 138 | if (serial_buffer[0] != 'p') | ||
| 139 | { | ||
| 140 | // NOTE: This is stupid, need to rewrite to properly scan for NUM_CHANNELS + 2 | ||
| 141 | // | ||
| 142 | sscanf(serial_buffer, "%d %d %d %d %d %d %d %d", | ||
| 143 | &lastMeasured[0], | ||
| 144 | &lastMeasured[1], | ||
| 145 | &lastMeasured[2], | ||
| 146 | &lastMeasured[3], | ||
| 147 | &lastMeasured[4], | ||
| 148 | &lastMeasured[5], | ||
| 149 | &samplesAveraged, | ||
| 150 | &LED | ||
| 151 | ); | ||
| 152 | for (int i = 0; i < NUM_CHANNELS; i++) | ||
| 153 | { | ||
| 154 | if (samples_total > 0) | ||
| 155 | trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] + | ||
| 156 | AVG_RATE[i]*(float)lastMeasured[i]; | ||
| 157 | else | ||
| 158 | { | ||
| 159 | trailingAverage[i] = (float)lastMeasured[i]; | ||
| 160 | } | ||
| 161 | } | ||
| 162 | samples_total++; | ||
| 163 | } | ||
| 164 | // Clear rest of string for printing onscreen | ||
| 165 | while(serial_buffer_pos++ < MAX_BUFFER) serial_buffer[serial_buffer_pos] = ' '; | ||
| 166 | serial_buffer_pos = 0; | ||
| 167 | } | ||
| 168 | } | ||
| 169 | } | ||
| 170 | |||
| 171 | |||
| 172 | |||
| 173 | |||