Showing the changes to interface/src/SerialInterface.cpp (1 of 2 files). Show all
Unified · Split
| @@ -21,20 +21,14 @@ · 20 unmodified lines | |||
| 21 | #include <regex.h> | 21 | #include <regex.h> |
| 22 | #endif | 22 | #endif |
| 23 | 23 | ||
| 24 | |||
| 25 | int serial_fd; | 24 | int serial_fd; |
| 26 | const int MAX_BUFFER = 255; | 25 | const int MAX_BUFFER = 255; |
| 27 | char serial_buffer[MAX_BUFFER]; | 26 | char serial_buffer[MAX_BUFFER]; |
| 28 | int serial_buffer_pos = 0; | 27 | int serial_buffer_pos = 0; |
| 29 | int samples_total = 0; | ||
| 30 | 28 | ||
| 31 | const int ZERO_OFFSET = 2048; | 29 | const int ZERO_OFFSET = 2048; |
| 32 | const short NO_READ_MAXIMUM = 10; | 30 | const short NO_READ_MAXIMUM = 10; |
| 33 | 31 | const short SAMPLES_TO_DISCARD = 100; | |
| 34 | SerialInterface::SerialInterface() { | ||
| 35 | active = false; | ||
| 36 | noReadCount = 0; | ||
| 37 | } | ||
| 38 | 32 | ||
| 39 | void SerialInterface::pair() { | 33 | void SerialInterface::pair() { |
| 40 | // look for a matching gyro setup | 34 | // look for a matching gyro setup |
| @@ -99,20 +93,10 @@ int SerialInterface::init(char* portname, int baud) · 58 unmodified lines | |||
| 99 | options.c_cflag &= ~CSIZE; | 93 | options.c_cflag &= ~CSIZE; |
| 100 | options.c_cflag |= CS8; | 94 | options.c_cflag |= CS8; |
| 101 | tcsetattr(serial_fd,TCSANOW,&options); | 95 | tcsetattr(serial_fd,TCSANOW,&options); |
| 102 | 96 | ||
| 103 | // Clear the measured and average channel data | ||
| 104 | for (int i = 1; i < NUM_CHANNELS; i++) | ||
| 105 | { | ||
| 106 | lastMeasured[i] = 0; | ||
| 107 | trailingAverage[i] = 0.0; | ||
| 108 | } | ||
| 109 | // Clear serial input buffer | ||
| 110 | for (int i = 1; i < MAX_BUFFER; i++) { | ||
| 111 | serial_buffer[i] = ' '; | ||
| 112 | } | ||
| 113 | 97 | ||
| 114 | printf("Serial interface opened!\n"); | 98 | printf("Serial interface opened!\n"); |
| 115 | 99 | resetSerial(); | |
| 116 | active = true; | 100 | active = true; |
| 117 | 101 | ||
| 118 | return 0; | 102 | return 0; |
| @@ -178,12 +162,10 @@ void SerialInterface::readData() { · 59 unmodified lines | |||
| 178 | const float AVG_RATE[] = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01}; | 162 | const float AVG_RATE[] = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01}; |
| 179 | char bufchar[1]; | 163 | char bufchar[1]; |
| 180 | 164 | ||
| 181 | bool atLeastOneRead = false; | 165 | int initialSamples = totalSamples; |
| 182 | 166 | ||
| 183 | while (read(serial_fd, &bufchar, 1) > 0) | 167 | while (read(serial_fd, &bufchar, 1) > 0) |
| 184 | { | 168 | { |
| 185 | atLeastOneRead = true; | ||
| 186 | |||
| 187 | //std::cout << bufchar[0]; | 169 | //std::cout << bufchar[0]; |
| 188 | serial_buffer[serial_buffer_pos] = bufchar[0]; | 170 | serial_buffer[serial_buffer_pos] = bufchar[0]; |
| 189 | serial_buffer_pos++; | 171 | serial_buffer_pos++; |
| @@ -205,41 +187,48 @@ void SerialInterface::readData() { · 15 unmodified lines | |||
| 205 | } else LED = atoi(serialLine.c_str()); | 187 | } else LED = atoi(serialLine.c_str()); |
| 206 | serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1); | 188 | serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1); |
| 207 | } | 189 | } |
| 208 | //std::cout << LED << "\n"; | ||
| 209 | 190 | ||
| 210 | for (int i = 0; i < NUM_CHANNELS; i++) { | 191 | for (int i = 0; i < NUM_CHANNELS; i++) { |
| 211 | if (samples_total > 0) | 192 | if (totalSamples > SAMPLES_TO_DISCARD) { |
| 212 | trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] + | 193 | trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] + |
| 213 | AVG_RATE[i]*(float)lastMeasured[i]; | 194 | AVG_RATE[i]*(float)lastMeasured[i]; |
| 214 | else trailingAverage[i] = (float)lastMeasured[i]; | 195 | } else { |
| 196 | trailingAverage[i] = (float)lastMeasured[i]; | ||
| 197 | } | ||
| 215 | 198 | ||
| 216 | } | 199 | } |
| 217 | samples_total++; | 200 | totalSamples++; |
| 218 | serial_buffer_pos = 0; | 201 | serial_buffer_pos = 0; |
| 219 | } | 202 | } |
| 220 | } | 203 | } |
| 221 | 204 | ||
| 222 | if (!atLeastOneRead) { | 205 | if (initialSamples == totalSamples) { |
| 223 | noReadCount++; | 206 | noReadCount++; |
| 224 | std::cout << "#" << noReadCount << " blank read from serial.\n"; | 207 | std::cout << "#" << noReadCount << " blank read from serial.\n"; |
| 208 | |||
| 225 | if (noReadCount >= NO_READ_MAXIMUM) { | 209 | if (noReadCount >= NO_READ_MAXIMUM) { |
| 226 | disconnectSerial(); | 210 | resetSerial(); |
| 227 | } | 211 | } |
| 228 | } | 212 | } |
| 229 | } | 213 | } |
| 230 | 214 | ||
| 231 | void SerialInterface::disconnectSerial() { | 215 | void SerialInterface::resetSerial() { |
| 232 | std::cout << "Reached maximum blank read count. Shutting down serial.\n"; | 216 | std::cout << "Reached maximum blank read count. Shutting down serial.\n"; |
| 233 | 217 | ||
| 234 | active = false; | 218 | active = false; |
| 235 | noReadCount = 0; | 219 | noReadCount = 0; |
| 220 | totalSamples = 0; | ||
| 236 | 221 | ||
| 237 | // Clear the measured and average channel data | 222 | // Clear the measured and average channel data |
| 238 | for (int i = 1; i < NUM_CHANNELS; i++) | 223 | for (int i = 0; i < NUM_CHANNELS; i++) |
| 239 | { | 224 | { |
| 240 | lastMeasured[i] = 0; | 225 | lastMeasured[i] = 0; |
| 241 | trailingAverage[i] = 0.0; | 226 | trailingAverage[i] = 0.0; |
| 242 | } | 227 | } |
| 228 | // Clear serial input buffer | ||
| 229 | for (int i = 1; i < MAX_BUFFER; i++) { | ||
| 230 | serial_buffer[i] = ' '; | ||
| 231 | } | ||
| 243 | } | 232 | } |
| 244 | 233 | ||
| 245 | 234 | ||