vos/corelibs/tracking/intersenseorientation.cc

Go to the documentation of this file.

/*
    This file is part of the Virtual Object System of
    the Interreality project (http://interreality.org).

    Copyright (C) 2001, 2002 Peter Amstutz

    This software was written at the University of Massachusetts with
    support from NSF grant #EIA 9703217

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307  USA

    Peter Amstutz <tetron@interreality.org>
*/

/*
   setting serial port options with tcsettr is covered in the Linux Serial Programming HOWTO:
   http://linuxdoc.org/HOWTO/Serial-Programming-HOWTO.html
*/

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include <termios.h>

#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>

#include <string>

#include "intersenseorientation.hh"

using namespace std;

IntersenseOrientation::~IntersenseOrientation()
{
    tcsetattr(fd,TCSANOW,&oldtio);
}

IntersenseOrientation::IntersenseOrientation(const char* device, int baud) throw (DeviceError)
{
    fd = open(device, O_RDWR | O_NOCTTY );
    if (fd < 0) {
        string err("Cannot open device: ");
        err += strerror(errno);
        throw DeviceError(err.c_str());
    }
    tcgetattr(fd, &oldtio); /* save current serial port settings */
    memset(&newtio, 0, sizeof(newtio)); /* clear struct for new port settings */

    int baudflag=0;
    switch(baud) {
    case 9600:
        baudflag = B9600;
        break;
    case 19200:
        baudflag = B19200;
        break;
    case 38400:
        baudflag = B38400;
        break;
    case 115200:
        baudflag = B115200;
        break;
    }

    newtio.c_cflag = baudflag | CRTSCTS | CS8 | CLOCAL | CREAD;
    newtio.c_iflag = IGNPAR;
    newtio.c_oflag = 0;
    newtio.c_lflag = 0;
    newtio.c_cc[VTIME]    = 1;   /* inter-character timer */
//  newtio.c_cc[VMIN]     = 1;   /* blocking read until at least 1 char received */

    tcflush(fd, TCIFLUSH);
    tcsetattr(fd,TCSANOW,&newtio);

    filep=fdopen(fd, "r+");

    // specify that we're only interested in the orientation cosines
    write(fd, "\r\n\r\n", 4); // get us out of any inconsistant protocol states
    write(fd, "c", 1); // set to polled mode
    write(fd, "O1,4,0,5,0,6,0,7,1\r\n", 20); // specify we want the cosines to fill in our matrix
}

void IntersenseOrientation::getOrientationMatrix(double mat[3][3])
throw(DeviceError)
{
    write(fd, "P", 1);

    char info[256];
    fgets(info, sizeof(info), filep);


    /* let's see, matrices are generally indexed [row][column] */
    int station;
    double t;
    static double m[3][3];  // use a temporary, just to be safe. necesary?
    if(sscanf(info, "%i %lf%lf%lf %lf%lf%lf %lf%lf%lf %lf%lf%lf",
              &station, &t, &t, &t,
              &m[0][0], &m[0][1], &m[0][2],
              &m[1][0], &m[1][1], &m[1][2],
              &m[2][0], &m[2][1], &m[2][2]) < 13) {
        throw DeviceError("Misparse of data from Intersense IS300\n");
    }
    for(int i = 0; i < 3; i++)
        for(int j = 0; j < 3; j++)
            mat[i][j] = m[i][j];
}

void IntersenseOrientation::getEulerAngles(double* yaw, double* pitch, double* roll)  throw(DeviceError)
{
    char info[256];
    static int station;
    static double y, p, r;  // use a temporary, just to be safe. necesary?
    write(fd, "P", 1);
    fgets(info, sizeof(info), filep);
    if(sscanf(info, "%i %lf%lf%lf",
              &station,
              &y, &p, &r) < 4) {
        throw(DeviceError("Misparse of data from Intersense IS-300 device"));
    } else {
        *yaw = y + eulerYawOffset;
        *pitch = p + eulerPitchOffset;
        *roll = r + eulerRollOffset;
    }
}

---