diff --git a/Source/SerialInterface.cpp b/Source/SerialInterface.cpp index a7be21761a..755e97082b 100644 --- a/Source/SerialInterface.cpp +++ b/Source/SerialInterface.cpp @@ -1,30 +1,33 @@ // // SerialInterface.cpp +// 2012 by Philip Rosedale for High Fidelity Inc. // -// Used to read Arduino/Maple ADC data from an external device using a serial port (over USB) +// Read interface data from the gyros/accelerometer board using SerialUSB +// +// Channels are received in the following order (integer 0-4096 based on voltage 0-3.3v) +// +// AIN 15: Pitch Gyro (nodding your head 'yes') +// AIN 16: Yaw Gyro (shaking your head 'no') +// AIN 17: Roll Gyro (looking quizzical, tilting your head) +// AIN 18: Lateral acceleration (moving from side-to-side in front of your monitor) +// AIN 19: Up/Down acceleration (sitting up/ducking in front of your monitor) +// AIN 20: Forward/Back acceleration (Toward or away from your monitor) // #include "SerialInterface.h" -#include - -// These includes are for serial port reading/writing -#include -#include -#include - int serial_fd; - const int MAX_BUFFER = 100; char serial_buffer[MAX_BUFFER]; int serial_buffer_pos = 0; +int samples_total = 0; -// For accessing the serial port -int init_port(int baud) +// Init the serial port to the specified values +int SerialInterface::init(char * portname, int baud) { - serial_fd = open(SERIAL_PORT_NAME, O_RDWR | O_NOCTTY | O_NDELAY); - - if (serial_fd == -1) return -1; // Failed to open port + serial_fd = open(portname, O_RDWR | O_NOCTTY | O_NDELAY); + + if (serial_fd == -1) return -1; // Failed to open port struct termios options; tcgetattr(serial_fd,&options); @@ -53,16 +56,70 @@ int init_port(int baud) options.c_cflag |= CS8; tcsetattr(serial_fd,TCSANOW,&options); - return 0; // Success! + // Clear the measured and average channel data + for (int i = 1; i < NUM_CHANNELS; i++) + { + lastMeasured[i] = 0; + trailingAverage[i] = 0.0; + } + + return 0; // Init serial port was a success } -int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_channels, int * samples_averaged, int * LED_state) -{ - // Channels: - // 0, 1 = Head Pitch and Yaw - // 2,3,4 = Head X,Y,Z Acceleration - // - int samples_read = 0; +// Reset Trailing averages to the current measurement +void SerialInterface::resetTrailingAverages() { + for (int i = 1; i < NUM_CHANNELS; i++) trailingAverage[i] = lastMeasured[i]; +} + +// Render the serial interface channel values onscreen as vertical lines +void SerialInterface::renderLevels(int width, int height) { + int i; + int disp_x = 10; + const int GAP = 16; + char val[10]; + for(i = 0; i < NUM_CHANNELS; i++) + { + // Actual value + glLineWidth(2.0); + glColor4f(1, 1, 1, 1); + glBegin(GL_LINES); + glVertex2f(disp_x, height*0.95); + glVertex2f(disp_x, height*(0.25 + 0.75f*getValue(i)/4096)); + glEnd(); + // Trailing Average value + glColor4f(1, 1, 0, 1); + glBegin(GL_LINES); + glVertex2f(disp_x + 2, height*0.95); + glVertex2f(disp_x + 2, height*(0.25 + 0.75f*getTrailingValue(i)/4096)); + glEnd(); + + glColor3f(1,0,0); + glBegin(GL_LINES); + glLineWidth(2.0); + glVertex2f(disp_x - 10, height*0.5 - getRelativeValue(i)); + glVertex2f(disp_x + 10, height*0.5 - getRelativeValue(i)); + glEnd(); + sprintf(val, "%d", getValue(i)); + drawtext(disp_x-GAP/2, (height*0.95)+2, 0.08, 90, 1.0, 0, val, 0, 1, 0); + + disp_x += GAP; + } + // Display Serial latency block + if (LED) { + glColor3f(1,0,0); + glBegin(GL_QUADS); { + glVertex2f(width - 100, height - 100); + glVertex2f(width, height - 100); + glVertex2f(width, height); + glVertex2f(width - 100, height); + } + glEnd(); + } + +} +void SerialInterface::readData() { + // This array sets the rate of trailing averaging for each channel. + // If the sensor rate is 100Hz, 0.001 will make the long term average a 10-second average const float AVG_RATE[] = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001}; char bufchar[1]; while (read(serial_fd, bufchar, 1) > 0) @@ -72,39 +129,40 @@ int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_chan // Have we reached end of a line of input? if ((bufchar[0] == '\n') || (serial_buffer_pos >= MAX_BUFFER)) { - samples_read++; // At end - Extract value from string to variables if (serial_buffer[0] != 'p') { - sscanf(serial_buffer, "%d %d %d %d %d %d %d %d", /* Needs to match Num Channels */ - &adc_channels[0], - &adc_channels[1], - &adc_channels[2], - &adc_channels[3], - &adc_channels[4], - &adc_channels[5], - samples_averaged, - LED_state + // NOTE: This is stupid, need to rewrite to properly scan for NUM_CHANNELS + 2 + // + sscanf(serial_buffer, "%d %d %d %d %d %d %d %d", + &lastMeasured[0], + &lastMeasured[1], + &lastMeasured[2], + &lastMeasured[3], + &lastMeasured[4], + &lastMeasured[5], + &samplesAveraged, + &LED ); for (int i = 0; i < NUM_CHANNELS; i++) { - if (!first_measurement) - avg_adc_channels[i] = (1.f - AVG_RATE[i])*avg_adc_channels[i] + - AVG_RATE[i]*(float)adc_channels[i]; + if (samples_total > 0) + trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] + + AVG_RATE[i]*(float)lastMeasured[i]; else { - avg_adc_channels[i] = (float)adc_channels[i]; + trailingAverage[i] = (float)lastMeasured[i]; } } + samples_total++; } // Clear rest of string for printing onscreen while(serial_buffer_pos++ < MAX_BUFFER) serial_buffer[serial_buffer_pos] = ' '; serial_buffer_pos = 0; } } - - return samples_read; } + diff --git a/Source/SerialInterface.h b/Source/SerialInterface.h index cc75b15d38..39070d344e 100644 --- a/Source/SerialInterface.h +++ b/Source/SerialInterface.h @@ -6,11 +6,18 @@ #ifndef __interface__SerialInterface__ #define __interface__SerialInterface__ -int init_port (int baud); -int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_channels, int * samples_averaged, int * LED_state); +#include "glm.hpp" +#include "util.h" +#include "world.h" +#include +#include + +// These includes are for serial port reading/writing +#include +#include +#include #define NUM_CHANNELS 6 -#define SERIAL_PORT_NAME "/dev/tty.usbmodemfd131" // Acceleration sensors, in screen/world coord system (X = left/right, Y = Up/Down, Z = fwd/back) #define ACCEL_X 4 @@ -22,4 +29,22 @@ int read_sensors(int first_measurement, float * avg_adc_channels, int * adc_chan #define YAW_RATE 1 #define ROLL_RATE 2 +class SerialInterface { +public: + int init(char * portname, int baud); + void readData(); + int getLED() {return LED;}; + int getNumSamples() {return samplesAveraged;}; + int getValue(int num) {return lastMeasured[num];}; + int getRelativeValue(int num) {return lastMeasured[num] - trailingAverage[num];}; + float getTrailingValue(int num) {return trailingAverage[num];}; + void resetTrailingAverages(); + void renderLevels(int width, int height); +private: + int lastMeasured[NUM_CHANNELS]; + float trailingAverage[NUM_CHANNELS]; + int samplesAveraged; + int LED; +}; + #endif diff --git a/Source/head.cpp b/Source/head.cpp index 93327a426b..77c3054d90 100644 --- a/Source/head.cpp +++ b/Source/head.cpp @@ -10,6 +10,7 @@ #include "Head.h" #include "Util.h" #include "vector_angle.hpp" +#include "SerialInterface.h" float skinColor[] = {1.0, 0.84, 0.66}; float browColor[] = {210.0/255.0, 105.0/255.0, 30.0/255.0}; @@ -58,14 +59,14 @@ void Head::reset() leanForward = leanSideways = 0; } -void Head::UpdatePos(float frametime, int * adc_channels, float * avg_adc_channels, int head_mirror, glm::vec3 * gravity) +void Head::UpdatePos(float frametime, SerialInterface * serialInterface, int head_mirror, glm::vec3 * gravity) // Using serial data, update avatar/render position and angles { - float measured_pitch_rate = adc_channels[PITCH_RATE] - avg_adc_channels[PITCH_RATE]; - float measured_yaw_rate = adc_channels[YAW_RATE] - avg_adc_channels[YAW_RATE]; - float measured_lateral_accel = adc_channels[ACCEL_X] - avg_adc_channels[ACCEL_X]; - float measured_fwd_accel = avg_adc_channels[ACCEL_Z] - adc_channels[ACCEL_Z]; - float measured_roll_rate = adc_channels[ROLL_RATE] - avg_adc_channels[ROLL_RATE]; + float measured_pitch_rate = serialInterface->getRelativeValue(PITCH_RATE); + float measured_yaw_rate = serialInterface->getRelativeValue(YAW_RATE); + float measured_lateral_accel = serialInterface->getRelativeValue(ACCEL_X); + float measured_fwd_accel = serialInterface->getRelativeValue(ACCEL_Z); + float measured_roll_rate = serialInterface->getRelativeValue(ROLL_RATE); // Update avatar head position based on measured gyro rates const float HEAD_ROTATION_SCALE = 0.20; @@ -82,20 +83,6 @@ void Head::UpdatePos(float frametime, int * adc_channels, float * avg_adc_channe addRoll(measured_roll_rate * HEAD_ROLL_SCALE * frametime); addLean(measured_lateral_accel * frametime * -HEAD_LEAN_SCALE, measured_fwd_accel*frametime * HEAD_LEAN_SCALE); } - - - // Try to measure absolute roll from sensors - const float MIN_ROLL = 3.0; - glm::vec3 v1(gravity->x, gravity->y, 0); - glm::vec3 v2(adc_channels[ACCEL_X], adc_channels[ACCEL_Y], 0); - float newRoll = acos(glm::dot(glm::normalize(v1), glm::normalize(v2))) ; - if (newRoll != NAN) { - newRoll *= 1000.0; - if (newRoll > MIN_ROLL) { - if (adc_channels[ACCEL_X] > gravity->x) newRoll *= -1.0; - //SetRoll(newRoll); - } - } } void Head::addLean(float x, float z) { diff --git a/Source/head.h b/Source/head.h index 882517436e..a3c8ddbc3c 100644 --- a/Source/head.h +++ b/Source/head.h @@ -54,8 +54,7 @@ class Head { public: Head(void); void reset(); - void UpdatePos(float frametime, int * adc_channels, float * avg_adc_channels, - int head_mirror, glm::vec3 * gravity); + void UpdatePos(float frametime, SerialInterface * serialInterface, int head_mirror, glm::vec3 * gravity); void setNoise (float mag) { noise = mag; } void setPitch(float p) {Pitch = p; } void setYaw(float y) {Yaw = y; } diff --git a/Source/main.cpp b/Source/main.cpp index 3bbf7fe25e..adc144f67e 100644 --- a/Source/main.cpp +++ b/Source/main.cpp @@ -163,15 +163,19 @@ int speed; // Serial USB Variables // +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]; -int sensor_samples = 0; -int sensor_LED = 0; +//int adc_channels[NUM_CHANNELS]; +//float avg_adc_channels[NUM_CHANNELS]; +//int sensor_samples = 0; +//int sensor_LED = 0; + glm::vec3 gravity; int first_measurement = 1; -int samplecount = 0; +//int samplecount = 0; // Frame rate Measurement @@ -220,7 +224,6 @@ void Timer(int extra) packets_per_second = (float)packetcount / ((float)diffclock(timer_start,timer_end) / 1000.f); bytes_per_second = (float)bytescount / ((float)diffclock(timer_start,timer_end) / 1000.f); framecount = 0; - samplecount = 0; packetcount = 0; bytescount = 0; @@ -244,7 +247,7 @@ void display_stats(void) drawtext(10, 30, 0.10, 0, 1.0, 0, stats); if (serial_on) { sprintf(stats, "ADC samples = %d, LED = %d", - sensor_samples, sensor_LED); + serialPort.getNumSamples(), serialPort.getLED()); drawtext(500, 30, 0.10, 0, 1.0, 0, stats); } #ifdef MARKER_CAPTURE @@ -309,8 +312,6 @@ void initDisplay(void) void init(void) { - int i; - myHead.setRenderYaw(start_yaw); if (audio_on) { @@ -322,13 +323,6 @@ void init(void) printf( "Audio started.\n" ); } - // Clear serial channels - for (i = i; i < NUM_CHANNELS; i++) - { - adc_channels[i] = 0; - avg_adc_channels[i] = 0.0; - } - head_mouse_x = WIDTH/2; head_mouse_y = HEIGHT/2; head_lean_x = WIDTH/2; @@ -349,17 +343,16 @@ void init(void) // Call readsensors for a while to get stable initial values on sensors printf( "Stabilizing sensors... " ); gettimeofday(&timer_start, NULL); - read_sensors(1, &avg_adc_channels[0], &adc_channels[0], &sensor_samples, &sensor_LED); int done = 0; while (!done) { - read_sensors(0, &avg_adc_channels[0], &adc_channels[0], &sensor_samples, &sensor_LED); + serialPort.readData(); gettimeofday(&timer_end, NULL); if (diffclock(timer_start,timer_end) > 1000) done = 1; } - gravity.x = avg_adc_channels[ACCEL_X]; - gravity.y = avg_adc_channels[ACCEL_Y]; - gravity.z = avg_adc_channels[ACCEL_Z]; + 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" ); @@ -413,18 +406,18 @@ void reset_sensors() myHead.reset(); myHand.reset(); - if (serial_on) read_sensors(1, &avg_adc_channels[0], &adc_channels[0], &sensor_samples, &sensor_LED); + if (serial_on) serialPort.resetTrailingAverages(); } void update_pos(float frametime) // Using serial data, update avatar/render position and angles { - float measured_pitch_rate = adc_channels[PITCH_RATE] - avg_adc_channels[PITCH_RATE]; - float measured_yaw_rate = adc_channels[YAW_RATE] - avg_adc_channels[YAW_RATE]; - float measured_lateral_accel = adc_channels[ACCEL_X] - avg_adc_channels[ACCEL_X]; - float measured_fwd_accel = avg_adc_channels[ACCEL_Z] - adc_channels[ACCEL_Z]; + float measured_pitch_rate = serialPort.getRelativeValue(PITCH_RATE); + float measured_yaw_rate = serialPort.getRelativeValue(YAW_RATE); + float measured_lateral_accel = serialPort.getRelativeValue(ACCEL_X); + float measured_fwd_accel = serialPort.getRelativeValue(ACCEL_Z); - myHead.UpdatePos(frametime, &adc_channels[0], &avg_adc_channels[0], head_mirror, &gravity); + myHead.UpdatePos(frametime, &serialPort, head_mirror, &gravity); // Update head_mouse model const float MIN_MOUSE_RATE = 30.0; @@ -668,54 +661,11 @@ void display(void) } - // Show detected levels from the serial I/O ADC channel sensors - if (display_levels) - { - int i; - int disp_x = 10; - const int GAP = 16; - char val[10]; - for(i = 0; i < NUM_CHANNELS; i++) - { - // Actual value - glLineWidth(2.0); - glColor4f(1, 1, 1, 1); - glBegin(GL_LINES); - glVertex2f(disp_x, HEIGHT*0.95); - glVertex2f(disp_x, HEIGHT*(0.25 + 0.75f*adc_channels[i]/4096)); - glEnd(); - // Trailing Average value - glColor4f(1, 1, 0, 1); - glBegin(GL_LINES); - glVertex2f(disp_x + 2, HEIGHT*0.95); - glVertex2f(disp_x + 2, HEIGHT*(0.25 + 0.75f*avg_adc_channels[i]/4096)); - glEnd(); - - glColor3f(1,0,0); - glBegin(GL_LINES); - glLineWidth(2.0); - glVertex2f(disp_x - 10, HEIGHT*0.5 - (adc_channels[i] - avg_adc_channels[i])); - glVertex2f(disp_x + 10, HEIGHT*0.5 - (adc_channels[i] - avg_adc_channels[i])); - glEnd(); - sprintf(val, "%d", adc_channels[i]); - drawtext(disp_x-GAP/2, (HEIGHT*0.95)+2, 0.08, 90, 1.0, 0, val, 0, 1, 0); - - disp_x += GAP; - } - // Display Serial latency block - if (latency_display && sensor_LED) { - glColor3f(1,0,0); - glBegin(GL_QUADS); { - glVertex2f(WIDTH - 100, HEIGHT - 100); - glVertex2f(WIDTH, HEIGHT - 100); - glVertex2f(WIDTH, HEIGHT); - glVertex2f(WIDTH - 100, HEIGHT); - } - glEnd(); - } - } - - if (stats_on) display_stats(); + // Show detected levels from the serial I/O ADC channel sensors + if (display_levels) serialPort.renderLevels(WIDTH,HEIGHT); + + // Display miscellaneous text stats onscreen + if (stats_on) display_stats(); #ifdef MARKER_CAPTURE /* Render marker acquisition stuff */ @@ -894,9 +844,7 @@ void idle(void) // Read network packets read_network(); // Read serial data - if (serial_on) samplecount += read_sensors(0, &avg_adc_channels[0], &adc_channels[0], - &sensor_samples, &sensor_LED); - + if (serial_on) serialPort.readData(); if (SLEEP) { usleep(SLEEP); @@ -997,8 +945,8 @@ int main(int argc, char** argv) // // Try to connect the serial port I/O // - if(init_port(115200) == -1) { - perror("Unable to open serial port\n"); + if(serialPort.init(serial_portname, 115200) == -1) { + printf("Unable to open serial port: %s\n", serial_portname); serial_on = 0; } else diff --git a/Source/oscilloscope.cpp b/Source/oscilloscope.cpp new file mode 100644 index 0000000000..a589906322 --- /dev/null +++ b/Source/oscilloscope.cpp @@ -0,0 +1,30 @@ +// +// oscilloscope.cpp +// interface +// +// Created by Philip on 1/28/13. +// Copyright (c) 2013 Rosedale Lab. All rights reserved. +// + +#include "oscilloscope.h" + +Oscilloscope::Oscilloscope(int w, + int h) { + width = w; + height = h; + data = new float[width]; + for (int i = 0; i < width; i++) { + data[i] = 0.0; + } + current_sample = 0; +} + +void Oscilloscope::addData(float d) { + data[current_sample++] = d; +} + +void Oscilloscope::render() { + glBegin(GL_LINES); + //glVertex2f( + glEnd(); +} \ No newline at end of file diff --git a/Source/oscilloscope.h b/Source/oscilloscope.h new file mode 100644 index 0000000000..2a9edf8cee --- /dev/null +++ b/Source/oscilloscope.h @@ -0,0 +1,30 @@ +// +// oscilloscope.h +// interface +// +// Created by Philip on 1/28/13. +// Copyright (c) 2013 Rosedale Lab. All rights reserved. +// + +#ifndef __interface__oscilloscope__ +#define __interface__oscilloscope__ + +#include "glm.hpp" +#include "util.h" +#include "world.h" +#include +#include + +class Oscilloscope { +public: + Oscilloscope(int width, + int height); + void addData(float data); + void render(); +private: + int width; + int height; + float * data; + int current_sample; +}; +#endif /* defined(__interface__oscilloscope__) */ diff --git a/Source/util.cpp b/Source/util.cpp index 7f533881bf..74cfb4b41d 100644 --- a/Source/util.cpp +++ b/Source/util.cpp @@ -29,8 +29,6 @@ float angle_to(glm::vec3 head_pos, glm::vec3 source_pos, float render_yaw, float return atan2(head_pos.x - source_pos.x, head_pos.z - source_pos.z) * 180 / PI + render_yaw + head_yaw; } - - void render_vector(glm::vec3 * vec) { // Show edge of world diff --git a/interface.xcodeproj/project.pbxproj b/interface.xcodeproj/project.pbxproj index 66f28d1020..6c0531f1a9 100644 --- a/interface.xcodeproj/project.pbxproj +++ b/interface.xcodeproj/project.pbxproj @@ -63,6 +63,7 @@ B6BDADE415F44AC7002A07DF /* AudioUnit.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = B6BDADDC15F444D3002A07DF /* AudioUnit.framework */; }; D40BDFD513404BA300B0BE1F /* GLUT.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = D40BDFD413404BA300B0BE1F /* GLUT.framework */; }; D40BDFD713404BB300B0BE1F /* OpenGL.framework in Frameworks */ = {isa = PBXBuildFile; fileRef = D40BDFD613404BB300B0BE1F /* OpenGL.framework */; }; + D477AEAC16B7696200F3F8F6 /* oscilloscope.cpp in Sources */ = {isa = PBXBuildFile; fileRef = D477AEAA16B7696200F3F8F6 /* oscilloscope.cpp */; }; /* End PBXBuildFile section */ /* Begin PBXCopyFilesBuildPhase section */ @@ -525,6 +526,8 @@ B6BDADDE15F444DB002A07DF /* CoreServices.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = CoreServices.framework; path = System/Library/Frameworks/CoreServices.framework; sourceTree = SDKROOT; }; D40BDFD413404BA300B0BE1F /* GLUT.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = GLUT.framework; path = /System/Library/Frameworks/GLUT.framework; sourceTree = ""; }; D40BDFD613404BB300B0BE1F /* OpenGL.framework */ = {isa = PBXFileReference; lastKnownFileType = wrapper.framework; name = OpenGL.framework; path = /System/Library/Frameworks/OpenGL.framework; sourceTree = ""; }; + D477AEAA16B7696200F3F8F6 /* oscilloscope.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = oscilloscope.cpp; sourceTree = ""; }; + D477AEAB16B7696200F3F8F6 /* oscilloscope.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = oscilloscope.h; sourceTree = ""; }; /* End PBXFileReference section */ /* Begin PBXFrameworksBuildPhase section */ diff --git a/interface.xcodeproj/project.xcworkspace/xcuserdata/philip.xcuserdatad/UserInterfaceState.xcuserstate b/interface.xcodeproj/project.xcworkspace/xcuserdata/philip.xcuserdatad/UserInterfaceState.xcuserstate index 0cd3691198..82a71f4e65 100644 Binary files a/interface.xcodeproj/project.xcworkspace/xcuserdata/philip.xcuserdatad/UserInterfaceState.xcuserstate and b/interface.xcodeproj/project.xcworkspace/xcuserdata/philip.xcuserdatad/UserInterfaceState.xcuserstate differ