int vrpn_Tng3::reset(void)
{
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons and analogs
clear_values();
//-----------------------------------------------------------------------
// sending an end at this time will force the ibox into the reset mode, if it
// was not already. if the box is in the power up mode, nothing will happen because
// it 'should' be waiting to sync up the baudrate
// try to synchronize for 2 seconds
bDataPacketStart = 0x55;
if (syncDatastream (2.0)) {
printf("vrpn_Tng3 found\n");
} else {
return -1;
}
printf("TNG3B found\n");
status = STATUS_SYNCING;
vrpn_gettimeofday(&_timestamp, NULL); // Set watchdog now
return 0;
}
vrpn_Tng3::vrpn_Tng3 (const char * name,
vrpn_Connection * c,
const char * port,
int baud,
const int numbuttons,
const int numchannels):
vrpn_Serial_Analog(name, c, port, baud),
vrpn_Button(name, c),
_numbuttons(numbuttons),
_numchannels(numchannels)
{
// Verify the validity of the parameters
if (_numbuttons > MAX_TBUTTONS) {
fprintf(stderr,"vrpn_Tng3: Can only support %d buttons, not %d\n", (int)MAX_TBUTTONS, (int)_numbuttons);
_numbuttons = MAX_TBUTTONS;
}
if (_numchannels > MAX_TCHANNELS) {
fprintf(stderr,"vrpn_Tng3: Can only support %d analog channels, not %d\n", (int)MAX_TCHANNELS, (int)_numchannels);
_numchannels = MAX_TCHANNELS;
}
// Set the parameters in the parent classes
vrpn_Button::num_buttons = _numbuttons;
vrpn_Analog::num_channel = _numchannels;
// Set the status of the buttons, analogs and encoders to 0 to start
clear_values();
// Set the mode to reset
_status = STATUS_RESETTING;
}
// This routine will reset the Spaceball, zeroing the position,
// mode and making the device beep.
int vrpn_Spaceball::reset(void) {
/* Spaceball initialization string. Newer documentation suggests */
/* eliminating several init settings, leaving them at defaults. */
const char *reset_str = "CB\rNT\rFTp\rFRp\rP@r@r\rMSSV\rZ\rBcCcC\r";
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
/* Reset some state variables back to zero */
bufpos = 0;
packtype = 0;
packlen = 0;
escapedchar = 0;
erroroccured = 0;
resetoccured = 0;
spaceball4000 = 0; /* re-determine which type it is */
leftymode4000 = 0; /* re-determine if its in lefty mode */
null_radius = 8; // The NULL radius is now set to 8
// Send commands to the device to cause it to reset and beep.
vrpn_flush_input_buffer(serial_fd);
vrpn_write_slowly(serial_fd, (const unsigned char *)reset_str, strlen(reset_str), 5);
// We're now waiting for a response from the box
status = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
vrpn_inertiamouse::vrpn_inertiamouse (const char* name,
vrpn_Connection * c,
const char* port,
int baud_rate)
: vrpn_Serial_Analog (name, c, port, baud_rate)
, vrpn_Button_Filter (name, c)
, numbuttons_ (Buttons)
, numchannels_ (Channels)
, null_radius_ (0)
, bufcount_ (0)
, expected_chars_ (1)
, dcb_ ()
, lp_ ()
{
vrpn_Button::num_buttons = numbuttons_;
vrpn_Analog::num_channel = numchannels_;
vel_ = new double[numchannels_];
if (vel_ == NULL) {
fprintf(stderr,"vrpn_inertiamouse::vrpn_inertiamouse(): Out of memory\n");
}
clear_values();
status_ = STATUS_RESETTING;
}
vrpn_inertiamouse::vrpn_inertiamouse (const char* name,
vrpn_Connection * c,
const char* port,
int baud_rate)
: vrpn_Serial_Analog (name, c, port, baud_rate)
, vrpn_Button_Filter (name, c)
, numbuttons_ (Buttons)
, numchannels_ (Channels)
, expected_chars_ (1)
, bufcount_ (0)
, null_radius_ (0)
, dcb_ ()
, lp_ ()
{
vrpn_Button::num_buttons = numbuttons_;
vrpn_Analog::num_channel = numchannels_;
vel_ = new double[numchannels_];
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
clear_values();
status_ = STATUS_RESETTING;
}
vrpn_Streaming_Arduino::vrpn_Streaming_Arduino(std::string name,
vrpn_Connection * c,
std::string port,
int numchannels,
int baud)
:
vrpn_Serial_Analog(name.c_str(), c, port.c_str(), baud),
vrpn_Button_Filter(name.c_str(), c),
m_numchannels(numchannels)
{
// Verify the validity of the parameters
if (m_numchannels > MAX_TCHANNELS) {
std::cerr << "vrpn_Streaming_Arduino: Can only support "
<< MAX_TCHANNELS << " analog channels, not "
<< m_numchannels << std::endl;
m_numchannels = MAX_TCHANNELS;
}
// Set the parameters in the parent classes
vrpn_Analog::num_channel = m_numchannels;
// Set the status of the buttons, analogs and encoders to 0 to start
clear_values();
// Set the mode to reset
m_status = STATUS_RESETTING;
}
//
// Function: init()
//
// Purpose: Initialization for the member func that is specific
// to the type of member function. This is a VIRTUAL
// function and as such must be called AFTER the object
// is created (virtual funcs can not be called from
// the constructor).
//
// Arguments:
//
// none
//
// Returns:
//
// void
//
// Author: Michael Zarozinski
// Date: 5/99
//
// Modification History
// Author Date Modification
// ------ ---- ------------
//
//
void MemberFuncBase::init( )
{
// allocate the values array
alloc_values_array();
// zero out values...
clear_values( );
} // end MemberFuncBase::init();
vrpn_Tracker_3DMouse::vrpn_Tracker_3DMouse(const char *name, vrpn_Connection *c,
const char *port, long baud, int filtering_count):
vrpn_Tracker_Serial(name, c, port, baud),
vrpn_Button_Filter(name, c),
_filtering_count(filtering_count),
_numbuttons(5)
{
vrpn_Button::num_buttons = _numbuttons;
clear_values();
}
int vrpn_inertiamouse::reset(void)
{
clear_values();
vrpn_flush_input_buffer(serial_fd);
status_ = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
vrpn_ImmersionBox::vrpn_ImmersionBox (const char * name,
vrpn_Connection * c,
const char * port,
int baud,
const int numbuttons,
const int numchannels,
const int numencoders):
vrpn_Serial_Analog(name, c, port, baud),
vrpn_Button_Filter(name, c),
vrpn_Dial(name, c),
_numbuttons(numbuttons),
_numchannels(numchannels),
_numencoders(numencoders)
{
// Verify the validity of the parameters
if (_numbuttons > MAX_IBUTTONS) {
fprintf(stderr,"vrpn_ImmersionBox: Can only support %d buttons, not %d\n", MAX_IBUTTONS, _numbuttons);
_numbuttons = MAX_IBUTTONS;
}
if (_numchannels > MAX_ICHANNELS) {
fprintf(stderr,"vrpn_ImmersionBox: Can only support %d analog channels, not %d\n", MAX_ICHANNELS, _numchannels);
_numchannels = MAX_ICHANNELS;
}
if (_numencoders > MAX_IENCODERS) {
fprintf(stderr,"vrpn_ImmersionBox: Can only support %d encoders, not %d\n", MAX_IENCODERS, _numencoders);
_numencoders = MAX_IENCODERS;
}
// explicitly clear the identification variables
iname[0] = 0;
comment[0] = 0;
serial[0] = 0;
id[0] = 0;
model[0] = 0;
vers[0] = 0;
parmf[0] = 0;
// Set the parameters in the parent classes
vrpn_Button::num_buttons = _numbuttons;
vrpn_Analog::num_channel = _numchannels;
vrpn_Dial::num_dials = _numencoders;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
// Set the status of the buttons, analogs and encoders to 0 to start
clear_values();
// Set the mode to reset
_status = STATUS_RESETTING;
}
/// custom contructor for falcon device tracker. handles a single device.
vrpn_Tracker_NovintFalcon::vrpn_Tracker_NovintFalcon(const char *name,
vrpn_Connection *c,
const int devidx,
const char *grip,
const char *kine,
const char *damp)
: vrpn_Tracker(name, c), vrpn_Button_Filter(name, c),
vrpn_ForceDevice(name, c), m_dev(NULL), m_obj(NULL), m_update_rate(1000.0), m_damp(0.9)
{
m_devflags=vrpn_NovintFalcon_Device::MASK_DEVICEIDX & devidx;
if (grip != NULL) {
if (0 == strcmp(grip,"4-button")) {
m_devflags |= vrpn_NovintFalcon_Device::GRIP_FOURBUTTON;
vrpn_Button::num_buttons = 4;
} else {
fprintf(stderr, "WARNING: Unknown grip for Novint Falcon #%d: %s \n", devidx, grip);
m_devflags = -1;
return;
}
}
if (kine != NULL) {
if (0 == strcmp(kine,"stamper")) {
m_devflags |= vrpn_NovintFalcon_Device::KINE_STAMPER;
} else {
fprintf(stderr, "WARNING: Unknown kinematic model for Novint Falcon #%d: %s \n", devidx, kine);
m_devflags = -1;
return;
}
}
if (damp != NULL) {
vrpn_float64 val= atof(damp);
if (val >= 1.0 && val <= 10000.0) {
m_damp = 1.0 - 1.0/val;
} else {
fprintf(stderr, "WARNING: Ignoring illegal force effect damping factor: %g \n", val);
}
}
clear_values();
if (register_autodeleted_handler(forcefield_message_id,
handle_forcefield_change_message, this, vrpn_ForceDevice::d_sender_id)) {
fprintf(stderr,"vrpn_Tracker_NovintFalcon:can't register force handler\n");
return;
}
vrpn_gettimeofday(&m_timestamp, NULL);
status = vrpn_TRACKER_RESETTING;
}
int vrpn_GlobalHapticsOrb::reset(void)
{
struct timeval timeout;
unsigned char inbuf[1]; // Response from the Orb
int ret;
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// Clear the input buffer to make sure we're starting with a clean slate.
// Send the "reset" command to the box, then wait for a response and
// make sure it matches what we expect.
vrpn_flush_input_buffer(serial_fd);
vrpn_write_characters(serial_fd, &reset_char, 1);
timeout.tv_sec = 2;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, 1, &timeout);
if (ret < 0) {
perror("vrpn_GlobalHapticsOrb::reset(): Error reading from Orb\n");
return -1;
}
if (ret == 0) {
send_text_message("vrpn_GlobalHapticsOrb::reset(): No response from Orb", d_timestamp, vrpn_TEXT_ERROR);
return -1;
}
if (inbuf[0] != 0xfc) {
char message[1024];
sprintf(message, "vrpn_GlobalHapticsOrb::reset(): Bad response from Orb (%02X)", inbuf[0]);
send_text_message(message, d_timestamp, vrpn_TEXT_ERROR);
return -1;
}
//-----------------------------------------------------------------------
// Figure out how many characters to expect in each report from the device,
// which is just 1 for the Orb.
d_expected_chars = 1;
vrpn_gettimeofday(&d_timestamp, NULL); // Set watchdog now
send_text_message("vrpn_GlobalHapticsOrb::reset(): Reset complete (this is good)", d_timestamp, vrpn_TEXT_ERROR);
// Set the mode to synchronizing
d_status = STATUS_SYNCING;
return 0;
}
/******************************************************
* clear_list *
* input : *
* -list : pointer to the address of the list *
* to clear. *
* output : *
* -NONE *
* desc : *
* This function clear the whole list pointed to *
* by the input. *
* *
*******************************************************/
void clear_list(Element ** list) {
//if the list is not empty
if (*list != NULL) {
//while we have not reached the last element
while (*list != NULL) {
//save next element address in pTmp
Element * pTmp = (*list)->next;
//clear rssi list of the device to free
clear_values(&(*list)->measurements);
//free device
free(*list);
//get saved next device address
*list = pTmp;
}
}
return;
}
//
// Function: calc()
//
// Purpose: Virtual function that calculates the 'y' points for this curve
// and fills in the values[] array.
//
// Arguments:
//
// none
//
// Returns:
//
// void
//
// Author: Michael Zarozinski
// Date: 5/99
//
// Modification History
// Author Date Modification
// ------ ---- ------------
//
//
void MemberFuncTri::calc()
{
int i; // counter
double m; // slope of the curve, this is the 'm' in y = mx + b
double b; // b for y = mx + b
// clear out the whole values[] array
clear_values( );
// calc the slope for the first part of the line...
double divisor = nodes[1].x - nodes[0].x;
m = 1.0;
// make sure we don't div by 0...
if (divisor )
m /= divisor; // this is 1/divisor cuz the 'y' max is 1.0
// fill in the 'y' values for the first part of this member func
for (i = nodes[0].x; i <= nodes[1].x; i++)
{
// we "shift" things by subtracting nodes[0].x so the calc of the y = mx + b
set_value(i, m * (i - nodes[0].x) ); // y = mx + b (b is zero in this case)
}
// calc the slope for the 2nd part of the line...
divisor = nodes[2].x - nodes[1].x;
m = -1.0; // slope is negative for this part of the curve
if (divisor)
m /= divisor;
b = 1.0;
// fill in the 'y' values for the 2nd part of the curve
for (i = nodes[1].x; i <= nodes[2].x; i++)
{
// we "shift" things by subtracting nodes[1].x so the calc of the y = mx + b
set_value(i, (m * (i - nodes[1].x)) + b );
}
} // end MemberFuncTri::calc()
// This creates a vrpn_Spaceball and sets it to reset mode. It opens
// the serial device using the code in the vrpn_Serial_Analog constructor.
vrpn_Spaceball::vrpn_Spaceball (const char * name, vrpn_Connection * c,
const char * port, int baud):
vrpn_Serial_Analog(name, c, port, baud),
vrpn_Button_Filter(name, c),
_numbuttons(12),
_numchannels(6),
null_radius(8)
{
// Set the parameters in the parent classes
vrpn_Button::num_buttons = _numbuttons;
vrpn_Analog::num_channel = _numchannels;
// Set the status of the buttons and analogs to 0 to start
clear_values();
// Set the mode to reset
status = STATUS_RESETTING;
}
int vrpn_Streaming_Arduino::reset(void)
{
//-----------------------------------------------------------------------
// Set the values back to zero for all analogs
clear_values();
//-----------------------------------------------------------------------
// Opening the port should cause the board to reset. (We can also do this
// by pulling DTR lines to low and then back high again.)
// @todo Lower and raise DTR lines. Or close and re-open the serial
// port.
// Wait for two seconds to let the device reset itself and then
// clear the input buffer, both the hardware input buffer and
// the local input buffer.
vrpn_SleepMsecs(2100);
vrpn_flush_input_buffer(serial_fd);
m_buffer.clear();
// Send a command telling how many ports we want. Then make sure we
// get a response within a reasonable amount of time.
std::ostringstream msg;
msg << m_numchannels;
msg << "\n";
vrpn_write_characters(serial_fd,
static_cast<const unsigned char *>(
static_cast<const void*>(msg.str().c_str())), msg.str().size());
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 10000;
unsigned char buffer;
int ret = vrpn_read_available_characters(serial_fd, &buffer, 1, &timeout);
if (ret != 1) {
std::cout << "vrpn_Streaming_Arduino: Could not reset" << std::endl;
return -1;
}
m_buffer += buffer;
// We already got the first byte of the record, so we drop directly
// into reading.
status = STATUS_READING;
vrpn_gettimeofday(&m_timestamp, NULL); // Set watchdog now
return 0;
}
/******************************************************************************
* NAME : vrpn_5DT::vrpn_5DT
* ROLE : This creates a vrpn_5DT and sets it to reset mode. It opens
* the serial device using the code in the vrpn_Serial_Analog constructor.
* ARGUMENTS :
* RETURN :
******************************************************************************/
vrpn_5dt::vrpn_5dt (const char * p_name, vrpn_Connection * p_c, const char * p_port, int p_baud, int p_mode, bool tenbytes):
vrpn_Serial_Analog (p_name, p_c, p_port, p_baud, 8, vrpn_SER_PARITY_NONE),
_announced(false), // Not yet announced our warning.
_wireless (p_baud == 9600), // 9600 baud implies a wireless glove.
_gotInfo (false), // used to know if we have gotten a first full wireless report.
_numchannels (8), // This is an estimate; will change when reports come
_tenbytes (tenbytes) // Do we expect ten-byte messages?
{
if (_wireless) {
// All wireless gloves continually send 10 byte reports and ignore
// all requests.
_tenbytes = true;
p_mode = 2;
}
// Set the parameters in the parent classes
vrpn_Analog::num_channel = _numchannels;
// Set the status of the buttons and analogs to 0 to start
clear_values();
// Set the mode to reset
_status = STATUS_RESETTING;
//Init the mode
_mode = p_mode;
// Sebastien Kuntz reports the following about the driver they have
// written: It works great except for one small detail :
// for unknown reasons, there's an extra byte hanging around in the data
// stream sent by the 5DT glove in stream mode.
// We have the header, finger infos, pitch and roll, checksum and after
// that we receive a 0x55 Byte.
// The official doc (http://www.5dt.com/downloads/5DTDataGlove5Manual.pdf)
// says we should get only 9 bytes, so I don't know if it's a response
// from a command somewhere; but the fact is we receive 10 bytes.
if (_tenbytes) {
_expected_chars = 10;
} else {
_expected_chars = 9;
}
}
void vrpn_Tracker_NovintFalcon::reset()
{
// nothing to do
if (m_devflags < 0) return;
static int numResets = 0; // How many resets have we tried?
int ret, i;
numResets++; // We're trying another reset
clear_values();
fprintf(stderr, "Resetting the NovintFalcon #%d (attempt %d)\n",
vrpn_NovintFalcon_Device::MASK_DEVICEIDX & m_devflags, numResets);
if (m_dev)
delete m_dev;
m_dev = new vrpn_NovintFalcon_Device(m_devflags);
if (!m_dev) {
#ifdef VERBOSE
fprintf(stderr, "Device constructor failed!\n");
#endif
status = vrpn_TRACKER_FAIL;
return;
}
if (!m_dev->init()) {
#ifdef VERBOSE
fprintf(stderr, "Device init failed!\n");
#endif
status = vrpn_TRACKER_FAIL;
return;
}
fprintf(stderr, "Reset Completed.\n");
numResets = 0; // clear variable to avoid confusion.
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
// This creates a vrpn_CerealBox and sets it to reset mode. It opens
// the serial device using the code in the vrpn_Serial_Analog constructor.
// The box seems to autodetect the baud rate when the "T" command is sent
// to it.
vrpn_CerealBox::vrpn_CerealBox (const char * name, vrpn_Connection * c,
const char * port, int baud,
const int numbuttons, const int numchannels, const int numencoders):
vrpn_Serial_Analog(name, c, port, baud),
vrpn_Button_Filter(name, c),
vrpn_Dial(name, c),
_numbuttons(numbuttons),
_numchannels(numchannels),
_numencoders(numencoders)
{
// Verify the validity of the parameters
if (_numbuttons > 24) {
fprintf(stderr,"vrpn_CerealBox: Can only support 24 buttons, not %d\n",
_numbuttons);
_numbuttons = 24;
}
if (_numchannels > 8) {
fprintf(stderr,"vrpn_CerealBox: Can only support 8 analogs, not %d\n",
_numchannels);
_numchannels = 8;
}
if (_numencoders > 8) {
fprintf(stderr,"vrpn_CerealBox: Can only support 8 encoders, not %d\n",
_numencoders);
_numencoders = 8;
}
// Set the parameters in the parent classes
vrpn_Button::num_buttons = _numbuttons;
vrpn_Analog::num_channel = _numchannels;
vrpn_Dial::num_dials = _numencoders;
// Set the status of the buttons, analogs and encoders to 0 to start
clear_values();
// Set the mode to reset
_status = STATUS_RESETTING;
}
// This creates a vrpn_GlobalHapticsOrb and sets it to reset mode. It opens
// the serial device using the code in the vrpn_Serial_Analog constructor.
vrpn_GlobalHapticsOrb::vrpn_GlobalHapticsOrb(const char * name, vrpn_Connection * c,
const char * port, int baud) :
vrpn_Serial_Analog(name, c, port, baud),
vrpn_Button_Filter(name, c),
vrpn_Dial(name, c)
{
// Set the parameters in the parent classes
vrpn_Button::num_buttons = 30;
vrpn_Analog::num_channel = 3;
vrpn_Dial::num_dials = 3;
// Set the status of the buttons, analogs and encoders to 0 to start
clear_values();
// Set a callback handler for when the first connection is made so
// that it can reset the analogs and buttons whenever this happens.
register_autodeleted_handler(
d_connection->register_message_type(vrpn_got_first_connection),
handle_firstConnection, this);
// Set the mode to reset
d_status = STATUS_RESETTING;
}
// This creates a vrpn_Magellan and sets it to reset mode. It opens
// the serial device using the code in the vrpn_Serial_Analog constructor.
// The box seems to autodetect the baud rate when the "T" command is sent
// to it.
vrpn_Magellan::vrpn_Magellan (const char * name, vrpn_Connection * c,
const char * port, int baud, bool altreset):
vrpn_Serial_Analog(name, c, port, baud),
vrpn_Button_Filter(name, c),
_numbuttons(9),
_numchannels(6),
_null_radius(8),
_altreset(altreset)
{
// Set the parameters in the parent classes
vrpn_Button::num_buttons = _numbuttons;
vrpn_Analog::num_channel = _numchannels;
// Set the status of the buttons and analogs to 0 to start
clear_values();
// Set the mode to reset
_status = STATUS_RESETTING;
// Wait before the first time we attempt a reset - seems to be a race condition
// with the device needing time between opening of the serial connection and
// receiving the reset commands. (SpaceMouse Plus XT Serial, version 6.60)
vrpn_SleepMsecs(1000);
}
int vrpn_ImmersionBox::reset(void)
{
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// sending an end at this time will force the ibox into the reset mode, if it
// was not already. if the ibox is in the power up mode, nothing will happen because
// it 'should' be waiting to sync up the baudrate
// try to synchronize for 2 seconds
if (syncBaudrate (10.0)) {
printf("vrpn_ImmersionBox found\n");
} else {
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETNAME, (char *) &iname, .1)) {
fprintf(stderr,"problems with ibox command CMD_GETNAME\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETPRID, (char *) &id, .1)) {
fprintf(stderr,"problems with ibox command CMD_GETPRID\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETMODL, (char *) &model, .1)){
fprintf(stderr,"problems with ibox command CMD_GETMODL\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETSERN, (char *) &serial, .1)){
fprintf(stderr,"problems with ibox command CMD_GETSERN\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETCOMM, (char *) &comment, .1)){
fprintf(stderr,"problems with ibox command CMD_GETCOMM\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETPERF, (char *) &parmf, .1)){
fprintf(stderr,"problems with ibox command CMD_GETPERF\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETVERS, (char *) &vers, .1)){
fprintf(stderr,"problems with ibox command CMD_GETVERS\n");
return -1;
}
#ifdef VERBOSE
printf("%s\n%s\n%s\n%s\n%s\n%s\n", iname, id, serial, comment, parmf, vers);
#endif
//-----------------------------------------------------------------------
// Compute the proper string to initialize the device based on how many
// of each type of input/output that is selected.
setupCommand (0, _numchannels, _numencoders);
unsigned char command[26] = "";
command[0] = 0xcf;
command[1] = 0x0;
command[2] = (unsigned char) 10; // milliseconds between reports
command[3] = commandByte;
for (int i = 4; i < 25;i++)
command[i] = 0x0;
//-----------------------------------------------------------------------
int result = vrpn_write_characters(serial_fd, (const unsigned char *) command, 25);
// Ask the box to send a report, ensure echo received
if (result < 25) {
fprintf(stderr,"vrpnImmersionBox::reset: could not write command\n");
return -1;
}
pause (.1);
// look for command echo
result = vrpn_read_available_characters(serial_fd, (unsigned char *) command, 1);
if (result <= 0 || command[0] != 0xcf) {
fprintf(stderr,"vrpnImmersionBox::reset: no command echo\n");
return -1;
}
// flush the input buffer
vrpn_flush_input_buffer(serial_fd);
printf("ImmersionBox reset complete.\n");
status = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
//-------------------------------------------------------------------
void conf_restore() {
int fd, rcnt, i;
unsigned short id, size;
char *buf;
int offs;
struct stat st;
conf_init_defaults();
conf_load_defaults();
if( stat(CONF_FILE,&st) != 0 || st.st_size < sizeof(int))
return;
if(!(buf=umalloc(st.st_size)))
return;
fd = open(CONF_FILE, O_RDONLY, 0777);
if( fd < 0 ) {
ufree(buf);
return;
}
rcnt = read(fd,buf,st.st_size);
close(fd);
// read magick value
if (*(int *)buf!=CONF_MAGICK_VALUE || rcnt != st.st_size) {
ufree(buf);
return;
}
offs=sizeof(int);
while (1) {
if (offs + sizeof(short) > rcnt)
break;
id=*((short *)(buf + offs));
offs += sizeof(short);
if (offs + sizeof(short) > rcnt)
break;
size=*((short *)(buf + offs));
offs += sizeof(short);
for (i=0; i<CONF_NUM; ++i) {
if (conf_info[i].id==id && conf_info[i].size==size) {
if (offs + size <= rcnt) {
memcpy(conf_info[i].var, buf+offs, size);
if (conf_info[i].func) {
conf_info[i].func();
}
}
offs += size;
break;
}
}
if (i == CONF_NUM) { // unknown id, just skip data
offs += size;
}
}
ufree(buf);
// clear any "clear on restart" values
clear_values();
}
/* Process each packet that passes the capture filter */
void parse_http_packet(u_char *args, const struct pcap_pkthdr *header, const u_char *pkt) {
struct tm *pkt_time;
char *header_line, *req_value;
char saddr[INET6_ADDRSTRLEN], daddr[INET6_ADDRSTRLEN];
char sport[PORTSTRLEN], dport[PORTSTRLEN];
char ts[MAX_TIME_LEN];
int is_request = 0, is_response = 0;
unsigned int eth_type = 0, offset;
const struct eth_header *eth;
const struct ip_header *ip;
const struct ip6_header *ip6;
const struct tcp_header *tcp;
const char *data;
int size_ip, size_tcp, size_data, family;
/* Check the ethernet type and insert a VLAN offset if necessary */
eth = (struct eth_header *) pkt;
eth_type = ntohs(eth->ether_type);
if (eth_type == ETHER_TYPE_VLAN) {
offset = link_offset + 4;
} else {
offset = link_offset;
}
offset += eth_skip_bits;
/* Position pointers within packet stream and do sanity checks */
ip = (struct ip_header *) (pkt + offset);
ip6 = (struct ip6_header *) (pkt + offset);
switch (IP_V(ip)) {
case 4: family = AF_INET; break;
case 6: family = AF_INET6; break;
default: return;
}
if (family == AF_INET) {
size_ip = IP_HL(ip) * 4;
if (size_ip < 20) return;
if (ip->ip_p != IPPROTO_TCP) return;
} else { /* AF_INET6 */
size_ip = sizeof(struct ip6_header);
if (ip6->ip6_nh != IPPROTO_TCP)
size_ip = process_ip6_nh(pkt, size_ip, header->caplen, offset);
if (size_ip < 40) return;
}
tcp = (struct tcp_header *) (pkt + offset + size_ip);
size_tcp = TH_OFF(tcp) * 4;
if (size_tcp < 20) return;
data = (char *) (pkt + offset + size_ip + size_tcp);
size_data = (header->caplen - (offset + size_ip + size_tcp));
if (size_data <= 0) return;
/* Check if we appear to have a valid request or response */
if (is_request_method(data)) {
is_request = 1;
} else if (strncmp(data, HTTP_STRING, strlen(HTTP_STRING)) == 0) {
is_response = 1;
} else {
return;
}
/* Copy packet data to editable buffer that was created in main() */
if (size_data > BUFSIZ) size_data = BUFSIZ;
memcpy(buf, data, size_data);
buf[size_data] = '\0';
/* Parse header line, bail if malformed */
if ((header_line = parse_header_line(buf)) == NULL) return;
if (is_request) {
if (parse_client_request(header_line)) return;
} else if (is_response) {
if (parse_server_response(header_line)) return;
}
/* Iterate through request/entity header fields */
while ((header_line = parse_header_line(NULL)) != NULL) {
if ((req_value = strchr(header_line, ':')) == NULL) continue;
*req_value++ = '\0';
while (isspace(*req_value)) req_value++;
insert_value(header_line, req_value);
}
/* Grab source/destination IP addresses */
if (family == AF_INET) {
inet_ntop(family, &ip->ip_src, saddr, sizeof(saddr));
inet_ntop(family, &ip->ip_dst, daddr, sizeof(daddr));
} else { /* AF_INET6 */
inet_ntop(family, &ip6->ip_src, saddr, sizeof(saddr));
inet_ntop(family, &ip6->ip_dst, daddr, sizeof(daddr));
}
insert_value("source-ip", saddr);
insert_value("dest-ip", daddr);
/* Grab source/destination ports */
snprintf(sport, PORTSTRLEN, "%d", ntohs(tcp->th_sport));
snprintf(dport, PORTSTRLEN, "%d", ntohs(tcp->th_dport));
insert_value("source-port", sport);
insert_value("dest-port", dport);
/* Extract packet capture time */
pkt_time = localtime((time_t *) &header->ts.tv_sec);
strftime(ts, MAX_TIME_LEN, "%Y-%m-%d %H:%M:%S", pkt_time);
insert_value("timestamp", ts);
if (rate_stats) {
update_host_stats(get_value("host"), header->ts.tv_sec);
clear_values();
} else {
print_format_values();
}
if (dumpfile)
pcap_dump((unsigned char *) dumpfile, header, pkt);
num_parsed++;
if (parse_count && (num_parsed >= parse_count))
pcap_breakloop(pcap_hnd);
return;
}
int vrpn_Magellan::reset(void)
{
struct timeval timeout, now;
unsigned char inbuf[45];
const char *reset_str = "z\rm3\rc30\rnH\rbH\r"; // Reset string sent to box
const char *expect_back = "z\rm3\rc30\rnH\rb\r"; // What we expect back
int ret;
//-----------------------------------------------------------------------
// See if we should be using the alternative reset string.
// XXX The "expect_back" string here is almost certainly wrong. Waiting
// to hear back what the correct one should be.
if (_altreset) {
reset_str = "z\rm3\rnH\rp?0\rq00\r";
expect_back = "z\rm3\rnH\rp?0\rq00\r";
}
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// Send the list of commands to the device to cause it to reset and beep.
// Read back the response and make sure it matches what we expect.
// Give it a reasonable amount of time to finish, then timeout
vrpn_flush_input_buffer(serial_fd);
vrpn_write_slowly(serial_fd, (unsigned char *)reset_str, strlen(reset_str), 5);
timeout.tv_sec = 1;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, strlen(expect_back), &timeout);
inbuf[strlen(expect_back)] = 0; // Make sure string is NULL-terminated
vrpn_gettimeofday(&now, NULL);
if (ret < 0) {
send_text_message("vrpn_Magellan reset: Error reading from device", now);
return -1;
}
if (ret == 0) {
send_text_message("vrpn_Magellan reset: No response from device", now);
return -1;
}
if (ret != (int)strlen(expect_back)) {
send_text_message("vrpn_Magellan reset: Got less than expected number of characters", now);
//,ret, strlen(expect_back));
return -1;
}
// Make sure the string we got back is what we expected
if ( strcmp((char *)inbuf, expect_back) != 0 ) {
send_text_message("vrpn_Magellan reset: Bad reset string", now);
//(want %s, got %s)\n", expect_back, inbuf);
return -1;
}
// The NULL radius is now set to 8
_null_radius = 8;
// We're now waiting for a response from the box
status = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
int vrpn_CerealBox::reset(void)
{
struct timeval timeout;
unsigned char inbuf[45];
const char *Cpy = "Copyright (c), BG Systems";
int major, minor, bug; // Version of the EEPROM
unsigned char reset_str[32]; // Reset string sent to box
int ret;
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// Check that the box exists and has the correct EEPROM version. The
// "T" command to the box should cause the 44-byte EEPROM string to be
// returned. This string defines the version and type of the box.
// Give it a reasonable amount of time to finish (2 seconds), then timeout
vrpn_flush_input_buffer(serial_fd);
vrpn_write_characters(serial_fd, (const unsigned char *)"T", 1);
timeout.tv_sec = 2;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, 44, &timeout);
inbuf[44] = 0; // Make sure string is NULL-terminated
if (ret < 0) {
perror("vrpn_CerealBox: Error reading from box\n");
return -1;
}
if (ret == 0) {
fprintf(stderr,"vrpn_CerealBox: No response from box\n");
return -1;
}
if (ret != 44) {
fprintf(stderr,"vrpn_CerealBox: Got %d of 44 expected characters\n",ret);
return -1;
}
// Parse the copyright string for the version and other information
// Code here is similar to check_rev() function in the BG example code.
if (strncmp((char *)inbuf, Cpy, strlen(Cpy))) {
fprintf(stderr,"vrpn_CerealBox: Copyright string mismatch: %s\n",inbuf);
return -1;
}
major = inbuf[38] - '0';
minor = inbuf[40] - '0';
bug = inbuf[41] - '0';
if ( (3 != major) || (0 != minor) || (7 != bug) ) {
fprintf(stderr, "vrpn_CerealBox: Bad EEPROM version (want 3.07, got %d.%d%d)\n",
major, minor, bug);
return -1;
}
printf("vrpn_CerealBox: Box of type %c found\n",inbuf[42]);
//-----------------------------------------------------------------------
// Compute the proper string to initialize the device based on how many
// of each type of input/output that is selected. This follows init_cereal()
// in BG example code.
{ int i;
char ana1_4 = 0; // Bits 1-4 do analog channels 1-4
char ana5_8 = 0; // Bits 1-4 do analog channels 5-8
char dig1_3 = 0; // Bits 1-3 enable groups of 8 inputs
char enc1_4 = 0; // Bits 1-4 enable encoders 1-4
char enc5_8 = 0; // Bits 1-4 enable encoders 5-8
// Figure out which analog channels to use and set them
for (i = 0; i < 4; i++) {
if (i < _numchannels) {
ana1_4 |= (1<<i);
}
}
for (i = 0; i < 4; i++) {
if (i+4 < _numchannels) {
ana5_8 |= (1<<i);
}
}
// Figure out which banks of digital inputs to use and set them
for (i = 0; i < _numbuttons; i++) {
dig1_3 |= (1 << (i/8));
}
// Figure out which encoder channels to use and set them
for (i = 0; i < 4; i++) {
if (i < _numencoders) {
enc1_4 |= (1<<i);
}
}
for (i = 0; i < 4; i++) {
if (i+4 < _numencoders) {
enc5_8 |= (1<<i);
}
}
reset_str[0] = 'c';
reset_str[1] = (unsigned char)(ana1_4 + offset); // Hope we don't need to set baud rate
reset_str[2] = (unsigned char)((ana5_8 | (dig1_3 << 4)) + offset);
reset_str[3] = (unsigned char)(0 + offset);
reset_str[4] = (unsigned char)(0 + offset);
reset_str[5] = (unsigned char)(enc1_4 + offset);
reset_str[6] = (unsigned char)(enc1_4 + offset); // Set encoders 1-4 for incremental
reset_str[7] = (unsigned char)(enc5_8 + offset);
reset_str[8] = (unsigned char)(enc5_8 + offset); // Set encoders 5-8 for incremental
reset_str[9] = '\n';
reset_str[10] = 0;
}
// Send the string and then wait for an acknowledgement from the box.
vrpn_write_characters(serial_fd, reset_str, 10);
timeout.tv_sec = 2;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, 2, &timeout);
if (ret < 0) {
perror("vrpn_CerealBox: Error reading ack from box\n");
return -1;
}
if (ret == 0) {
fprintf(stderr,"vrpn_CerealBox: No ack from box\n");
return -1;
}
if (ret != 2) {
fprintf(stderr,"vrpn_CerealBox: Got %d of 2 expected ack characters\n",ret);
return -1;
}
if (inbuf[0] != 'a') {
fprintf(stderr,"vrpn_CerealBox: Bad ack: wanted 'a', got '%c'\n",inbuf[0]);
return -1;
}
//-----------------------------------------------------------------------
// Ask the box to send a report, and go into SYNCING mode to get it.
vrpn_write_characters(serial_fd, (unsigned char *)"pE", 2);
status = STATUS_SYNCING;
printf("CerealBox reset complete.\n");
//-----------------------------------------------------------------------
// Figure out how many characters to expect in each report from the device
// There is a 'p' to start and '\n' to finish, two bytes for each analog
// value, 4 bytes for each encoder. Buttons are enabled in banks of 8,
// but each bank of 8 is returned in 2 bytes (4 bits each).
_expected_chars = 2 + 2*_numchannels + _numencoders*4 +
((_numbuttons+7) / 8) * 2;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
ConfSection::~ConfSection()
{
clear_values();
}
void vrpn_Tracker_3DMouse::reset()
{
static int numResets = 0; // How many resets have we tried?
int ret, i;
numResets++; // We're trying another reset
clear_values();
fprintf(stderr, "Resetting the 3DMouse (attempt %d)\n", numResets);
if (vrpn_write_characters(serial_fd, (unsigned char*)"*R", 2) == 2)
{
fprintf(stderr,".");
vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
}
else
{
perror("3DMouse: Failed writing to 3DMouse");
status = vrpn_TRACKER_FAIL;
return;
}
fprintf(stderr,"\n");
// Get rid of the characters left over from before the reset
vrpn_flush_input_buffer(serial_fd);
// Make sure that the tracker has stopped sending characters
vrpn_SleepMsecs(1000.0*2);
if ( (ret = vrpn_read_available_characters(serial_fd, _buffer, 80)) != 0)
{
fprintf(stderr, "Got >=%d characters after reset\n", ret);
for (i = 0; i < ret; i++)
{
if (isprint(_buffer[i])) fprintf(stderr,"%c",_buffer[i]);
else fprintf(stderr,"[0x%02X]",_buffer[i]);
}
fprintf(stderr, "\n");
vrpn_flush_input_buffer(serial_fd); // Flush what's left
}
// Asking for tracker status
if (vrpn_write_characters(serial_fd, (const unsigned char *) "*\x05", 2) == 2)
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
else
{
perror(" 3DMouse write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Read Status
bool success = true;
ret = vrpn_read_available_characters(serial_fd, _buffer, 2);
if (ret != 2) fprintf(stderr, " Got %d of 5 characters for status\n",ret);
fprintf(stderr, " Control Unit test : ");
if (_buffer[0] & 1) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Processor test : ");
if (_buffer[0] & 2) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " EPROM checksum test : ");
if (_buffer[0] & 4) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " RAM checksum test : ");
if (_buffer[0] & 8) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Transmitter test : ");
if (_buffer[0] & 16) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Receiver test : ");
if (_buffer[0] & 32) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Serial Port test : ");
if (_buffer[1] & 1) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " EEPROM test : ");
if (_buffer[0] & 2) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
if (!success)
{
fprintf(stderr, "Bad status report from 3DMouse, retrying reset\n");
status = vrpn_TRACKER_FAIL;
return;
}
else
{
fprintf(stderr, "3DMouse gives status (this is good)\n");
numResets = 0; // Success, use simple reset next time
}
// Set filtering count if the constructor parameter said to.
if (_filtering_count > 1)
{
if (!set_filtering_count(_filtering_count)) return;
}
fprintf(stderr, "Reset Completed (this is good)\n");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
// This creates a vrpn_3DMicroscribe and sets it to reset mode.
vrpn_3DMicroscribe::vrpn_3DMicroscribe (const char * name, vrpn_Connection * c,
const char * Port, long int BaudRate,
float OffsetX/* = 0.0f*/,
float OffsetY/* = 0.0f*/,
float OffsetZ/* = 0.0f*/,
float Scale/*=1.0f*/):
vrpn_Tracker(name, c),
vrpn_Button_Filter(name, c),
_numbuttons(2)
{
// Set the parameters in the parent classes
vrpn_Button::num_buttons = _numbuttons;
if(!strcmp(Port, "COM1") )
m_PortNumber=1;
else if(!strcmp(Port, "COM2") )
m_PortNumber=2;
else if(!strcmp(Port, "COM3") )
m_PortNumber=3;
else if(!strcmp(Port, "COM4") )
m_PortNumber=4;
m_BaudRate=BaudRate;
m_OffSet[0]=OffsetX; m_OffSet[1]=OffsetY; m_OffSet[2]=OffsetZ;
m_Scale=Scale;
// Set the status of the buttons and analogs to 0 to start
clear_values();
#ifdef VRPN_USE_MICROSCRIBE
int iResult;
iResult=ArmStart(NULL);
if(ARM_SUCCESS != iResult)
{
//error starting the MicroScribe drivers
VRPN_MSG_ERROR( "Unable to start MicroScribe ArmDll32." );
return;
}
//don't use error handlers
iResult = ArmSetErrorHandlerFunction(NO_HCI_HANDLER, NULL);
iResult = ArmSetErrorHandlerFunction(BAD_PORT_HANDLER, NULL);
iResult = ArmSetErrorHandlerFunction(CANT_OPEN_HANDLER, NULL);
iResult = ArmSetErrorHandlerFunction(CANT_BEGIN_HANDLER, NULL);
//connect to the correct port
switch(m_PortNumber)
{
case 1:
iResult = ArmConnect(1, m_BaudRate);
break;
case 2:
iResult = ArmConnect(2, m_BaudRate);
break;
case 3:
iResult = ArmConnect(3, m_BaudRate);
break;
case 4:
iResult = ArmConnect(4, m_BaudRate);
break;
default:
iResult = ArmConnect(0, 0); //try all available ports and baud rates
break;
}
if(ARM_SUCCESS != iResult)
{
//error connecting, end the thread
ArmEnd();
VRPN_MSG_ERROR( "Unable to connect to the MicroScribe." );
return;
}
#endif
// Set the mode to reset
status = STATUS_RESETTING;
}
