Mirrors/overte
3
0
Fork 0
mirror of https://github.com/overte-org/overte.git synced 2025-04-20 11:45:36 +02:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merge remote-tracking branch 'origin'

Browse source
This commit is contained in:
Philip Rosedale 2013-02-15 16:46:23 -08:00
commit 01767719eb
3 changed files with 103 additions and 68 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

106
interface/src/SerialInterface.cpp
View file

@ -15,20 +15,58 @@
//
#include "SerialInterface.h"
#include <dirent.h>
#include <sys/time.h>
#ifdef __APPLE__
#include <regex.h>
#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;
const short SAMPLES_TO_DISCARD = 100;
// Init the serial port to the specified values
int SerialInterface::init(char * portname, int baud)
void SerialInterface::pair() {
#ifdef __APPLE__
// look for a matching gyro setup
DIR *devDir;
struct dirent *entry;
int matchStatus;
regex_t regex;
// 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(&regex, "tty\\.usb", REG_EXTENDED|REG_NOSUB);
matchStatus = regexec(&regex, 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(&regex);
}
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;
@ -57,20 +95,13 @@ 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] = ' ';
}
return 0; // Init serial port was a success
printf("Serial interface opened!\n");
resetSerial();
active = true;
return 0;
}
// Reset Trailing averages to the current measurement
@ -133,8 +164,10 @@ void SerialInterface::readData() {
const float AVG_RATE[] = {0.01, 0.01, 0.01, 0.01, 0.01, 0.01};
char bufchar[1];
int initialSamples = totalSamples;
while (read(serial_fd, &bufchar, 1) > 0)
{
{
//std::cout << bufchar[0];
serial_buffer[serial_buffer_pos] = bufchar[0];
serial_buffer_pos++;
@ -156,19 +189,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 (initialSamples == totalSamples) {
noReadCount++;
std::cout << "#" << noReadCount << " blank read from serial.\n";
if (noReadCount >= NO_READ_MAXIMUM) {
resetSerial();
}
}
}
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 = 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] = ' ';
}
}

8
interface/src/SerialInterface.h
View file

@ -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,16 @@ 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 resetSerial();
int lastMeasured[NUM_CHANNELS];
float trailingAverage[NUM_CHANNELS];
int samplesAveraged;
int LED;
int noReadCount;
int totalSamples;
};
#endif

57
interface/src/main.cpp
View file

@ -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);
@ -334,26 +331,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){
@ -401,7 +379,10 @@ void reset_sensors()
myHead.reset();
myHand.reset();
if (serial_on) serialPort.resetTrailingAverages();
if (serialPort.active) {
serialPort.resetTrailingAverages();
}
}
void update_pos(float frametime)
@ -860,10 +841,8 @@ void idle(void)
}
// Read serial data
if (serial_on) serialPort.readData();
if (SLEEP)
{
usleep(SLEEP);
if (serialPort.active) {
serialPort.readData();
}
}
@ -990,18 +969,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;