12#define STATUS_NOT_INITIALIZED (-2)
13#define STATUS_RESETTING (-1)
14#define STATUS_READING (0)
18static const int REPORT_LENGTH = 16 + 16 + 12 + 36 + 4 + 4 + 1;
19#define MAX_TIME_INTERVAL (2000000)
30 ,
double frames_per_second
31 ,
const char *reset_commands[])
38 const char **ptr = reset_commands;
41 if (ptr != NULL)
while ((*ptr) != NULL) {
89 fprintf(stderr,
"vrpn_YEI_3Space::~vrpn_YEI_3Space(): delete failed\n");
102 ,
double red_LED_color
103 ,
double green_LED_color
104 ,
double blue_LED_color
108 unsigned char set_LED_mode[2] = { 0xC4, 0 };
109 set_LED_mode[1] =
static_cast<unsigned char>(LED_mode);
111 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::init: Unable to send set-LED-mode command\n");
114 printf(
"LED mode set\n");
118 unsigned char set_LED_color[13] = { 0xEE, 0,0,0,0,0,0,0,0,0,0,0,0 };
119 unsigned char *bufptr = &set_LED_color[1];
120 vrpn_int32 buflen = 12;
121 vrpn_float32 LEDs[3];
122 LEDs[0] =
static_cast<vrpn_float32
>(red_LED_color);
123 LEDs[1] =
static_cast<vrpn_float32
>(green_LED_color);
124 LEDs[2] =
static_cast<vrpn_float32
>(blue_LED_color);
129 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::init: Unable to send set-LED-color command\n");
133 if (calibrate_gyros_on_setup) {
134 unsigned char begin_gyroscope_calibration[1] = { 0xA5 };
135 if (!
send_binary_command (begin_gyroscope_calibration,
sizeof(begin_gyroscope_calibration))) {
136 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::init: Unable to send set-gyroscope-calibration command\n");
142 unsigned char tare[1] = { 0x60 };
144 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::init: Unable to send tare command\n");
152 printf(
"init() complete\n");
164 struct timeval l_timeout;
168 unsigned char stop_streaming[1] = { 0x56 };
170 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send stop-streaming command\n");
175 unsigned char get_led_mode[1] = { 0xC8 };
177 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send get-led-mode command\n");
180 l_timeout.tv_sec = 2;
181 l_timeout.tv_usec = 0;
183 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to read get-led-mode response\n");
188 unsigned char get_led_values[1] = { 0xEF };
190 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send get-led-values command\n");
194 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to read get-led-mode response\n");
199 unsigned char set_rh_system[] = { 0x74, 0x01 };
201 VRPN_MSG_ERROR(
"vrpn_YEI_3Space::reset: Unable to send coordinate system selection command\n");
206 unsigned char set_streaming_timing[13] = { 0x52, 0,0,0,0,0,0,0,0,0,0,0,0 };
207 vrpn_uint32 interval;
209 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Bad frames/second value, setting to maximum\n");
214 vrpn_uint32 duration = 0xFFFFFFFF;
215 vrpn_uint32 delay = 0;
216 unsigned char *bufptr = &set_streaming_timing[1];
217 vrpn_int32 buflen = 12;
222 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send set-streaming-timing command\n");
231 unsigned char set_streaming_slots[9] = { 0x50,
241 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send set-streaming-slots command\n");
249 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send additional reset command\n");
255 unsigned char start_streaming[1] = { 0x55 };
257 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send start-streaming command\n");
280 unsigned char *bufptr =
report;
287 for (
int i = 0; i < 2; i++) {
297 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::handle_report(): Error sending sensor report");
311 static const double GRAVITY = 9.80665;
312 acc[Q_X] = value * GRAVITY;
314 acc[Q_Y] = value * GRAVITY;
316 acc[Q_Z] = value * GRAVITY;
320 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::handle_report(): Error sending acceleration report");
335 VRPN_MSG_ERROR(
"vrpn_YEI_3Space::handle_report(): Invalid confidence, resetting");
342 for (
int i = 0; i < 8; i++) {
343 buttons[i] = (b & (1 << i)) != 0;
419 struct timeval current_time;
422 sprintf (l_errmsg,
"vrpn_YEI_3Space::mainloop: Timeout... current_time=%ld:%ld, timestamp=%ld:%ld",
424 static_cast<long> (current_time.tv_usec),
434 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::mainloop: Unknown mode (internal error)");
449 ,
const char * p_port
451 ,
bool calibrate_gyros_on_setup
453 ,
double frames_per_second
454 ,
double red_LED_color
455 ,
double green_LED_color
456 ,
double blue_LED_color
458 ,
const char *reset_commands[])
464 perror(
"vrpn_YEI_3Space_Sensor::vrpn_YEI_3Space_Sensor: Cannot open serial port");
465 fprintf(stderr,
" (port %s)\n", p_port);
470 init(calibrate_gyros_on_setup
471 , tare_on_setup, red_LED_color
472 , green_LED_color, blue_LED_color, LED_mode);
484 unsigned char stop_streaming[1] = { 0x56 };
486 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::~vrpn_YEI_3Space_Sensor_Wireless: Unable to send stop-streaming command\n");
512 const unsigned char START_OF_PACKET = 0xF7;
518 for (
int i = 0; i < p_len; i++) {
519 checksum += p_cmd[i];
524 unsigned char buffer[256];
525 buffer[0] = START_OF_PACKET;
526 memcpy(&(buffer[1]), p_cmd, p_len);
527 buffer[p_len + 1] =
static_cast<unsigned char>(checksum);
533 if (l_ret == p_len+2) {
557 if (strlen(p_cmd) == 0) {
562 int buflen =
static_cast<int>(strlen(p_cmd) + 3);
563 unsigned char *buffer;
564 try { buffer =
new unsigned char[buflen]; }
571 memcpy(&buffer[1], p_cmd, strlen(p_cmd));
572 buffer[buflen-2] =
'\n';
573 buffer[buflen-1] = 0;
582 fprintf(stderr,
"vrpn_YEI_3Space_Sensor::send_ascii_command(): delete failed\n");
587 if (l_ret == buflen) {
609 if (ret !=
sizeof(value)) {
629 unsigned char buffer[3*
sizeof(vrpn_float32)];
631 if (ret !=
sizeof(buffer)) {
635 unsigned char *bufptr = buffer;
665 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor::get_report(): Error reading the sensor, resetting");
670 if (l_ret != 0) printf(
"... got %d characters (%d total)\n",l_ret,
d_characters_read);
724 ,
const char * p_port
726 ,
bool calibrate_gyros_on_setup
728 ,
double frames_per_second
729 ,
double red_LED_color
730 ,
double green_LED_color
731 ,
double blue_LED_color
733 ,
const char *reset_commands[])
741 perror(
"vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless: Cannot open serial port");
742 fprintf(stderr,
" (port %s)\n", p_port);
745 printf(
"Serial port opened\n");
751 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless: Could not configure dongle\n");
757 printf(
"Dongle configured\n");
763 printf(
"Logical ID set\n");
767 init(calibrate_gyros_on_setup
768 , tare_on_setup, red_LED_color
769 , green_LED_color, blue_LED_color, LED_mode);
771 printf(
"Constructor done\n");
786 ,
int serial_file_descriptor
787 ,
bool calibrate_gyros_on_setup
789 ,
double frames_per_second
790 ,
double red_LED_color
791 ,
double green_LED_color
792 ,
double blue_LED_color
794 ,
const char *reset_commands[])
804 init(calibrate_gyros_on_setup
805 , tare_on_setup, red_LED_color
806 , green_LED_color, blue_LED_color, LED_mode);
818 unsigned char stop_streaming[1] = { 0x56 };
820 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::~vrpn_YEI_3Space_Sensor_Wireless: Unable to send stop-streaming command\n");
838 unsigned char set_mode[2] = { 0xb0, 0 };
840 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::configure_dongle: Unable to send set-streaming-mode command\n");
857 unsigned char set_id[6] = { 0xd1, 0, 0,0,0,0 };
858 unsigned char *bufptr = &set_id[1];
859 vrpn_int32 buflen = 5;
863 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::set_logical_id: Unable to send set-logical-id command\n");
867 printf(
"... Logical id %d set to serial_number %x\n", logical_id, serial_number);
884 const unsigned char START_OF_PACKET = 0xF7;
890 for (
int i = 0; i < p_len; i++) {
891 checksum += p_cmd[i];
896 unsigned char buffer[256];
897 buffer[0] = START_OF_PACKET;
898 memcpy(&(buffer[1]), p_cmd, p_len);
899 buffer[p_len + 1] =
static_cast<unsigned char>(checksum);
905 if (l_ret == p_len+2) {
932 const unsigned char START_OF_PACKET = 0xF8;
935 unsigned char buffer[256];
936 buffer[0] = START_OF_PACKET;
938 memcpy(&(buffer[2]), p_cmd, p_len);
945 for (
int i = 0; i < p_len+1; i++) {
946 checksum += buffer[1 + i];
949 buffer[p_len + 2] =
static_cast<unsigned char>(checksum);
954 if (l_ret != p_len+3) {
955 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Could not send command\n");
959 printf(
"... packet of length %d sent\n", l_ret);
967 struct timeval timeout;
969 timeout.tv_usec = 500000;
972 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Error (%d) from ID %d\n", buffer[0], buffer[1]);
974 }
else if (ret != 3) {
975 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Timeout waiting for command status (got %d chars)\n", ret);
978 if (buffer[0] != 0) {
979 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Command failed\n");
983 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Got response for incorrect logical ID\n");
987 printf(
"..... send succeded\n");
1013 if (strlen(p_cmd) == 0) {
1024 int buflen =
static_cast<int>(strlen(buffer) + 1);
1025 unsigned char *bufptr =
static_cast<unsigned char *
>(
1026 static_cast<void*
>(buffer));
1030 if (l_ret != buflen) {
1031 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Error sending command\n");
1040 struct timeval timeout;
1042 timeout.tv_usec = 500000;
1045 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Timeout waiting for command status\n");
1049 if (buffer[0] !=
'0') {
1050 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Command failed: response (%s)\n", buffer);
1053 if (buffer[strlen(buffer)-1] !=
'\n') {
1054 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Got ill-formatted response: (%s).\n", buffer);
1074 unsigned char value;
1076 if (ret !=
sizeof(value)) {
1096 unsigned char buffer[3*
sizeof(vrpn_float32)];
1098 if (ret !=
sizeof(buffer)) {
1102 unsigned char *bufptr = buffer;
1128 unsigned char release_report[2] = { 0xB4, 0 };
1131 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::get_report: Unable to send release-report command\n");
1137 struct timeval timeout;
1139 timeout.tv_usec = 1000;
1144 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::get_report(): Error reading the sensor, resetting");
1149 if (l_ret != 0) printf(
"... got %d characters\n",l_ret);
1158 if (l_ret != REPORT_LENGTH + 3) {
1159 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::get_report(): Truncated report, resetting");
1164 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::get_report(): Error reported, resetting");
1169 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::get_report(): Report from wrong sensor received, resetting");
vrpn_int32 getNumChannels(void) const
vrpn_float64 channel[vrpn_CHANNEL_MAX]
vrpn_Analog(const char *name, vrpn_Connection *c=NULL)
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report whether something has changed or not (for servers) Optionally, tell what time to stamp ...
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
virtual int init(void)
Initialize things that the constructor can't. Returns 0 on success, -1 on failure.
Generic connection class not specific to the transport mechanism.
virtual int report_pose_acceleration(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_float64 interval, const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
vrpn_Tracker_Server(const char *name, vrpn_Connection *c, vrpn_int32 sensors=1)
virtual int report_pose(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
These functions should be called to report changes in state, once per sensor.
virtual bool set_logical_id(vrpn_uint8 logical_id, vrpn_int32 serial_number)
Insert our serial number into the specified slot.
bool receive_LED_values_response(struct timeval *timeout=NULL)
Read and parse the response to an LED-values request command. NULL timeout pointer means wait forever...
bool send_ascii_command(const char *cmd)
Put a ':' character at the front and ' ' at the end and then send the resulting command as an ASCII c...
virtual bool configure_dongle(void)
Configure the dongle (called if we are the first one)
bool receive_LED_mode_response(struct timeval *timeout=NULL)
Read and parse the response to an LED-state request command. NULL timeout pointer means wait forever....
virtual ~vrpn_YEI_3Space_Sensor_Wireless()
Destructor.
vrpn_YEI_3Space_Sensor_Wireless(const char *name, vrpn_Connection *c, int logical_id, const char *port, int baud=115200, bool calibrate_gyros_on_setup=false, bool tare_on_setup=false, double frames_per_second=50, double red_LED_color=0, double green_LED_color=0, double blue_LED_color=0, int LED_mode=1, const char *reset_commands[]=NULL)
Constructor for the first device, which will open the serial port and configure the dongle.
virtual void flush_input(void)
Flush any incoming characters in the communications channel.
virtual bool get_report(void)
Get and handle a report from the device if one is available. Return true if one was available,...
bool send_binary_command(const unsigned char *cmd, int len)
Compute the CRC for the message, append it, and send message. Returns true on success,...
bool send_binary_command_to_dongle(const unsigned char *cmd, int len)
Compute the CRC for the message, append it, and send message to the dongle directly (not a wireless c...
bool receive_LED_values_response(struct timeval *timeout=NULL)
Read and parse the response to an LED-values request command. NULL timeout pointer means wait forever...
virtual bool get_report(void)
virtual void flush_input(void)
Flush any incoming characters in the communications channel.
bool receive_LED_mode_response(struct timeval *timeout=NULL)
Read and parse the response to an LED-state request command. NULL timeout pointer means wait forever....
bool send_ascii_command(const char *cmd)
Put a ':' character at the front and ' ' at the end and then send the resulting command as an ASCII c...
virtual ~vrpn_YEI_3Space_Sensor()
Destructor.
bool send_binary_command(const unsigned char *cmd, int len)
Compute the CRC for the message, append it, and send message. Returns true on success,...
vrpn_YEI_3Space_Sensor(const char *name, vrpn_Connection *c, const char *port, int baud=115200, bool calibrate_gyros_on_setup=false, bool tare_on_setup=false, double frames_per_second=50, double red_LED_color=0, double green_LED_color=0, double blue_LED_color=0, int LED_mode=1, const char *reset_commands[]=NULL)
Constructor.
virtual bool receive_LED_values_response(struct timeval *timeout=NULL)=0
Read and parse the response to an LED-values request command. NULL timeout pointer means wait forever...
virtual void handle_report(unsigned char *report)
vrpn_float32 d_LED_color[3]
double d_frames_per_second
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
virtual void mainloop()
Called once through each main loop iteration to handle updates.
vrpn_YEI_3Space(const char *name, vrpn_Connection *c, double frames_per_second=50, const char *reset_commands[]=NULL)
Constructor.
virtual bool receive_LED_mode_response(struct timeval *timeout=NULL)=0
Read and parse the response to an LED-state request command. NULL timeout pointer means wait forever....
virtual bool get_report(void)=0
virtual bool send_ascii_command(const char *cmd)=0
Put a ':' character at the front and ' ' at the end and then send the resulting command as an ASCII c...
unsigned d_expected_characters
int d_reset_command_count
unsigned d_characters_read
virtual void flush_input(void)=0
Flush any incoming characters in the communications channel.
virtual ~vrpn_YEI_3Space()
Destructor.
virtual bool send_binary_command(const unsigned char *cmd, int len)=0
Compute the CRC for the message, append it, and send message. Returns true on success,...
unsigned char d_buffer[128]
#define MAX_TIME_INTERVAL
Header containing macros formerly duplicated in a lot of implementation files.
#define VRPN_MSG_ERROR(msg)
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
int vrpn_close_commport(int comm)
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
int vrpn_open_commport(const char *portname, long baud, int charsize, vrpn_SER_PARITY parity, bool rts_flow)
Open a serial port, given its name and baud rate.
VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
Utility routine for taking a struct timeval from a buffer that was sent as a message.
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
VRPN_API int vrpn_buffer(char **insertPt, vrpn_int32 *buflen, const timeval t)
Utility routine for placing a timeval struct into a buffer that is to be sent as a message.
void vrpn_SleepMsecs(double dMilliSecs)
#define vrpn_gettimeofday
#define STATUS_NOT_INITIALIZED