12 #define STATUS_NOT_INITIALIZED (-2) // Not yet initialized 13 #define STATUS_RESETTING (-1) // Resetting the device 14 #define STATUS_READING (0) // Looking for the a report 18 static const int REPORT_LENGTH = 16 + 16 + 12 + 36 + 4 + 4 + 1;
19 #define MAX_TIME_INTERVAL (2000000) // max time between reports (usec) 30 ,
bool calibrate_gyros_on_setup
32 ,
double frames_per_second
33 ,
double red_LED_color
34 ,
double green_LED_color
35 ,
double blue_LED_color
37 ,
const char *reset_commands[])
41 , d_frames_per_second(frames_per_second)
44 const char **ptr = reset_commands;
47 if (ptr != NULL)
while ((*ptr) != NULL) {
54 fprintf(stderr,
"vrpn_YEI_3Space::vrpn_YEI_3Space(): Out of memory, ignoring reset commands\n");
64 fprintf(stderr,
"vrpn_YEI_3Space::vrpn_YEI_3Space(): Out of memory, giving up\n");
109 ,
double frames_per_second
110 ,
double red_LED_color
111 ,
double green_LED_color
112 ,
double blue_LED_color
116 unsigned char set_LED_mode[2] = { 0xC4, 0 };
117 set_LED_mode[1] = static_cast<unsigned char>(LED_mode);
119 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::init: Unable to send set-LED-mode command\n");
122 printf(
"LED mode set\n");
126 unsigned char set_LED_color[13] = { 0xEE, 0,0,0,0,0,0,0,0,0,0,0,0 };
127 unsigned char *bufptr = &set_LED_color[1];
128 vrpn_int32 buflen = 12;
129 vrpn_float32 LEDs[3];
130 LEDs[0] = static_cast<vrpn_float32>(red_LED_color);
131 LEDs[1] = static_cast<vrpn_float32>(green_LED_color);
132 LEDs[2] = static_cast<vrpn_float32>(blue_LED_color);
137 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::init: Unable to send set-LED-color command\n");
141 if (calibrate_gyros_on_setup) {
142 unsigned char begin_gyroscope_calibration[1] = { 0xA5 };
143 if (!
send_binary_command (begin_gyroscope_calibration,
sizeof(begin_gyroscope_calibration))) {
144 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::init: Unable to send set-gyroscope-calibration command\n");
150 unsigned char tare[1] = { 0x60 };
152 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::init: Unable to send tare command\n");
160 printf(
"init() complete\n");
172 struct timeval l_timeout;
176 unsigned char stop_streaming[1] = { 0x56 };
178 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send stop-streaming command\n");
183 unsigned char get_led_mode[1] = { 0xC8 };
185 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send get-led-mode command\n");
188 l_timeout.tv_sec = 2;
189 l_timeout.tv_usec = 0;
191 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to read get-led-mode response\n");
196 unsigned char get_led_values[1] = { 0xEF };
198 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send get-led-values command\n");
202 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to read get-led-mode response\n");
207 unsigned char set_rh_system[] = { 0x74, 0x01 };
209 VRPN_MSG_ERROR(
"vrpn_YEI_3Space::reset: Unable to send coordinate system selection command\n");
214 unsigned char set_streaming_timing[13] = { 0x52, 0,0,0,0,0,0,0,0,0,0,0,0 };
215 vrpn_uint32 interval;
217 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Bad frames/second value, setting to maximum\n");
222 vrpn_uint32 duration = 0xFFFFFFFF;
223 vrpn_uint32 delay = 0;
224 unsigned char *bufptr = &set_streaming_timing[1];
225 vrpn_int32 buflen = 12;
230 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send set-streaming-timing command\n");
239 unsigned char set_streaming_slots[9] = { 0x50,
249 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send set-streaming-slots command\n");
257 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send additional reset command\n");
263 unsigned char start_streaming[1] = { 0x55 };
265 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::reset: Unable to send start-streaming command\n");
288 unsigned char *bufptr =
report;
295 for (
int i = 0; i < 2; i++) {
305 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::handle_report(): Error sending sensor report");
319 static const double GRAVITY = 9.80665;
320 acc[Q_X] = value * GRAVITY;
322 acc[Q_Y] = value * GRAVITY;
324 acc[Q_Z] = value * GRAVITY;
328 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::handle_report(): Error sending acceleration report");
343 VRPN_MSG_ERROR(
"vrpn_YEI_3Space::handle_report(): Invalid temperature, resetting");
352 VRPN_MSG_ERROR(
"vrpn_YEI_3Space::handle_report(): Invalid confidence, resetting");
359 for (
int i = 0; i < 8; i++) {
360 buttons[i] = (b & (1 << i)) != 0;
436 struct timeval current_time;
439 sprintf (l_errmsg,
"vrpn_YEI_3Space::mainloop: Timeout... current_time=%ld:%ld, timestamp=%ld:%ld",
441 static_cast<long> (current_time.tv_usec),
451 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::mainloop: Unknown mode (internal error)");
466 ,
const char * p_port
468 ,
bool calibrate_gyros_on_setup
470 ,
double frames_per_second
471 ,
double red_LED_color
472 ,
double green_LED_color
473 ,
double blue_LED_color
475 ,
const char *reset_commands[])
477 , tare_on_setup, frames_per_second, red_LED_color
478 , green_LED_color, blue_LED_color, LED_mode
484 perror(
"vrpn_YEI_3Space_Sensor::vrpn_YEI_3Space_Sensor: Cannot open serial port");
485 fprintf(stderr,
" (port %s)\n", p_port);
490 init(calibrate_gyros_on_setup
491 , tare_on_setup, frames_per_second, red_LED_color
492 , green_LED_color, blue_LED_color, LED_mode);
504 unsigned char stop_streaming[1] = { 0x56 };
506 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::~vrpn_YEI_3Space_Sensor_Wireless: Unable to send stop-streaming command\n");
532 const unsigned char START_OF_PACKET = 0xF7;
538 for (
int i = 0; i < p_len; i++) {
539 checksum += p_cmd[i];
544 unsigned char buffer[256];
545 buffer[0] = START_OF_PACKET;
546 memcpy(&(buffer[1]), p_cmd, p_len);
547 buffer[p_len + 1] = static_cast<unsigned char>(checksum);
553 if (l_ret == p_len+2) {
577 if (strlen(p_cmd) == 0) {
582 int buflen = static_cast<int>(strlen(p_cmd) + 3);
583 unsigned char *buffer =
new unsigned char[buflen];
584 if (buffer == NULL) {
590 memcpy(&buffer[1], p_cmd, strlen(p_cmd));
591 buffer[buflen-2] =
'\n';
592 buffer[buflen-1] = 0;
601 if (l_ret == buflen) {
623 if (ret !=
sizeof(value)) {
643 unsigned char buffer[3*
sizeof(vrpn_float32)];
645 if (ret !=
sizeof(buffer)) {
649 unsigned char *bufptr = buffer;
679 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor::get_report(): Error reading the sensor, resetting");
684 if (l_ret != 0) printf(
"... got %d characters (%d total)\n",l_ret,
d_characters_read);
739 ,
const char * p_port
741 ,
bool calibrate_gyros_on_setup
743 ,
double frames_per_second
744 ,
double red_LED_color
745 ,
double green_LED_color
746 ,
double blue_LED_color
748 ,
const char *reset_commands[])
750 , tare_on_setup, frames_per_second, red_LED_color
751 , green_LED_color, blue_LED_color, LED_mode
759 perror(
"vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless: Cannot open serial port");
760 fprintf(stderr,
" (port %s)\n", p_port);
763 printf(
"Serial port opened\n");
769 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless: Could not configure dongle\n");
775 printf(
"Dongle configured\n");
779 if (!
set_logical_id(static_cast<vrpn_uint8>(logical_id), serial_number)) {
780 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless: Could not set logical ID\n");
786 printf(
"Logical ID set\n");
790 init(calibrate_gyros_on_setup
791 , tare_on_setup, frames_per_second, red_LED_color
792 , green_LED_color, blue_LED_color, LED_mode);
794 printf(
"Constructor done\n");
810 ,
int serial_file_descriptor
811 ,
bool calibrate_gyros_on_setup
813 ,
double frames_per_second
814 ,
double red_LED_color
815 ,
double green_LED_color
816 ,
double blue_LED_color
818 ,
const char *reset_commands[])
820 , tare_on_setup, frames_per_second, red_LED_color
821 , green_LED_color, blue_LED_color, LED_mode
823 , d_i_am_first(false)
824 , d_serial_fd(serial_file_descriptor)
828 if (!
set_logical_id(static_cast<vrpn_uint8>(logical_id), serial_number)) {
829 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless: Could not set logical ID\n");
836 init(calibrate_gyros_on_setup
837 , tare_on_setup, frames_per_second, red_LED_color
838 , green_LED_color, blue_LED_color, LED_mode);
850 unsigned char stop_streaming[1] = { 0x56 };
852 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::~vrpn_YEI_3Space_Sensor_Wireless: Unable to send stop-streaming command\n");
870 unsigned char set_mode[2] = { 0xb0, 0 };
872 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::configure_dongle: Unable to send set-streaming-mode command\n");
889 unsigned char set_id[6] = { 0xd1, 0, 0,0,0,0 };
890 unsigned char *bufptr = &set_id[1];
891 vrpn_int32 buflen = 5;
895 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::set_logical_id: Unable to send set-logical-id command\n");
899 printf(
"... Logical id %d set to serial_number %x\n", logical_id, serial_number);
916 const unsigned char START_OF_PACKET = 0xF7;
922 for (
int i = 0; i < p_len; i++) {
923 checksum += p_cmd[i];
928 unsigned char buffer[256];
929 buffer[0] = START_OF_PACKET;
930 memcpy(&(buffer[1]), p_cmd, p_len);
931 buffer[p_len + 1] = static_cast<unsigned char>(checksum);
937 if (l_ret == p_len+2) {
964 const unsigned char START_OF_PACKET = 0xF8;
967 unsigned char buffer[256];
968 buffer[0] = START_OF_PACKET;
970 memcpy(&(buffer[2]), p_cmd, p_len);
977 for (
int i = 0; i < p_len+1; i++) {
978 checksum += buffer[1 + i];
981 buffer[p_len + 2] = static_cast<unsigned char>(checksum);
986 if (l_ret != p_len+3) {
987 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Could not send command\n");
991 printf(
"... packet of length %d sent\n", l_ret);
999 struct timeval timeout;
1001 timeout.tv_usec = 500000;
1004 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Error (%d) from ID %d\n", buffer[0], buffer[1]);
1006 }
else if (ret != 3) {
1007 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Timeout waiting for command status (got %d chars)\n", ret);
1010 if (buffer[0] != 0) {
1011 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Command failed\n");
1015 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Got response for incorrect logical ID\n");
1019 printf(
"..... send succeded\n");
1045 if (strlen(p_cmd) == 0) {
1056 int buflen = static_cast<int>(strlen(buffer) + 1);
1057 unsigned char *bufptr = static_cast<unsigned char *>(
1058 static_cast<void*>(buffer));
1062 if (l_ret != buflen) {
1063 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Error sending command\n");
1072 struct timeval timeout;
1074 timeout.tv_usec = 500000;
1077 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Timeout waiting for command status\n");
1081 if (buffer[0] !=
'0') {
1082 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Command failed: response (%s)\n", buffer);
1085 if (buffer[strlen(buffer)-1] !=
'\n') {
1086 fprintf(stderr,
"vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Got ill-formatted response: (%s).\n", buffer);
1106 unsigned char value;
1108 if (ret !=
sizeof(value)) {
1128 unsigned char buffer[3*
sizeof(vrpn_float32)];
1130 if (ret !=
sizeof(buffer)) {
1134 unsigned char *bufptr = buffer;
1162 unsigned char release_report[2] = { 0xB4, 0 };
1165 VRPN_MSG_ERROR (
"vrpn_YEI_3Space::get_report: Unable to send release-report command\n");
1171 struct timeval timeout;
1173 timeout.tv_usec = 1000;
1178 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::get_report(): Error reading the sensor, resetting");
1183 if (l_ret != 0) printf(
"... got %d characters\n",l_ret);
1192 if (l_ret != REPORT_LENGTH + 3) {
1193 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::get_report(): Truncated report, resetting");
1198 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::get_report(): Error reported, resetting");
1203 VRPN_MSG_ERROR (
"vrpn_YEI_3Space_Sensor_Wireless::get_report(): Report from wrong sensor received, resetting");
double d_frames_per_second
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...
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
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...
int vrpn_close_commport(int comm)
virtual bool configure_dongle(void)
Configure the dongle (called if we are the first one)
void vrpn_SleepMsecs(double dMsecs)
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.
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)
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...
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
virtual bool set_logical_id(vrpn_uint8 logical_id, vrpn_int32 serial_number)
Insert our serial number into the specified slot.
virtual void flush_input(void)=0
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....
Header containing macros formerly duplicated in a lot of implementation files.
unsigned d_expected_characters
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.
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 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...
vrpn_float64 channel[vrpn_CHANNEL_MAX]
int d_reset_command_count
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 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.
Base class with routines for YEI 3Space units.
unsigned char d_buffer[128]
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...
Generic connection class not specific to the transport mechanism.
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 ~vrpn_YEI_3Space_Sensor()
Destructor.
vrpn_float32 d_LED_color[3]
virtual bool get_report(void)=0
vrpn_YEI_3Space(const char *name, vrpn_Connection *c, 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 get_report(void)
Get and handle a report from the device if one is available. Return true if one was available,...
#define STATUS_NOT_INITIALIZED
virtual int init(void)
Initialize things that the constructor can't. Returns 0 on success, -1 on failure.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
unsigned d_characters_read
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,...
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....
vrpn_YEI_3Space_Sensor_Wireless(const char *name, vrpn_Connection *c, int logical_id, int serial_number, 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 handle_report(unsigned char *report)
virtual void mainloop()
Called once through each main loop iteration to handle updates.
virtual ~vrpn_YEI_3Space_Sensor_Wireless()
Destructor.
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...
vrpn_int32 getNumChannels(void) const
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
virtual void flush_input(void)
Flush any incoming characters in the communications channel.
#define vrpn_gettimeofday
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.
#define MAX_TIME_INTERVAL
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,...
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 long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
virtual ~vrpn_YEI_3Space()
Destructor.
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 void flush_input(void)
Flush any incoming characters in the communications channel.
#define VRPN_MSG_ERROR(msg)
virtual bool get_report(void)