Mirrors/overte-JulianGro
3
0
Fork 0
mirror of https://github.com/JulianGro/overte.git synced 2025-04-07 02:23:25 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Big changes to serialInterface class to make better

Browse source
This commit is contained in:
Philip Rosedale 2013-01-30 10:19:05 -08:00
parent ffc5883191
commit 0a34ba29f6
7 changed files with 161 additions and 143 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

134
Source/SerialInterface.cpp
View file

@ -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 <iostream>
// These includes are for serial port reading/writing
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
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;
}

31
Source/SerialInterface.h
View file

@ -6,11 +6,18 @@
#ifndef interface_SerialInterface_h
#define interface_SerialInterface_h
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 <GLUT/glut.h>
#include <iostream>
// These includes are for serial port reading/writing
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#define NUM_CHANNELS 6
#define SERIAL_PORT_NAME "/dev/tty.usbmodem411"
// 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

27
Source/head.cpp
View file

@ -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) {

3
Source/head.h
View file

@ -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; }

108
Source/main.cpp
View file

@ -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

1
interface.xcodeproj/project.pbxproj
View file

@ -1379,6 +1379,7 @@
5325C26116AF4DBE0051A40B /* util.cpp in Sources */,
5325C26416AF4E2C0051A40B /* audio.cpp in Sources */,
D477AEAC16B7696200F3F8F6 /* oscilloscope.cpp in Sources */,
539853CE16B765EE00B2D585 /* UDPSocket.cpp in Sources */,
);
runOnlyForDeploymentPostprocessing = 0;
};

BIN
interface.xcodeproj/project.xcworkspace/xcuserdata/philip.xcuserdatad/UserInterfaceState.xcuserstate generated
View file

Binary file not shown.