vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
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
19const 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
29vrpn_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
67static 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
91static 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;
334 _bufcount = 0;
335
336 return 1;
337}
338
343int 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
430void vrpn_Nikon_Controls::report_changes(vrpn_uint32 class_of_service)
431{
433
434 vrpn_Analog::report_changes(class_of_service);
435}
436
437void 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
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}
vrpn_float64 o_channel[vrpn_CHANNEL_MAX]
vrpn_int32 request_channels_m_id
struct timeval o_timestamp
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
struct timeval timestamp
Definition: vrpn_Analog.h:41
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
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
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
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.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
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_int32 d_ping_message_id
Ask the server if they are there.
Generic connection class not specific to the transport mechanism.
struct timeval last_poll
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.
unsigned char _buffer[512]
vrpn_Nikon_Controls(const char *device_name, vrpn_Connection *con=NULL, const char *port_name="COM1")
virtual int set_channel(unsigned chan_num, vrpn_float64 value)
Set the channel associated with the index to the specified value.
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
Send values whether or not they have changed.
virtual void mainloop()
This routine is called each time through the server's main loop.
virtual int reset(void)
struct timeval timestamp
static int VRPN_CALLBACK handle_connect_message(void *userdata, vrpn_HANDLERPARAM p)
Responds to a connection request with a report of the values.
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_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
Send changes since the last time.
virtual int get_report(void)
This structure is what is passed to a vrpn_Connection message callback.
const char * buffer
#define STATUS_SYNCING
#define STATUS_READING
#define STATUS_RESETTING
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
@ vrpn_TEXT_ERROR
#define TIMEOUT_TIME_INTERVAL
const vrpn_uint32 vrpn_CONNECTION_RELIABLE
Classes of service for messages, specify multiple by ORing them together Priority of satisfying these...
#define POLL_INTERVAL
Definition: vrpn_IDEA.C:26
Header containing macros formerly duplicated in a lot of implementation files.
#define VRPN_MSG_ERROR(msg)
#define VRPN_MSG_WARNING(msg)
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
Definition: vrpn_Serial.C:651
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:438
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:488
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:515
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
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:321
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
Definition: vrpn_Shared.C:138
void vrpn_SleepMsecs(double dMilliSecs)
Definition: vrpn_Shared.C:166
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:99
const char * VALID_REPORT_CHARS
Characters that can start a report of the type we recognize.