From d0203158c8bf22ed214648c41fa8466b724519db Mon Sep 17 00:00:00 2001 From: Stephen Birarda Date: Fri, 15 Feb 2013 14:58:07 -0800 Subject: [PATCH 1/3] initial setup of polling for newly connected headset --- interface/src/SerialInterface.cpp | 81 +++++++++++++++++++++++++++++-- interface/src/SerialInterface.h | 7 ++- interface/src/main.cpp | 57 +++++----------------- 3 files changed, 94 insertions(+), 51 deletions(-) diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 26841eeaf7..b38eedec38 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -15,6 +15,12 @@ // #include "SerialInterface.h" +#include +#include +#ifdef __APPLE__ +#include +#endif + int serial_fd; const int MAX_BUFFER = 255; @@ -23,12 +29,48 @@ int serial_buffer_pos = 0; int samples_total = 0; const int ZERO_OFFSET = 2048; +const short NO_READ_MAXIMUM = 10; -// Init the serial port to the specified values -int SerialInterface::init(char * portname, int baud) +SerialInterface::SerialInterface() { + active = false; + noReadCount = 0; +} + +void SerialInterface::pair() { + // look for a matching gyro setup + DIR *devDir; + struct dirent *entry; + int matchStatus; + regex_t regex; + +#ifdef __APPLE__ + // for now this only works on OS X, where the usb serial shows up as /dev/tty.usb* + if((devDir = opendir("/dev"))) { + while((entry = readdir(devDir))) { + regcomp(®ex, "tty\\.usb", REG_EXTENDED|REG_NOSUB); + matchStatus = regexec(®ex, entry->d_name, (size_t) 0, NULL, 0); + if (matchStatus == 0) { + char *serialPortname = new char[100]; + sprintf(serialPortname, "/dev/%s", entry->d_name); + + init(serialPortname, 115200); + + delete [] serialPortname; + } + regfree(®ex); + } + closedir(devDir); + } +#endif +} + +// Init the serial port to the specified values +int SerialInterface::init(char* portname, int baud) { serial_fd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY); + printf("Attemping to open serial interface: %s\n", portname); + if (serial_fd == -1) return -1; // Failed to open port struct termios options; @@ -68,9 +110,12 @@ int SerialInterface::init(char * portname, int baud) for (int i = 1; i < MAX_BUFFER; i++) { serial_buffer[i] = ' '; } - - - return 0; // Init serial port was a success + + printf("Serial interface opened!\n"); + + active = true; + + return 0; } // Reset Trailing averages to the current measurement @@ -133,8 +178,12 @@ void SerialInterface::readData() { const float AVG_RATE[] = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01}; char bufchar[1]; + bool atLeastOneRead = false; + while (read(serial_fd, &bufchar, 1) > 0) { + atLeastOneRead = true; + //std::cout << bufchar[0]; serial_buffer[serial_buffer_pos] = bufchar[0]; serial_buffer_pos++; @@ -169,6 +218,28 @@ void SerialInterface::readData() { serial_buffer_pos = 0; } } + + if (!atLeastOneRead) { + noReadCount++; + std::cout << "#" << noReadCount << " blank read from serial.\n"; + if (noReadCount >= NO_READ_MAXIMUM) { + disconnectSerial(); + } + } +} + +void SerialInterface::disconnectSerial() { + std::cout << "Reached maximum blank read count. Shutting down serial.\n"; + + active = false; + noReadCount = 0; + + // Clear the measured and average channel data + for (int i = 1; i < NUM_CHANNELS; i++) + { + lastMeasured[i] = 0; + trailingAverage[i] = 0.0; + } } diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h index bcad56ef3e..be739bb110 100644 --- a/interface/src/SerialInterface.h +++ b/interface/src/SerialInterface.h @@ -31,7 +31,8 @@ class SerialInterface { public: - int init(char * portname, int baud); + SerialInterface(); + void pair(); void readData(); int getLED() {return LED;}; int getNumSamples() {return samplesAveraged;}; @@ -40,11 +41,15 @@ public: float getTrailingValue(int num) {return trailingAverage[num];}; void resetTrailingAverages(); void renderLevels(int width, int height); + bool active; private: + int init(char * portname, int baud); + void disconnectSerial(); int lastMeasured[NUM_CHANNELS]; float trailingAverage[NUM_CHANNELS]; int samplesAveraged; int LED; + int noReadCount; }; #endif diff --git a/interface/src/main.cpp b/interface/src/main.cpp index cae318a6ec..3e0f7b80e7 100644 --- a/interface/src/main.cpp +++ b/interface/src/main.cpp @@ -113,7 +113,6 @@ Field field; Audio audio(&myHead, &audioScope); #define RENDER_FRAME_MSECS 8 -#define SLEEP 0 int steps_per_frame = 0; float yaw =0.f; // The yaw, pitch for the avatar head @@ -161,9 +160,7 @@ int speed; // SerialInterface serialPort; -char serial_portname[] = "/dev/tty.usbmodem411"; -int serial_on = 0; int latency_display = 1; //int adc_channels[NUM_CHANNELS]; //float avg_adc_channels[NUM_CHANNELS]; @@ -253,10 +250,10 @@ void Timer(int extra) printf("packet test = %4.1f\n", sendTime); } - - // Send a message to ourselves - //char test[]="T"; - //agentSocket.send("127.0.0.1", AGENT_UDP_PORT, test, 1); + // if we haven't detected gyros, check for them now + if (!serialPort.active) { + serialPort.pair(); + } } void display_stats(void) @@ -269,7 +266,7 @@ void display_stats(void) sprintf(stats, "FPS = %3.0f Pkts/s = %d Bytes/s = %d ", FPS, packets_per_second, bytes_per_second); drawtext(10, 30, 0.10, 0, 1.0, 0, stats); - if (serial_on) { + if (serialPort.active) { sprintf(stats, "ADC samples = %d, LED = %d", serialPort.getNumSamples(), serialPort.getLED()); drawtext(300, 30, 0.10, 0, 1.0, 0, stats); @@ -329,26 +326,7 @@ void init(void) myHead.setNoise(noise); } - if (serial_on) - { - // Call readsensors for a while to get stable initial values on sensors - printf( "Stabilizing sensors... " ); - gettimeofday(&timer_start, NULL); - int done = 0; - while (!done) - { - serialPort.readData(); - gettimeofday(&timer_end, NULL); - if (diffclock(&timer_start, &timer_end) > 1000) done = 1; - } - gravity.x = serialPort.getValue(ACCEL_X); - gravity.y = serialPort.getValue(ACCEL_Y); - gravity.z = serialPort.getValue(ACCEL_Z); - - std::cout << "Gravity: " << gravity.x << "," << gravity.y << "," << gravity.z << "\n"; - printf( "Done.\n" ); - - } + #ifdef MARKER_CAPTURE if(marker_capture_enabled){ @@ -396,7 +374,10 @@ void reset_sensors() myHead.reset(); myHand.reset(); - if (serial_on) serialPort.resetTrailingAverages(); + + if (serialPort.active) { + serialPort.resetTrailingAverages(); + } } void update_pos(float frametime) @@ -855,10 +836,8 @@ void idle(void) } // Read serial data - if (serial_on) serialPort.readData(); - if (SLEEP) - { - usleep(SLEEP); + if (serialPort.active) { + serialPort.readData(); } } @@ -985,18 +964,6 @@ int main(int argc, char** argv) printf("Stdev=FAIL "); printf("\n"); - // - // Try to connect the serial port I/O - // - if(serialPort.init(serial_portname, 115200) == -1) { - printf("Unable to open serial port: %s\n", serial_portname); - serial_on = 0; - } - else - { - printf("Serial Port Initialized\n"); - serial_on = 1; - } // create thread for receipt of data via UDP pthread_t networkReceiveThread; From 660630e98192e6ba3defc2861696fc38042cd9ee Mon Sep 17 00:00:00 2001 From: Stephen Birarda Date: Fri, 15 Feb 2013 15:06:09 -0800 Subject: [PATCH 2/3] discard initial garbage reads, reset properly on disconnect --- interface/src/SerialInterface.cpp | 53 ++++++++++++------------------- interface/src/SerialInterface.h | 5 +-- 2 files changed, 24 insertions(+), 34 deletions(-) diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index b38eedec38..59338b6c63 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -21,20 +21,14 @@ #include #endif - int serial_fd; const int MAX_BUFFER = 255; char serial_buffer[MAX_BUFFER]; int serial_buffer_pos = 0; -int samples_total = 0; const int ZERO_OFFSET = 2048; const short NO_READ_MAXIMUM = 10; - -SerialInterface::SerialInterface() { - active = false; - noReadCount = 0; -} +const short SAMPLES_TO_DISCARD = 100; void SerialInterface::pair() { // look for a matching gyro setup @@ -99,20 +93,10 @@ int SerialInterface::init(char* portname, int baud) options.c_cflag &= ~CSIZE; options.c_cflag |= CS8; 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"); - + resetSerial(); active = true; 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}; char bufchar[1]; - bool atLeastOneRead = false; + int initialSamples = totalSamples; while (read(serial_fd, &bufchar, 1) > 0) - { - atLeastOneRead = true; - + { //std::cout << bufchar[0]; serial_buffer[serial_buffer_pos] = bufchar[0]; serial_buffer_pos++; @@ -205,41 +187,48 @@ void SerialInterface::readData() { } else LED = atoi(serialLine.c_str()); serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1); } - //std::cout << LED << "\n"; 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] + - AVG_RATE[i]*(float)lastMeasured[i]; - else trailingAverage[i] = (float)lastMeasured[i]; + AVG_RATE[i]*(float)lastMeasured[i]; + } else { + trailingAverage[i] = (float)lastMeasured[i]; + } } - samples_total++; + totalSamples++; serial_buffer_pos = 0; } } - if (!atLeastOneRead) { + if (initialSamples == totalSamples) { noReadCount++; std::cout << "#" << noReadCount << " blank read from serial.\n"; + if (noReadCount >= NO_READ_MAXIMUM) { - disconnectSerial(); + resetSerial(); } } } -void SerialInterface::disconnectSerial() { +void SerialInterface::resetSerial() { std::cout << "Reached maximum blank read count. Shutting down serial.\n"; active = false; noReadCount = 0; + totalSamples = 0; // 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; trailingAverage[i] = 0.0; } + // Clear serial input buffer + for (int i = 1; i < MAX_BUFFER; i++) { + serial_buffer[i] = ' '; + } } diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h index be739bb110..46d47b4911 100644 --- a/interface/src/SerialInterface.h +++ b/interface/src/SerialInterface.h @@ -31,7 +31,7 @@ class SerialInterface { public: - SerialInterface(); + SerialInterface() {}; void pair(); void readData(); int getLED() {return LED;}; @@ -44,12 +44,13 @@ public: bool active; private: int init(char * portname, int baud); - void disconnectSerial(); + void resetSerial(); int lastMeasured[NUM_CHANNELS]; float trailingAverage[NUM_CHANNELS]; int samplesAveraged; int LED; int noReadCount; + int totalSamples; }; #endif From 8e4e5d049bad6aff94b2169bcea7b35a4a089985 Mon Sep 17 00:00:00 2001 From: Stephen Birarda Date: Fri, 15 Feb 2013 15:08:05 -0800 Subject: [PATCH 3/3] fix bad conditional serial pairing --- interface/src/SerialInterface.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp index 59338b6c63..c53d8dad55 100644 --- a/interface/src/SerialInterface.cpp +++ b/interface/src/SerialInterface.cpp @@ -31,13 +31,14 @@ const short NO_READ_MAXIMUM = 10; const short SAMPLES_TO_DISCARD = 100; void SerialInterface::pair() { + +#ifdef __APPLE__ // look for a matching gyro setup DIR *devDir; struct dirent *entry; int matchStatus; regex_t regex; -#ifdef __APPLE__ // for now this only works on OS X, where the usb serial shows up as /dev/tty.usb* if((devDir = opendir("/dev"))) { while((entry = readdir(devDir))) { @@ -56,6 +57,7 @@ void SerialInterface::pair() { closedir(devDir); } #endif + } // Init the serial port to the specified values