Something went wrong on our end
vrpn_Analog_Radamec_SPI.C 19.62 KiB
// vrpn_Radamec_SPI.C
// This is a driver for the Radamec Serial Position Interface
// "virtual set" camera tracking rig. It plugs into a serial
// line and communicates using RS-232 (this is a raw-mode driver).
// You can find out more at www.radamec.com. They have a manual with
// a section on the serial interface; this code is based on that and
// communications with the vendor and experimentation. It was written in
// August 2000 by Russ Taylor.
// INFO about how the device communicates:
// Reports coming from the device have a 1-byte header that tells
// which of 7 possible commands is being sent (0xA0-0xA6). Each then has
// a 1-byte Camera ID, then other parameters specific to the type, then
// a CRC checksum.
// Most of the report types can be sent to the instrument to set
// various parameters. Report type 4 is mainly a command/query message,
// which the device echoes.
#include <stdio.h> // for sprintf
#include <string.h> // for NULL, memcpy
#include "vrpn_Analog_Radamec_SPI.h"
#include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR, etc
#include "vrpn_Serial.h"
#include "vrpn_Shared.h" // for timeval, vrpn_unbuffer, etc
#include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
#undef VERBOSE
// Defines the modes in which the device can find itself.
#define STATUS_RESETTING (-1) // Resetting the device
#define STATUS_SYNCING (0) // Looking for the first character of report
#define STATUS_READING (1) // Looking for the rest of the report
#define MAX_TIME_INTERVAL (2000000) // max time between reports (usec)
// This creates a vrpn_Radamec_SPI and sets it to reset mode. It opens
// the serial device using the code in the vrpn_Serial_Analog constructor.
vrpn_Radamec_SPI::vrpn_Radamec_SPI (const char * name, vrpn_Connection * c,
const char * port, int baud):
vrpn_Serial_Analog(name, c, port, baud, 8, vrpn_SER_PARITY_ODD),
_numchannels(4), // This is an estimate; will change when reports come
_camera_id(-1) // Queried from the controller during reset
{
// 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;
}
void vrpn_Radamec_SPI::clear_values(void)
{
int i;
for (i = 0; i < _numchannels; i++) {
vrpn_Analog::channel[i] = vrpn_Analog::last[i] = 0;
}
}
/** This routine will compute the CRC for the message that starts at head
and is of length len. This needs to be added to each message sent to the
device, and should be checked for each command received by the device.
The manual states that the CRC is "calculated by subtracting the total sum
of each byte of the block from 40H."
*/
unsigned char vrpn_Radamec_SPI::compute_crc(const unsigned char *head, int len)
{
int i;
unsigned char sum;
// Sum up the bytes, allowing them to overflow the unsigned char
sum = 0;
for (i = 0; i < len; i++) {
sum = (unsigned char)( sum + head[i] );
}
// Unsigned subtraction from 40H, again allowing the subtraction to overflow
return (unsigned char)(0x40 - sum);
}
/** Compute the CRC for the command that is being sent, append that to the
message, and send the message.
Returns 0 on success, -1 on failure.
*/
int vrpn_Radamec_SPI::send_command(const unsigned char *cmd, int len)
{
int ret;
unsigned char *outbuf = new unsigned char[len+1]; // Leave room for the CRC
// Put the command into the output buffer
memcpy(outbuf, cmd, len);
// Add the CRC
outbuf[len] = compute_crc(cmd, len);
// Send the command
ret = vrpn_write_characters(serial_fd, outbuf, len+1);
// Tell if this all worked.
if (ret == len+1) {
return 0;
} else {
return -1;
}
}
/** Convert a 24-bit value from a buffer into an unsigned integer value.
The value has the most significant byte first in the buffer.
*/
vrpn_uint32 vrpn_Radamec_SPI::convert_24bit_unsigned(const unsigned char *buf)
{
vrpn_uint32 retval;
unsigned char bigend_buf[4];
const unsigned char *bufptr = bigend_buf;
// Store the three values into three bytes of a big-endian 32-bit integer
bigend_buf[0] = 0;
bigend_buf[1] = buf[0];
bigend_buf[2] = buf[1];
bigend_buf[3] = buf[2];
// Convert the value to an integer
vrpn_unbuffer((const char **)&bufptr, &retval);
return retval;
}
/** Convert a 16-bit unsigned value from a buffer into an integer value.
The value has the most significant byte first in the buffer.
*/
vrpn_int32 vrpn_Radamec_SPI::convert_16bit_unsigned(const unsigned char *buf)
{
vrpn_int32 retval;
unsigned char bigend_buf[4];
const unsigned char *bufptr = bigend_buf;
// Store the three values into two bytes of a big-endian 32-bit integer
bigend_buf[0] = 0;
bigend_buf[1] = 0;
bigend_buf[2] = buf[0];
bigend_buf[3] = buf[1];
// Convert the value to an integer
vrpn_unbuffer((const char **)&bufptr, &retval);
return retval;
}
/** ------------------- Conversion of encoder indices to values ----------------
Pan and tilt axis have an encoder index pulse which resets the value to
7FFFF hex each time the head passes through 0 degrees, the centre of range
of the head. This is indicated by two red dots on the pan axis, which when
aligned show the head is at 0 degrees. Tilt 0 position is when the camera
is horizontal. There are 900 encoder counts per degree and the direction
corresponding to an increase in count depends on the mechanical orientation
of the head and camera but is configurable using the configuration block
anyway.
Zoom and focus return an encoder count corresponding to the barrel
rotation of the lens. You will have to calibrate this to give a value for
field of view for the virtual set. This is not an easy task as lens
characteristics are non-linear. All virtual studio system suppliers have
varying methods of doing lens calibration, including our own proprietary
method used for Radamec Virtual Scenario.
*/
double vrpn_Radamec_SPI::int_to_pan(vrpn_uint32 val)
{
return (((int)val) - 0x7ffff) / 900.0;
}
double vrpn_Radamec_SPI::int_to_zoom(vrpn_uint32 val)
{
//XXX Unknown conversion, return the raw value
return val;
}
double vrpn_Radamec_SPI::int_to_focus(vrpn_uint32 val)
{
//XXX Unknown conversion, return the raw value
return val;
}
double vrpn_Radamec_SPI::int_to_height(vrpn_uint32 val)
{
//XXX Unknown conversion, send the integer along unchanged
return val;
}
/** Convert from the millimeter and fraction-of-millimeter
values returned by the device into meters.
*/
double vrpn_Radamec_SPI::int_to_X(vrpn_uint32 mm, vrpn_uint32 frac)
{
return 0.001 * (mm + frac/65536.0);
}
/** Convert from the 1/100 degree increments into degrees. */
double vrpn_Radamec_SPI::int_to_orientation(vrpn_uint32 val)
{
return 0.01 * val;
}
// This routine will reset the Radamec_SPI, requesting the camera number from the
// device and then turning on stream mode. XXX The device never seems to return a
// response to these queries, but always seems to go ahead and spew reports, even
// when not genlocked. If you find a device that doesn't send reports, this may
// be a good place to look for the problem.
// Commands Responses Meanings
// <A4><FF><02><CRC> <A4><##><02><CRC> Request camera number; returns camera number in ##
// <A4><##><01><CRC> <A4><##><01><CRC> Start stream mode on the camera we were told we are
int vrpn_Radamec_SPI::reset(void)
{
unsigned char command[128];
/* XXX commented out, since the response doesn't come, but the device still works.
struct timeval timeout;
unsigned char inbuf[128];
int ret;
char errmsg[256];
*/
//-----------------------------------------------------------------------
// Send the command to request the camera ID and then read the response.
// Give it a reasonable amount of time to finish (2 seconds), then timeout
vrpn_flush_input_buffer(serial_fd);
sprintf((char *)command, "%c%c%c", 0xa4, 0xff, 0x02);
send_command((unsigned char *)command, 3);
/* XXX commented out, since the response doesn't come, but the device still works.
timeout.tv_sec = 2;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, 4, &timeout);
inbuf[4] = 0; // Make sure string is NULL-terminated
if (ret < 0) {
perror("vrpn_Radamec_SPI reset: Error reading camera ID from device\n");
return -1;
}
if (ret == 0) {
VRPN_MSG_ERROR("reset: No response to camera ID from device");
return -1;
}
if (ret != 4) {
sprintf(errmsg,"reset: Got %d of %d expected characters for camera ID\n",ret, 4);
VRPN_MSG_ERROR(errmsg);
return -1;
}
// Make sure the string we got back is what we expected and then find out the camera ID
if ( (inbuf[0] != 0xa4) || (inbuf[2] != 0x02) || (inbuf[3] != compute_crc(inbuf,3)) ) {
VRPN_MSG_ERROR("reset: Bad response to camera # request");
return -1;
}
_camera_id = inbuf[1];
*/
//-----------------------------------------------------------------------
// Send the command to put the camera into stream mode and then read back
// to make sure we got a response.
sprintf((char *)command, "%c%c%c", 0xa4, _camera_id, 0x01);
send_command(command, 3);
/* XXX commented out, since the response doesn't come, but the device still works.
timeout.tv_sec = 2;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, 4, &timeout);
inbuf[4] = 0; // Make sure string is NULL-terminated if (ret < 0) {
VRPN_MSG_ERROR("reset: Error reading from device");
return -1;
}
if (ret == 0) {
VRPN_MSG_ERROR("reset: No response from device");
return -1;
}
if (ret != 4) {
sprintf(errmsg,"vrpn_Radamec_SPI reset: Got %d of %d expected characters\n",ret, 4);
VRPN_MSG_ERROR(errmsg);
return -1;
}
// Make sure the string we got back is what we expected
if ( (inbuf[0] != 0xa4) || (inbuf[1] != _camera_id) || (inbuf[2] != 0x01) ||
(inbuf[3] != compute_crc(inbuf,3)) ) {
VRPN_MSG_ERROR("reset: Bad response to start stream mode command");
return -1;
}
*/
// We're now waiting for a response from the box
status = STATUS_SYNCING;
VRPN_MSG_WARNING("reset complete (this is good)");
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
// This function will read characters until it has a full report, then
// put that report into analog fields and call the report methods on these.
// The time stored is that of the first character received as part of the
// report.
// Reports start with different characters, and the length of the report
// depends on what the first character of the report is. We switch based
// on the first character of the report to see how many more to expect and
// to see how to handle the report.
int vrpn_Radamec_SPI::get_report(void)
{
int ret; // Return value from function call to be checked
char errmsg[256];
//--------------------------------------------------------------------
// If we're SYNCing, then the next character we get should be the start
// of a report. If we recognize it, go into READing mode and tell how
// many characters we expect total. If we don't recognize it, then we
// must have misinterpreted a command or something; reset the Magellan
// and start over
//--------------------------------------------------------------------
if (status == STATUS_SYNCING) {
// Try to get a character. If none, just return.
if (vrpn_read_available_characters(serial_fd, _buffer, 1) != 1) {
return 0;
}
switch (_buffer[0]) {
case 0xa0:
_expected_chars = 15; status = STATUS_READING; break;
case 0xa1:
_expected_chars = 18; status = STATUS_READING; break;
case 0xa2:
_expected_chars = 30; status = STATUS_READING; break;
case 0xa3:
_expected_chars = 18; status = STATUS_READING; break;
case 0xa4:
_expected_chars = 4; status = STATUS_READING; break;
case 0xa5: _expected_chars = 5; status = STATUS_READING; break;
case 0xa6:
_expected_chars = 26; status = STATUS_READING; break;
default:
// Not a recognized command, keep looking
return 0;
}
// Got the first character of a report -- go into READING mode
// and record that we got one character at this time. The next
// bit of code will attempt to read the rest of the report.
// The time stored here is as close as possible to when the
// report was generated.
_bufcount = 1;
vrpn_gettimeofday(×tamp, NULL);
status = STATUS_READING;
#ifdef VERBOSE
printf("... Got the 1st char\n");
#endif
}
//--------------------------------------------------------------------
// Read as many bytes of this report as we can, storing them
// in the buffer. We keep track of how many have been read so far
// and only try to read the rest.
//--------------------------------------------------------------------
ret = vrpn_read_available_characters(serial_fd, &_buffer[_bufcount],
_expected_chars-_bufcount);
if (ret == -1) {
VRPN_MSG_ERROR("Error reading");
status = STATUS_RESETTING;
return 0;
}
_bufcount += ret;
#ifdef VERBOSE
if (ret != 0) printf("... got %d characters (%d total)\n",ret, _bufcount);
#endif
if (_bufcount < _expected_chars) { // Not done -- go back for more
return 0;
}
//--------------------------------------------------------------------
// We now have enough characters to make a full report. Check to make
// sure that its format matches what we expect. If it does, the next
// section will parse it. If it does not, we need to go back into
// synch mode and ignore this report. A well-formed report has the
// correct CRC as its last character, and has the camera ID matching
// what we expect as its second character.
//--------------------------------------------------------------------
if (_buffer[_expected_chars-1] != compute_crc(_buffer, _expected_chars-1) ) {
status = STATUS_SYNCING;
VRPN_MSG_WARNING("Bad CRC in report (ignoring this report)");
return 0;
}
_camera_id = _buffer[1];
#ifdef VERBOSE
printf("got a complete report (%d of %d)!\n", _bufcount, _expected_chars);
#endif
//--------------------------------------------------------------------
// Decode the report and store the values in it into the analog values
// if appropriate.
//--------------------------------------------------------------------
switch ( _buffer[0] ) { case 0xa0: // Pan, Tilt, Zoom, Focus
_numchannels = 4;
channel[0] = int_to_pan(convert_24bit_unsigned(&_buffer[2]));
channel[1] = int_to_tilt(convert_24bit_unsigned(&_buffer[5]));
channel[2] = int_to_zoom(convert_24bit_unsigned(&_buffer[8]));
channel[3] = int_to_focus(convert_24bit_unsigned(&_buffer[11]));
break;
case 0xa1: // Pan, Tilt, Zoom, Focus, Height
_numchannels = 5;
channel[0] = int_to_pan(convert_24bit_unsigned(&_buffer[2]));
channel[1] = int_to_tilt(convert_24bit_unsigned(&_buffer[5]));
channel[2] = int_to_zoom(convert_24bit_unsigned(&_buffer[8]));
channel[3] = int_to_focus(convert_24bit_unsigned(&_buffer[11]));
channel[4] = int_to_height(convert_24bit_unsigned(&_buffer[14]));
break;
case 0xa2: // Pan, Tilt, Zoom, Focus, Height, X, Y, Orientation
_numchannels = 8;
channel[0] = int_to_pan(convert_24bit_unsigned(&_buffer[2]));
channel[1] = int_to_tilt(convert_24bit_unsigned(&_buffer[5]));
channel[2] = int_to_zoom(convert_24bit_unsigned(&_buffer[8]));
channel[3] = int_to_focus(convert_24bit_unsigned(&_buffer[11]));
channel[4] = int_to_height(convert_24bit_unsigned(&_buffer[14]));
// Note that fraction is first, and is unsigned, in the buffer
channel[5] = int_to_X(convert_16bit_unsigned(&_buffer[19]),
convert_16bit_unsigned(&_buffer[17]));
// Note that fraction is first, and is unsigned, in the buffer
channel[6] = int_to_X(convert_16bit_unsigned(&_buffer[23]),
convert_16bit_unsigned(&_buffer[21]));
channel[7] = int_to_orientation(convert_16bit_unsigned(&_buffer[25]));
break;
case 0xa4: // Response to our reset commands -- ignore them
break;
// Note that case 0xa3 should never happen, since we don't send this.
// We'll let the "default" case complain about getting it.
// Note that 0xa5 should not happen, since we don't send it.
// We'll let the "default" case complain about getting it.
case 0xa6: // Pan, Tilt, Zoom, Focus, Height, X, Y, Orientation
// The same as A2, except that fractional X,Y not included.
_numchannels = 8;
channel[0] = int_to_pan(convert_24bit_unsigned(&_buffer[2]));
channel[1] = int_to_tilt(convert_24bit_unsigned(&_buffer[5]));
channel[2] = int_to_zoom(convert_24bit_unsigned(&_buffer[8]));
channel[3] = int_to_focus(convert_24bit_unsigned(&_buffer[11]));
channel[4] = int_to_height(convert_24bit_unsigned(&_buffer[14]));
// Note that fraction is not in the buffer
channel[5] = int_to_X(convert_16bit_unsigned(&_buffer[17]), 0);
// Note that fraction is not in the buffer
channel[6] = int_to_X(convert_16bit_unsigned(&_buffer[19]), 0);
channel[7] = int_to_orientation(convert_16bit_unsigned(&_buffer[21]));
break;
default:
sprintf(errmsg,"vrpn_Radamec_SPI: Unhandled command (0x%02x), resetting\n", _buffer[0]);
VRPN_MSG_ERROR(errmsg);
status = STATUS_RESETTING;
return 0;
}
//--------------------------------------------------------------------
// Done with the decoding, send the reports and go back to syncing
//--------------------------------------------------------------------
report(); // Report, rather than report_changes(), since it is an absolute device status = STATUS_SYNCING;
_bufcount = 0;
return 1;
}
void vrpn_Radamec_SPI::report_changes(vrpn_uint32 class_of_service)
{
vrpn_Analog::timestamp = timestamp;
vrpn_Analog::report_changes(class_of_service);
}
void vrpn_Radamec_SPI::report(vrpn_uint32 class_of_service)
{
vrpn_Analog::timestamp = timestamp;
vrpn_Analog::report(class_of_service);
}
/** This routine is called each time through the server's main loop. It will
take a course of action depending on the current status of the device,
either trying to reset it or trying to get a reading from it. It will
try to reset the device if no data has come from it for a couple of
seconds
*/
void vrpn_Radamec_SPI::mainloop()
{
char errmsg[256];
server_mainloop();
switch(status) {
case STATUS_RESETTING:
reset();
break;
case STATUS_SYNCING:
case STATUS_READING:
{
// It turns out to be important to get the report before checking
// to see if it has been too long since the last report. This is
// because there is the possibility that some other device running
// in the same server may have taken a long time on its last pass
// through mainloop(). Trackers that are resetting do this. When
// this happens, you can get an infinite loop -- where one tracker
// resets and causes the other to timeout, and then it returns the
// favor. By checking for the report here, we reset the timestamp
// if there is a report ready (ie, if THIS device is still operating).
while (get_report()) {}; // Keep getting reports so long as there are more
struct timeval current_time;
vrpn_gettimeofday(¤t_time, NULL);
if ( vrpn_TimevalDuration(current_time,timestamp) > MAX_TIME_INTERVAL) {
sprintf(errmsg,"Timeout... current_time=%ld:%ld, timestamp=%ld:%ld",
current_time.tv_sec, static_cast<long>(current_time.tv_usec),
timestamp.tv_sec, static_cast<long>(timestamp.tv_usec));
VRPN_MSG_ERROR(errmsg);
status = STATUS_RESETTING;
}
}
break;
default:
VRPN_MSG_ERROR("Unknown mode (internal error)");
break;
}
}