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.
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)
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.
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
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.
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
void vrpn_SleepMsecs(double dMilliSecs)
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99
const char * VALID_REPORT_CHARS
Characters that can start a report of the type we recognize.