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 | + | |