vrpn  07.33
Virtual Reality Peripheral Network
vrpn_nikon_controls.C
Go to the documentation of this file.
1 //XXXX Change this to use relative motion commands but still use
2 // absolute position requests. This will both reduce the number
3 // of characters to send (and time) and avoid the problem of the
4 // device being reset at an out-of-range place.
5 
6 #include <stdio.h> // for fprintf, stderr, sprintf, etc
7 #include <stdlib.h> // for atoi
8 #include <string.h> // for strlen, NULL, strncmp, etc
9 
10 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR, etc
11 #include "vrpn_Serial.h"
12 #include "vrpn_nikon_controls.h"
13 #include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
14 
15 //#define VERBOSE
16 //#define VERBOSE2
17 
19 const char *VALID_REPORT_CHARS = "anqo";
20 
21 // Defines the modes in which the device can find itself.
22 #define STATUS_RESETTING (-1) // Resetting the device
23 #define STATUS_SYNCING (0) // Looking for the first character of report
24 #define STATUS_READING (1) // Looking for the rest of the report
25 
26 #define TIMEOUT_TIME_INTERVAL (10000000L) // max time between reports (usec)
27 #define POLL_INTERVAL (1000000L) // time to poll if no response in a while (usec)
28 
29 vrpn_Nikon_Controls::vrpn_Nikon_Controls(const char *device_name, vrpn_Connection *con, const char *port_name)
30 : vrpn_Serial_Analog(device_name, con, port_name),
31  vrpn_Analog_Output(device_name, con)
32 {
33  num_channel = 1; // Focus control
34  o_num_channel = 1; // Focus control
35 
36  last_poll.tv_sec = 0;
37  last_poll.tv_usec = 0;
38 
39  // Set the mode to reset
41 
42  // Register to receive the message to request changes and to receive connection
43  // messages.
44  if (d_connection != NULL) {
46  this, d_sender_id)) {
47  fprintf(stderr,"vrpn_Nikon_Controls: can't register handler\n");
48  d_connection = NULL;
49  }
51  this, d_sender_id)) {
52  fprintf(stderr,"vrpn_Nikon_Controls: can't register handler\n");
53  d_connection = NULL;
54  }
56  this, d_sender_id)) {
57  fprintf(stderr,"vrpn_Nikon_Controls: can't register handler\n");
58  d_connection = NULL;
59  }
60  } else {
61  fprintf(stderr,"vrpn_Nikon_Controls: Can't get connection!\n");
62  }
63 }
64 
67 static const char *error_code_to_string(const int code)
68 {
69  switch (code) {
70  case 1: return "Command error";
71  case 2: return "Operand error";
72  case 3: return "Procedding timout error";
73  case 4: return "Receive buffer overflow error";
74  case 5: return "Device not mounted";
75  case 6: return "Drive count overflow error";
76  case 7: return "Processing prohibited error";
77  default: return "unknown error";
78  }
79 }
80 
90 
91 static int parse_focus_position_response(const char *inbuf, double &response_pos)
92 {
93  // Make sure that the command ends with [CR][LF]. All valid reports should end
94  // with this.
95  size_t len = strlen((const char *)inbuf);
96  if (len < 2) {
97  fprintf(stderr,"parse_focus_position_response(): String too short\n");
98  response_pos = 0; return 0;
99  }
100  if ( (inbuf[len-2] != '\r') || (inbuf[len-1] != '\n') ) {
101  fprintf(stderr,"parse_focus_position_response(): (%s) Not [CR][LF] at end\n",inbuf);
102  response_pos = 0; return 0;
103  }
104 
105  //-------------------------------------------------------------------------
106  // Parse the report based on the first part of the report.
107 
108 #ifdef VERBOSE
109  printf("Nikon control: parsing [%s]\n", inbuf);
110 #endif
111  // Reported focus
112  if (strncmp(inbuf, "aSPR", strlen("aSPR")) == 0) {
113  response_pos = atoi(&inbuf[4]);
114  return 1;
115 
116  // Error message
117  } else if (strncmp(inbuf, "nSPR", strlen("nSPR")) == 0) {
118  response_pos = atoi(&inbuf[4]);
119  return -1;
120 
121  // In the process of moving focus where it was requested
122  } else if (strncmp(inbuf, "qSMVA", strlen("qSMVA")) == 0) {
123  return 3;
124 
125  // Done moving focus where it was requested
126  } else if (strncmp(inbuf, "oSMV", strlen("oSMV")) == 0) {
127  return 2;
128 
129  // Error moving focus where it was requested
130  } else if (strncmp(inbuf, "nSMV", strlen("nSMV")) == 0) {
131  response_pos = atoi(&inbuf[4]);
132  return -1;
133 
134  // Don't know how to parse the command.
135  } else {
136  fprintf(stderr,"parse_focus_position_response(): Unknown response (%s)\n", inbuf);
137  response_pos = 0; return 0;
138  }
139 }
140 
141 // This function will flush any characters in the input buffer, then ask the
142 // Nikon for the state of its focus control. If it gets a valid response, all is
143 // well; otherwise, not.
144 // Commands Responses Meanings
145 // rSPR[CR] aSPR[value][CR][LF] Request focus: value tells the focus in number of ticks
146 // nSPR[errcode][CR][LF] errcode tells what went wrong
147 // fSMV[value][CR] qSMVA[CR][LF] Set focus to value: q command says in progress
148 // oSMV[CR][LF] o says that the requested position has been obtained
149 
151 {
152  unsigned char inbuf[256];
153  int ret;
154  char errmsg[256];
155  char cmd[256];
156  double response_pos; //< Where the focus says it is.
157 
158  //-----------------------------------------------------------------------
159  // Sleep a second and then drain the input buffer to make sure we start
160  // with a fresh slate.
161  vrpn_SleepMsecs(1000);
163 
164  //-----------------------------------------------------------------------
165  // Send the command to request the focus. Then wait 1 second for a response
166  sprintf(cmd, "rSPR\r");
167  if (vrpn_write_characters(serial_fd, (unsigned char *)cmd, strlen(cmd)) != (int)strlen(cmd)) {
168  fprintf(stderr,"vrpn_Nikon_Controls::reset(): Cannot send focus request\n");
169  return -1;
170  }
171  vrpn_SleepMsecs(1000);
172 
173  //-----------------------------------------------------------------------
174  // Read the response from the camera and then see if it is a good response,
175  // an error message, or nothing.
176 
177  ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf));
178  if (ret < 0) {
179  perror("vrpn_Nikon_Controls::reset(): Error reading position from device");
180  return -1;
181  }
182  if (ret == 0) {
183  fprintf(stderr, "vrpn_Nikon_Controls::reset(): No characters when reading position from device\n");
184  return -1;
185  }
186  inbuf[ret] = '\0'; //< Null-terminate the input string
187  ret = parse_focus_position_response((char *)inbuf, response_pos);
188  if (ret < 0) {
189  fprintf(stderr,"vrpn_Nikon_Controls::reset(): Error reading focus: %s\n",
190  error_code_to_string((int)(response_pos)));
191  return -1;
192  }
193  if (ret != 1) {
194  fprintf(stderr,"vrpn_Nikon_Controls::reset(): Unexpected response to focus request\n");
195  return -1;
196  }
197  channel[0] = response_pos;
198 
199  sprintf(errmsg,"Focus reported (this is good)");
200  VRPN_MSG_WARNING(errmsg);
201 
202  // We're now waiting for any responses from devices
204 
205  VRPN_MSG_WARNING("reset complete (this is good)");
206 
207  vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
208  return 0;
209 }
210 
211 
212 // This function will read characters until it has a full report, then
213 // put that report into analog field and call the report method on that.
214 // The time stored is that of the first character received as part of the
215 // report.
216 
218 {
219  int ret; // Return value from function call to be checked
220 
221  //--------------------------------------------------------------------
222  // If we're SYNCing, then the next character we get should be the start
223  // of a report. If we recognize it, go into READing mode and tell how
224  // many characters we expect total. If we don't recognize it, then we
225  // must have misinterpreted a command or something; reset
226  // and start over
227  //--------------------------------------------------------------------
228 
229  if (status == STATUS_SYNCING) {
230  // Try to get a character. If none, just return.
232  return 0;
233  }
234 
235  // See if we recognize the character as one of the ones making
236  // up a report. If not, then we need to continue syncing.
237  if (strchr(VALID_REPORT_CHARS, _buffer[0]) == NULL) {
238  VRPN_MSG_WARNING("Syncing");
239  return 0;
240  }
241 
242  // Got the first character of a report -- go into READING mode
243  // and record that we got one character at this time. The next
244  // bit of code will attempt to read the rest of the report.
245  // The time stored here is as close as possible to when the
246  // report was generated.
247  _bufcount = 1;
250 #ifdef VERBOSE2
251  printf("... Got the 1st char\n");
252 #endif
253  }
254 
255  //--------------------------------------------------------------------
256  // Read a character at a time from the serial port, storing them
257  // in the buffer. Do this until we either run out of characters to
258  // read or else find [CR][LF] along the way, which indicates the end
259  // of a command.
260  //--------------------------------------------------------------------
261 
262  do {
264  if (ret == -1) {
265  VRPN_MSG_ERROR("Error reading");
267  return 0;
268  }
269  _bufcount += ret;
270 #ifdef VERBOSE2
271  if (ret != 0) printf("... got %d characters (%d total)\n",ret, _bufcount);
272 #endif
273 
274  // See if the last characters in the buffer are [CR][LF]. If so, then
275  // we have a full report so null-terminate and go ahead and parse
276  if ( (_bufcount > 2) &&
277  (_buffer[_bufcount-2] == '\r') &&
278  (_buffer[_bufcount-1] == '\n') ) {
279  _buffer[_bufcount] = '\0';
280  break;
281  }
282 
283  // If we have a full buffer, then we've gotten into a bad way.
284  if (_bufcount >= sizeof(_buffer) - 1) {
285  VRPN_MSG_ERROR("Buffer full when reading");
287  return 0;
288  }
289  } while (ret != 0);
290  if (ret == 0) {
291  return 0;
292  }
293 
294  //--------------------------------------------------------------------
295  // We now have enough characters to make a full report. Check it by
296  // trying to parse it. If it is not valid, then we return to
297  // synch mode and ignore this report. A well-formed report has
298  // on of VALID_REPORT_CHARS as its command. We expect these responses
299  // either set the response position (due to a query) or to be valid
300  // but not set the response position (due to a position move request,
301  // for which we store the requested position into the value).
302  //--------------------------------------------------------------------
303 
304  double response_pos;
305  ret = parse_focus_position_response((const char *)_buffer, response_pos);
306  if (ret <= 0) {
307  fprintf(stderr,"Bad response from nikon [%s], syncing\n", _buffer);
308  fprintf(stderr," (%s)\n", error_code_to_string((int)response_pos));
310  return 0;
311  }
312  // Type 3 returns mean "in progress" for some function. Ignore this
313  // report.
314  if (ret == 3) {
316  _bufcount = 0;
317  return 1;
318  }
319 
320  // Type 2 means that the command we issued before has completed -- put
321  // the value we requested for the focus into the response since that is
322  // where we are now.
323  if (ret == 2) { // We must have gotten where we are going.
324  response_pos = _requested_focus;
325  }
326 
327  //--------------------------------------------------------------------
328  // Done with the decoding, send the reports and go back to syncing
329  //--------------------------------------------------------------------
330 
331  channel[0] = response_pos;
332  report_changes();
334  _bufcount = 0;
335 
336  return 1;
337 }
338 
343 int vrpn_Nikon_Controls::set_channel(unsigned chan_num, vrpn_float64 value)
344 {
345  // Ask to change the focus if this was channel 0. Also, store away where we
346  // asked the focus to go to so that we can know where we are when we get there
347  // (the response from the microscope just says "we got here!).
348  if ( (int)chan_num >= o_num_channel ) {
349  char msg[1024];
350  sprintf(msg,"vrpn_Nikon_Controls::set_channel(): Index out of bounds (%d of %d), value %lg\n",
351  chan_num, o_num_channel, value);
354  }
355 
356  char msg[256];
357  sprintf(msg, "fSMV%ld\r", (long)value);
358 #ifdef VERBOSE
359  printf("Nikon Control: Asking to move to %ld with command %s\n", (long)value, msg);
360 #endif
361  if (vrpn_write_characters(serial_fd, (unsigned char *)msg, strlen(msg)) != (int)strlen(msg)) {
362  fprintf(stderr, "vrpn_Nikon_Controls::set_channel(): Can't write command\n");
363  return -1;
364  }
366  fprintf(stderr, "vrpn_Nikon_Controls::set_channel(): Can't drain command\n");
367  return -1;
368  }
369  _requested_focus = value;
370 
371  return 0;
372 }
373 
374 // If the client requests a value for the focus control, then try to set the
375 // focus to the requested value.
377 {
378  const char *bufptr = p.buffer;
379  vrpn_int32 chan_num;
380  vrpn_int32 pad;
381  vrpn_float64 value;
383 
384  // Read the parameters from the buffer
385  vrpn_unbuffer(&bufptr, &chan_num);
386  vrpn_unbuffer(&bufptr, &pad);
387  vrpn_unbuffer(&bufptr, &value);
388 
389  // Set the position to the appropriate value, if the channel number is in the
390  me->set_channel(chan_num, value);
391  return 0;
392 }
393 
395 {
396  int i;
397  const char* bufptr = p.buffer;
398  vrpn_int32 num;
399  vrpn_int32 pad;
401 
402  // Read the values from the buffer
403  vrpn_unbuffer(&bufptr, &num);
404  vrpn_unbuffer(&bufptr, &pad);
405  if (num > me->o_num_channel) {
406  char msg[1024];
407  sprintf(msg,"vrpn_Nikon_Controls::handle_request_channels_message(): Index out of bounds (%d of %d), clipping\n",
408  num, me->o_num_channel);
410  num = me->o_num_channel;
411  }
412  for (i = 0; i < num; i++) {
413  vrpn_unbuffer(&bufptr, &(me->o_channel[i]));
414  me->set_channel(i, me->o_channel[i]);
415  }
416 
417  return 0;
418 }
419 
423 {
425 
427  return 0;
428 }
429 
430 void vrpn_Nikon_Controls::report_changes(vrpn_uint32 class_of_service)
431 {
433 
434  vrpn_Analog::report_changes(class_of_service);
435 }
436 
437 void vrpn_Nikon_Controls::report(vrpn_uint32 class_of_service)
438 {
440 
441  vrpn_Analog::report(class_of_service);
442 }
443 
444 
453 {
454  char errmsg[256];
455 
456  server_mainloop();
457 
458  switch(status) {
459  case STATUS_RESETTING:
460  reset();
461  break;
462 
463  case STATUS_SYNCING:
464  case STATUS_READING:
465  {
466  // It turns out to be important to get the report before checking
467  // to see if it has been too long since the last report. This is
468  // because there is the possibility that some other device running
469  // in the same server may have taken a long time on its last pass
470  // through mainloop(). Trackers that are resetting do this. When
471  // this happens, you can get an infinite loop -- where one tracker
472  // resets and causes the other to timeout, and then it returns the
473  // favor. By checking for the report here, we reset the timestamp
474  // if there is a report ready (ie, if THIS device is still operating).
475  while (get_report()) {}; // Keep getting reports so long as there are more
476 
477  struct timeval current_time;
478  vrpn_gettimeofday(&current_time, NULL);
479  if ( vrpn_TimevalDuration(current_time,timestamp) > POLL_INTERVAL) {
480 
482  // Ask the unit for its current focus location, which will cause it to respond.
483  char msg[256];
484  sprintf(msg, "rSPR\r");
485  if (vrpn_write_characters(serial_fd, (unsigned char *)msg, strlen(msg)) != (int)strlen(msg)) {
486  fprintf(stderr, "vrpn_Nikon_Controls::mainloop(): Can't write focus request command\n");
488  return;
489  }
491  } else {
492  return;
493  }
494  }
495 
497  sprintf(errmsg,"Timeout... current_time=%ld:%ld, timestamp=%ld:%ld",
498  current_time.tv_sec, static_cast<long>(current_time.tv_usec),
499  timestamp.tv_sec, static_cast<long>(timestamp.tv_usec));
500  VRPN_MSG_ERROR(errmsg);
502  }
503  }
504  break;
505 
506  default:
507  VRPN_MSG_ERROR("Unknown mode (internal error)");
508  break;
509  }
510 }
virtual void mainloop()
This routine is called each time through the server&#39;s main loop.
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.
Definition: vrpn_Serial.C:643
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...
Definition: vrpn_Analog.C:71
struct timeval o_timestamp
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
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.
Definition: vrpn_Shared.C:312
vrpn_int32 request_channels_m_id
#define STATUS_RESETTING
const vrpn_uint32 vrpn_CONNECTION_RELIABLE
Classes of service for messages, specify multiple by ORing them together Priority of satisfying these...
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
virtual int reset(void)
Header containing macros formerly duplicated in a lot of implementation files.
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
const char * buffer
vrpn_float64 o_channel[vrpn_CHANNEL_MAX]
static int VRPN_CALLBACK handle_connect_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a connection request with a report of the values.
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
vrpn_int32 d_ping_message_id
Ask the server if they are there.
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 ...
Definition: vrpn_Analog.C:94
#define VRPN_MSG_WARNING(msg)
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
Definition: vrpn_Serial.C:485
virtual int get_report(void)
virtual int set_channel(unsigned chan_num, vrpn_float64 value)
Set the channel associated with the index to the specified value.
struct timeval timestamp
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
static int VRPN_CALLBACK handle_request_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a request to change one of the values by setting the channel to that value.
int register_autodeleted_handler(vrpn_int32 type, vrpn_MESSAGEHANDLER handler, void *userdata, vrpn_int32 sender=vrpn_ANY_SENDER)
Registers a handler with the connection, and remembers to delete at destruction.
vrpn_Connection * d_connection
Connection that this object talks to.
This structure is what is passed to a vrpn_Connection message callback.
static int VRPN_CALLBACK handle_request_channels_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a request to change multiple channels at once.
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
Send values whether or not they have changed.
#define POLL_INTERVAL
unsigned char _buffer[512]
int send_text_message(const char *msg, struct timeval timestamp, vrpn_TEXT_SEVERITY type=vrpn_TEXT_NORMAL, vrpn_uint32 level=0)
Sends a NULL-terminated text message from the device d_sender_id.
vrpn_Nikon_Controls(const char *device_name, vrpn_Connection *con=NULL, const char *port_name="COM1")
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
#define STATUS_SYNCING
#define TIMEOUT_TIME_INTERVAL
struct timeval last_poll
vrpn_int32 d_sender_id
Sender ID registered with the connection.
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
Send changes since the last time.
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
Definition: vrpn_Shared.C:129
const char * VALID_REPORT_CHARS
Characters that can start a report of the type we recognize.
#define STATUS_READING
#define VRPN_MSG_ERROR(msg)
struct timeval timestamp
Definition: vrpn_Analog.h:41