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