/Users/craigcornelius/Projects/SPRING Mac Release 0.2/intersense.cpp

Go to the documentation of this file.

// $Id: intersense.cpp,v 1.9 2001/10/17 16:49:35 kevin Exp $
//
//  Driver for Intersense InterTrax30 inertial tracker
//

#include "intersense.h"
#include <stdio.h>
#include <math.h>

#ifdef _WIN32
#define M_PI 3.1415927
#endif

const char* Intersense::rcsid = "@(#) $Id: intersense.cpp,v 1.9 2001/10/17 16:49:35 kevin Exp $ $Copyright: (c)2001 National Biocomputation Center, Stanford University $";
int Intersense::debug = 0;

short Intersense::get_shorty()
{
    short val;
    GetBuffer((char*)&val, 2);
    
#ifdef _WIN32
    return(val);
#else
    // swab bytes (little endian-big endian thing)
    return( ((val & 0x00ff) << 8) | ((val & 0xff00) >> 8) ); 
#endif
    
}

Intersense::Intersense(char* port_name, int baud)
: Sensor(Sensor::isense_sensor), SerialDevice(port_name, baud)
{
    if (debug) 
        cerr << "Intersense::constructor(" << port_name << ", " << baud << ")\n";
    
    // setup for transmit at 1200 bps
    // Note: Documentation sez to start at 1200bps, initialize, then jump to
    //  38400 during data mode, but it looks like it inits fine at 38400
    
    // flush input buffer to clear out any leftovers
    FlushInputQueue();
    
    // command: send "command-mode" command ('c')
    SendByte('c');
    
    // ack: read back the acknowledgement ('o')
    while (GetByte() != 'o') ;
    
    { // get initialization info
        
        // command: read version info ("v\000" for version of device 0)
        SendByte('v'); SendByte('\000');
        
        // ack: read back acknowledged "v\000"
        char ackbuf[2];
        GetBuffer((char*)&ackbuf, 2);
        
        // read device type (short)- if 0, is normal
        short device_type = get_shorty();
        
        // read firmware version (short)- divide by 100
        short version = get_shorty()/100;
        
        // read product string (50 characters)
        char prodid[50];
        GetBuffer((char*)&prodid, 50);
        
        // command: read number of devices present ('p')
        SendByte('p');
        
        // ack: read number of devices present (char)
        char numdevs;
        GetBuffer(&numdevs, 1);
        
        // if in debug mode, print info
        if (debug) {
            cerr << "device_type = " << device_type << endl;
            cerr << "version = " << version << endl;
            cerr << "product obj_id = [" << prodid << "]\n";
            cerr << "number of devices = " << int(numdevs) << endl;
        }
    }
    
    // command: go into data mode ('d')
    SendByte('d');
    
    // ack: read back echo ('d')
    while (GetByte() != 'd') ;
    
    // switch to 38400 bps data mode
}

Intersense::~Intersense()
{
    if (debug) 
        cerr << "Intersense::destructor()\n";
}

void Intersense::Update()
{
    if (debug) cerr << "Intersense::Update()\n";
    
    // flush input queue of any old data
    FlushInputQueue();
    
    // command: request data ('3')
    SendByte('3');
    
    // FYI: there is a 1.5ms delay to get data
    
    // read the azimuth, elevation, twist
    short azimuth = get_shorty();
    short elevation = get_shorty();
    short twist = get_shorty();
    
    // assign the values into our sensor
    pos.x = pos.y = pos.z = 0.0;                // no need to recenter or scale

    Point3D rot;
    rot.x = elevation/32768.0 * 180.0;
    rot.y = azimuth/32768.0 * 180.0;
    rot.z = twist/32768.0 * 180.0;
    
    { // compute the rotation matrix
        double e = -rot.y * M_PI/180.0;
        double a = -rot.z * M_PI/180.0;
        double t = -rot.x * M_PI/180.0;
        m.m[0][0] = cos(e)*cos(a);
        m.m[0][1] = cos(e)*sin(a);
        m.m[0][2] = -sin(e);
        m.m[0][3] = 0.0;
        m.m[1][0] = -cos(t)*sin(a) + sin(t)*sin(e)*cos(a);
        m.m[1][1] = cos(t)*cos(a) + sin(t)*sin(e)*sin(a);
        m.m[1][2] = sin(t)*cos(e);
        m.m[1][3] = 0.0;
        m.m[2][0] = sin(t)*sin(a) + cos(t)*sin(e)*cos(a);
        m.m[2][1] = -sin(t)*cos(a) + cos(t)*sin(e)*sin(a);
        m.m[2][2] = cos(t)*cos(e);
        m.m[2][3] = 0.0;
        m.m[3][0] = m.m[3][1] = m.m[3][2] = 0.0;
        m.m[3][3] = 1.0;
    }
    
    // and set active??
    active[0] = 0.0;
}

---