Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-08-06 18:50:00 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

discard initial garbage reads, reset properly on disconnect

Browse source
This commit is contained in:
Stephen Birarda 2013-02-15 15:06:09 -08:00
parent d0203158c8
commit 660630e981
2 changed files with 24 additions and 34 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

53
interface/src/SerialInterface.cpp
View file

@ -21,20 +21,14 @@
#include <regex.h> #include <regex.h>
#endif #endif
int serial_fd; int serial_fd;
const int MAX_BUFFER = 255; const int MAX_BUFFER = 255;
char serial_buffer[MAX_BUFFER]; char serial_buffer[MAX_BUFFER];
int serial_buffer_pos = 0; int serial_buffer_pos = 0;
int samples_total = 0;
const int ZERO_OFFSET = 2048; const int ZERO_OFFSET = 2048;
const short NO_READ_MAXIMUM = 10; const short NO_READ_MAXIMUM = 10;
const short SAMPLES_TO_DISCARD = 100;
SerialInterface::SerialInterface() {
active = false;
noReadCount = 0;
}
void SerialInterface::pair() { void SerialInterface::pair() {
// look for a matching gyro setup // look for a matching gyro setup
@ -99,20 +93,10 @@ int SerialInterface::init(char* portname, int baud)
options.c_cflag &= ~CSIZE; options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8; options.c_cflag |= CS8;
tcsetattr(serial_fd,TCSANOW,&options); tcsetattr(serial_fd,TCSANOW,&options);
// Clear the measured and average channel data
for (int i = 1; i < NUM_CHANNELS; i++)
{
lastMeasured[i] = 0;
trailingAverage[i] = 0.0;
}
// Clear serial input buffer
for (int i = 1; i < MAX_BUFFER; i++) {
serial_buffer[i] = ' ';
}
printf("Serial interface opened!\n"); printf("Serial interface opened!\n");
resetSerial();
active = true; active = true;
return 0; return 0;
@ -178,12 +162,10 @@ void SerialInterface::readData() {
const float AVG_RATE[] = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01}; const float AVG_RATE[] = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01};
char bufchar[1]; char bufchar[1];
bool atLeastOneRead = false; int initialSamples = totalSamples;
while (read(serial_fd, &bufchar, 1) > 0) while (read(serial_fd, &bufchar, 1) > 0)
{ {
atLeastOneRead = true;
//std::cout << bufchar[0]; //std::cout << bufchar[0];
serial_buffer[serial_buffer_pos] = bufchar[0]; serial_buffer[serial_buffer_pos] = bufchar[0];
serial_buffer_pos++; serial_buffer_pos++;
@ -205,41 +187,48 @@ void SerialInterface::readData() {
} else LED = atoi(serialLine.c_str()); } else LED = atoi(serialLine.c_str());
serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1); serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1);
} }
//std::cout << LED << "\n";
for (int i = 0; i < NUM_CHANNELS; i++) { for (int i = 0; i < NUM_CHANNELS; i++) {
if (samples_total > 0) if (totalSamples > SAMPLES_TO_DISCARD) {
trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] + trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] +
AVG_RATE[i]*(float)lastMeasured[i]; AVG_RATE[i]*(float)lastMeasured[i];
else trailingAverage[i] = (float)lastMeasured[i]; } else {
trailingAverage[i] = (float)lastMeasured[i];
}
} }
samples_total++; totalSamples++;
serial_buffer_pos = 0; serial_buffer_pos = 0;
} }
} }
if (!atLeastOneRead) { if (initialSamples == totalSamples) {
noReadCount++; noReadCount++;
std::cout << "#" << noReadCount << " blank read from serial.\n"; std::cout << "#" << noReadCount << " blank read from serial.\n";
if (noReadCount >= NO_READ_MAXIMUM) { if (noReadCount >= NO_READ_MAXIMUM) {
disconnectSerial(); resetSerial();
} }
} }
} }
void SerialInterface::disconnectSerial() { void SerialInterface::resetSerial() {
std::cout << "Reached maximum blank read count. Shutting down serial.\n"; std::cout << "Reached maximum blank read count. Shutting down serial.\n";
active = false; active = false;
noReadCount = 0; noReadCount = 0;
totalSamples = 0;
// Clear the measured and average channel data // Clear the measured and average channel data
for (int i = 1; i < NUM_CHANNELS; i++) for (int i = 0; i < NUM_CHANNELS; i++)
{ {
lastMeasured[i] = 0; lastMeasured[i] = 0;
trailingAverage[i] = 0.0; trailingAverage[i] = 0.0;
} }
// Clear serial input buffer
for (int i = 1; i < MAX_BUFFER; i++) {
serial_buffer[i] = ' ';
}
} }

5
interface/src/SerialInterface.h
View file

@ -31,7 +31,7 @@
class SerialInterface { class SerialInterface {
public: public:
SerialInterface(); SerialInterface() {};
void pair(); void pair();
void readData(); void readData();
int getLED() {return LED;}; int getLED() {return LED;};
@ -44,12 +44,13 @@ public:
bool active; bool active;
private: private:
int init(char * portname, int baud); int init(char * portname, int baud);
void disconnectSerial(); void resetSerial();
int lastMeasured[NUM_CHANNELS]; int lastMeasured[NUM_CHANNELS];
float trailingAverage[NUM_CHANNELS]; float trailingAverage[NUM_CHANNELS];
int samplesAveraged; int samplesAveraged;
int LED; int LED;
int noReadCount; int noReadCount;
int totalSamples;
}; };
#endif #endif