ssize_t
NetStream::read_from_net(
void *buf,
size_t nbytes
) const
{
char *tmpbuf = (char*) buf;
int numread = 0;
double stime = 0;
while (nbytes) {
#ifdef WIN32
int readb = read_win32(_fd, tmpbuf, nbytes);
#else
int readb = ::read(_fd, tmpbuf, nbytes);
#endif
if (errno == EAGAIN) { // if nothing's left to read, then
if (Config::get_var_bool("PRINT_ERRS",false,true))
cerr << " bytes read from network (EAGAIN) = " << numread << endl;
return numread + (readb == -1 ? 0:readb); //just return what we have
}
if (readb < 0) {
perror("NetStream::read_from_net : Warning - ");
return -1;
}
if (readb == 0 && nbytes > 0)
{
#ifdef WIN32
//XXX - errno not set on WIN32 when there's not enough
//to read on a non-blocking fd... should prolly
//set some state in read_win32 to reflect this, but
//for now just assume this is the reason and
//return without error...
return numread + (readb == -1 ? 0:readb);
#else
if (stime == 0)
stime = the_time();
if (the_time() - stime > 1 || errno != EAGAIN) {
if (port_ > 0) {
// Only print out a message if reading from the net
cerr << "NetStream::read_from_net - read error: peer reset"
<< endl;
}
return -1;
}
#endif
}
nbytes -= readb;
tmpbuf += readb;
numread+= readb;
}
if (Config::get_var_bool("PRINT_ERRS",false,true))
cerr << " bytes read from network = " << numread << endl;
return numread;
}
void
vrpn_Wanda::report_new_valuator_info()
{
last_val_timestamp = the_time();
if (dbug_wanda) {
fprintf(stderr, "vals = %lf %lf\n", channel[0], channel[1]);
}
// Send the message on the connection;
if (vrpn_Analog::d_connection) {
char msgbuf[1000];
vrpn_int32 len = vrpn_Analog::encode_to(msgbuf);
#ifdef VERBOSE
vrpn_Analog::print();
#endif
if (vrpn_Analog::d_connection->pack_message(len, vrpn_Analog::timestamp,
channel_m_id, vrpn_Analog::d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"Tracker: cannot write message: tossing\n");
}
} else {
fprintf(stderr,"Tracker Fastrak: No valid connection\n");
}
}
void vrpn_Wanda::mainloop(void) {
static int first = 1;
static int num = 0; // total number of bytes read in 'buffer'
static int index = 0;
server_mainloop();
if (first) {
num += vrpn_read_available_characters(serial_fd,buffer+num,1024-num);
if (num < 2) return;
if (num > 2) {
fprintf(stderr,"wanda huh? expected 2 characters on opening (got %d)\n", num);
} else {
if (buffer[0] == 'M' && buffer[1] == '3') {
fprintf(stderr,"Read init message from wanda\n");
} else {
fprintf(stderr,"vrpn_Wanda: ERROR, expected 'M3' from wanda...\n");
}
}
num = 0;
first = 0;
return;
}
int new_button_info = 0;
int new_valuator_info = 0;
// read available characters into end of buffer
// num += vrpn_read_available_characters(serial_fd,buffer+num,1024-num);
int num_read = vrpn_read_available_characters(serial_fd,buffer+num,1024-num);
#if 1
if (dbug_wanda)
if (num_read > 0)
print_bits(buffer+num, num_read);
#endif
num += num_read;
// handling synching
while( index == 0 && num > 0 && !(buffer[0] & (1<<6)) ) {
fprintf(stderr,"synching wanda\n");
for(int i=0;i<num-1;i++)
buffer[i] = buffer[i+1];
num--;
}
#if 1
double curtime = the_time();
// XXX - hack. why doesn't wanda send a record when the user
// XXX - isn't pushing the joystick ball? we weren't getting
// XXX - the last record
// if (num == 3 && index == 3 && curtime - last_val_timestamp > 0.2) {
if (curtime - last_val_timestamp > 0.2) {
int new_valuator_info = 0;
if (channel[0] != 0) {
channel[0] = 0;
new_valuator_info = 1;
}
if (channel[1] != 0) {
channel[1] = 0;
new_valuator_info = 1;
}
if (new_valuator_info) {
if (dbug_wanda) {
fprintf(stderr, "timeout: %lf\n", curtime - last_val_timestamp);
}
report_new_valuator_info();
}
}
#endif
// process all data in buffer
#if 1
if (dbug_wanda)
if (num_read > 0)
fprintf(stderr, "\t(num = %d)\n", num);
#endif
while( num >= 3 && index < num ) {
// as soon as 3 bytes are available, report them...
if (index == 0 && num >= 3) {
// update valuators & buttons #0 & #2
signed char x = static_cast<char>((buffer[1] | ((buffer[0]&3) << 6)));
signed char y = static_cast<char>((buffer[2] | ((buffer[0]&12) << 4)));
double xd = -((double)x) / 34.0; // XXX - what is max range? 34?
double yd = ((double)y) / 34.0;
if (xd > 1.0) xd = 1.0;
if (xd <-1.0) xd =-1.0;
if (yd > 1.0) yd = 1.0;
if (yd <-1.0) yd =-1.0;
if (channel[0] != xd) {
channel[0] = xd;
new_valuator_info = 1;
}
if (channel[1] != yd) {
channel[1] = yd;
new_valuator_info = 1;
}
// decode button data
int blue_val = (buffer[0] & (1<<4)) ? 1 : 0; // blue button
int red_val = (buffer[0] & (1<<5)) ? 1 : 0; // red button
if (blue_val != buttons[0]) {
buttons[0] = static_cast<unsigned char>(blue_val);
new_button_info = 1;
}
if (red_val != buttons[2]) {
buttons[2] = static_cast<unsigned char>(red_val);
new_button_info = 1;
}
index = 3;
}
// if index is at 3 & there are more bytes & the next byte
// isn't the start of a new record, then process button info
if (index == 3 && num > 3 && !(buffer[3] & (1<<6))) {
int new_val = (buffer[3]) ? 1 : 0; // yellow button
if (new_val != buttons[1]) {
buttons[1] = static_cast<unsigned char>(new_val);
new_button_info = 1;
}
index = 4;
}
if (new_button_info)
report_new_button_info();
if (new_valuator_info)
report_new_valuator_info();
// if next byte is start of new record, shift bytes
if (index == 4 || (index == 3 && num > 3 && (buffer[3] & (1<<6)))) {
for(int i=index;i<num;i++)
buffer[i - index] = buffer[i];
num -= index;
index = 0;
}
}
}
}
void
vrpn_Wanda::report_new_button_info()
{
if (dbug_wanda) {
fprintf(stderr, "buttons = %d %d %d\n", int(buttons[0]), int(buttons[1]), int(buttons[2]));
}
vrpn_Button::report_changes(); // report any button event;
}
inline double the_time() {
struct timeval ts;
vrpn_gettimeofday(&ts, NULL);
return (double)(ts.tv_sec + ts.tv_usec/1e6);
}
static double last_val_timestamp = the_time();
void
vrpn_Wanda::report_new_valuator_info()
{
last_val_timestamp = the_time();
if (dbug_wanda) {
fprintf(stderr, "vals = %lf %lf\n", channel[0], channel[1]);
}
// Send the message on the connection;
if (vrpn_Analog::d_connection) {
char msgbuf[1000];
vrpn_int32 len = vrpn_Analog::encode_to(msgbuf);
#ifdef VERBOSE
