source: BAORadio/libindi/v1/drivers/focuser/robofocusdriver.c@ 504

Last change on this file since 504 was 490, checked in by campagne, 15 years ago

import libindi (JEC)
File size: 10.8 KB
Line 
1#if 0
2 Robofocus Driver
3 Copyright (C) 2006 Markus Wildi, markus.wildi@datacomm.ch
4
5 This library is free software; you can redistribute it and/or
6 modify it under the terms of the GNU Lesser General Public
7 License as published by the Free Software Foundation; either
8 version 2.1 of the License, or (at your option) any later version.
9
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 Lesser General Public License for more details.
14
15 You should have received a copy of the GNU Lesser General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
18
19#endif
20
21#include <stdio.h>
22#include <string.h>
23#include <unistd.h>
24
25#include "indicom.h"
26#include "indidevapi.h"
27#include "indiapi.h"
28#include "robofocusdriver.h"
29
30/**********************************************************************
31* Communication
32**********************************************************************/
33
34
35int portRFRead(int fd, char *buf, int nbytes, int timeout)
36{
37
38 int bytesRead = 0;
39 int totalBytesRead = 0;
40 int err;
41 char *tbuf= buf ;
42
43
44 while (nbytes > 0)
45 {
46 if ( (err = tty_timeout(fd, timeout)) < 0 )
47 return err;
48
49 bytesRead = read(fd, tbuf, nbytes);
50
51 if (bytesRead < 0 )
52 return -1;
53
54
55 if(( tbuf[0]== 'I')|| ( tbuf[0]== 'O')){
56
57 /*fprintf(stderr, "Moving\n") ;*/
58 tbuf[0]=0 ;
59 usleep(100000) ;
60 } else {
61 tbuf += bytesRead;
62 totalBytesRead++;
63 nbytes -= bytesRead;
64 }
65 }
66
67/* fprintf(stderr, "READ : (%s,%d), %d\n", buf, 9, totalBytesRead) ; */
68/* fprintf(stderr, "READ : ") ; */
69/* for(i=0; i < 9; i++) { */
70
71/* fprintf(stderr, "0x%2x ", (unsigned char)buf[i]) ; */
72/* } */
73/* fprintf(stderr, "\n") ; */
74 return 9;
75}
76
77
78int portRFWrite(int fd, const char * buf) {
79 int nbytes=9 ;
80 int totalBytesWritten=0 ;
81 int bytesWritten = 0;
82
83 while (nbytes > 0)
84 {
85
86 bytesWritten = write(fd, buf, nbytes);
87
88 if (bytesWritten < 0)
89 return -1;
90
91 buf += bytesWritten;
92 nbytes -= bytesWritten;
93 }
94
95 /* Returns the # of bytes written */
96 return (totalBytesWritten);
97}
98
99
100unsigned char calsum(char *rf_cmd) {
101
102 unsigned char val= 0 ;
103 int i=0 ;
104
105 for(i=0; i < 8; i++) {
106
107 val = val + (unsigned char) rf_cmd[i] ;
108 }
109
110 return val % 256 ;
111}
112
113
114unsigned char chksum(char *rf_cmd) {
115
116 char substr[255] ;
117 unsigned char val= 0 ;
118 int i= 0 ;
119
120 for(i=0; i < 8; i++) {
121
122 substr[i]=rf_cmd[i] ;
123 }
124
125 val = calsum( substr) ;
126 if( val== (unsigned char) rf_cmd[8]) {
127
128 } else {
129
130 fprintf(stderr, "Wrong (%s,%d), %x != %x\n", rf_cmd, strlen(rf_cmd), val, (unsigned char) rf_cmd[8]) ;
131 }
132
133 return val ;
134}
135
136/**********************************************************************
137* Commands
138**********************************************************************/
139
140
141int commRF(int fd, char *rf_cmd) {
142
143 int read_ret=0, check_ret=0;
144 char rf_cmd_cks[32] ;
145
146 unsigned char val= 0 ;
147
148 int nbytes= 9 ;
149 int i ;
150
151 val = calsum( rf_cmd ) ;
152 for(i=0; i < 8; i++) {
153
154 rf_cmd_cks[i]= rf_cmd[i] ;
155 }
156
157 rf_cmd_cks[8]= val ;
158 rf_cmd_cks[9]= 0 ;
159
160/* fprintf(stderr, "WRITE: ") ; */
161/* for(i=0; i < 9; i++) { */
162
163/* fprintf(stderr, "0x%2x ", rf_cmd_cks[i]) ; */
164/* } */
165/* fprintf(stderr, "\n") ; */
166
167 if(portRFWrite(fd, rf_cmd_cks) < 0)
168 return -1;
169
170 read_ret= portRFRead(fd, rf_cmd, nbytes, RF_TIMEOUT) ;
171
172 if (read_ret < 1)
173 return read_ret;
174
175 check_ret= chksum(rf_cmd) ;
176
177 rf_cmd[ read_ret- 1] = 0 ;
178 return 0;
179}
180
181int updateRFPosition(int fd, double *value) {
182
183 float temp ;
184 char rf_cmd[32] ;
185 int ret_read_tmp ;
186
187 strcpy(rf_cmd, "FG000000" ) ;
188
189 if ((ret_read_tmp= commRF(fd, rf_cmd)) < 0){
190
191 return ret_read_tmp;
192 }
193 if (sscanf(rf_cmd, "FD%6f", &temp) < 1){
194
195 return -1;
196 }
197
198 *value = (double) temp;
199
200 return 0;
201
202}
203
204int updateRFTemperature(int fd, double *value) {
205
206 float temp ;
207 char rf_cmd[32] ;
208 int ret_read_tmp ;
209
210 strcpy(rf_cmd, "FT000000" ) ;
211
212 if ((ret_read_tmp= commRF(fd, rf_cmd)) < 0)
213 return ret_read_tmp;
214
215
216 if (sscanf(rf_cmd, "FT%6f", &temp) < 1)
217 return -1;
218
219 *value = (double) temp/2.- 273.15;
220
221 return 0;
222}
223
224int updateRFBacklash(int fd, double *value) {
225
226 float temp ;
227 char rf_cmd[32] ;
228 char vl_tmp[4] ;
229 int ret_read_tmp ;
230 int sign= 0 ;
231
232 if(*value== BACKLASH_READOUT) {
233
234 strcpy(rf_cmd, "FB000000" ) ;
235
236 } else {
237
238 rf_cmd[0]= 'F' ;
239 rf_cmd[1]= 'B' ;
240
241 if( *value > 0) {
242
243 rf_cmd[2]= '3' ;
244
245 } else {
246 *value= - *value ;
247 rf_cmd[2]= '2' ;
248 }
249 rf_cmd[3]= '0' ;
250 rf_cmd[4]= '0' ;
251
252 if(*value > 99) {
253 sprintf( vl_tmp, "%3d", (int) *value) ;
254
255 } else if(*value > 9) {
256 sprintf( vl_tmp, "0%2d", (int) *value) ;
257 } else {
258 sprintf( vl_tmp, "00%1d", (int) *value) ;
259 }
260 rf_cmd[5]= vl_tmp[0] ;
261 rf_cmd[6]= vl_tmp[1] ;
262 rf_cmd[7]= vl_tmp[2] ;
263 }
264
265 if ((ret_read_tmp= commRF(fd, rf_cmd)) < 0)
266 return ret_read_tmp;
267
268
269 if (sscanf(rf_cmd, "FB%1d%5f", &sign, &temp) < 1)
270 return -1;
271
272 *value = (double) temp ;
273
274 if(( sign== 2) && ( *value > 0)) {
275 *value = - (*value) ;
276 }
277 return 0;
278}
279
280int updateRFFirmware(int fd, char *rf_cmd) {
281
282 int ret_read_tmp ;
283
284 strcpy(rf_cmd, "FV000000" ) ;
285
286 if ((ret_read_tmp= commRF(fd, rf_cmd)) < 0)
287 return ret_read_tmp;
288
289 return 0;
290}
291
292int updateRFMotorSettings(int fd, double *duty, double *delay, double *ticks) {
293
294 char rf_cmd[32] ;
295 int ret_read_tmp ;
296
297 if(( *duty== 0 ) && (*delay== 0) && (*ticks== 0) ){
298
299 strcpy(rf_cmd, "FC000000" ) ;
300
301 } else {
302
303 rf_cmd[0]= 'F' ;
304 rf_cmd[1]= 'C' ;
305 rf_cmd[2]= (char) *duty ;
306 rf_cmd[3]= (char) *delay ;
307 rf_cmd[4]= (char) *ticks ;
308 rf_cmd[5]= '0' ;
309 rf_cmd[6]= '0' ;
310 rf_cmd[7]= '0' ;
311 rf_cmd[8]= 0 ;
312
313 }
314
315 if ((ret_read_tmp= commRF(fd, rf_cmd)) < 0)
316 return ret_read_tmp;
317
318 *duty= (float) rf_cmd[2] ;
319 *delay= (float) rf_cmd[3] ;
320 *ticks= (float) rf_cmd[4] ;
321
322 return 0;
323}
324
325int updateRFPositionRelativeInward(int fd, double *value) {
326
327 char rf_cmd[32] ;
328 int ret_read_tmp ;
329 float temp ;
330 rf_cmd[0]= 0 ;
331
332 if(*value > 9999) {
333 sprintf( rf_cmd, "FI0%5d", (int) *value) ;
334 } else if(*value > 999) {
335 sprintf( rf_cmd, "FI00%4d", (int) *value) ;
336 } else if(*value > 99) {
337 sprintf( rf_cmd, "FI000%3d", (int) *value) ;
338 } else if(*value > 9) {
339 sprintf( rf_cmd, "FI0000%2d", (int) *value) ;
340 } else {
341 sprintf( rf_cmd, "FI00000%1d", (int) *value) ;
342 }
343
344 if ((ret_read_tmp= commRF(fd, rf_cmd)) < 0)
345 return ret_read_tmp;
346
347 if (sscanf(rf_cmd, "FD0%5f", &temp) < 1)
348 return -1;
349
350 *value = (double) temp ;
351
352 return 0;
353}
354
355int updateRFPositionRelativeOutward(int fd, double *value) {
356
357 char rf_cmd[32] ;
358 int ret_read_tmp ;
359 float temp ;
360
361 rf_cmd[0]= 0 ;
362
363 if(*value > 9999) {
364 sprintf( rf_cmd, "FO0%5d", (int) *value) ;
365 } else if(*value > 999) {
366 sprintf( rf_cmd, "FO00%4d", (int) *value) ;
367 } else if(*value > 99) {
368 sprintf( rf_cmd, "FO000%3d", (int) *value) ;
369 } else if(*value > 9) {
370 sprintf( rf_cmd, "FO0000%2d", (int) *value) ;
371 } else {
372 sprintf( rf_cmd, "FO00000%1d", (int) *value) ;
373 }
374
375 if ((ret_read_tmp= commRF(fd, rf_cmd)) < 0)
376 return ret_read_tmp;
377
378 if (sscanf(rf_cmd, "FD0%5f", &temp) < 1)
379 return -1;
380
381 *value = (double) temp ;
382
383 return 0;
384}
385
386int updateRFPositionAbsolute(int fd, double *value) {
387
388 char rf_cmd[32] ;
389 int ret_read_tmp ;
390 float temp ;
391
392 rf_cmd[0]= 0 ;
393
394 if(*value > 9999) {
395 sprintf( rf_cmd, "FG0%5d", (int) *value) ;
396 } else if(*value > 999) {
397 sprintf( rf_cmd, "FG00%4d", (int) *value) ;
398 } else if(*value > 99) {
399 sprintf( rf_cmd, "FG000%3d", (int) *value) ;
400 } else if(*value > 9) {
401 sprintf( rf_cmd, "FG0000%2d", (int) *value) ;
402 } else {
403 sprintf( rf_cmd, "FG00000%1d", (int) *value) ;
404 }
405
406 if ((ret_read_tmp= commRF(fd, rf_cmd)) < 0)
407 return ret_read_tmp;
408
409 if (sscanf(rf_cmd, "FD0%5f", &temp) < 1)
410 return -1;
411
412 *value = (double) temp ;
413
414 return 0;
415}
416int updateRFPowerSwitches(int fd, int s, int new_sn, int *cur_s1LL, int *cur_s2LR, int *cur_s3RL, int *cur_s4RR) {
417
418 char rf_cmd[32] ;
419 char rf_cmd_tmp[32] ;
420 int ret_read_tmp ;
421 int i = 0 ;
422
423
424 /* Get first the status */
425 strcpy(rf_cmd_tmp, "FP000000" ) ;
426
427 if ((ret_read_tmp= commRF(fd, rf_cmd_tmp)) < 0)
428 return ret_read_tmp ;
429
430 for(i= 0; i < 9; i++) {
431 rf_cmd[i]= rf_cmd_tmp[i] ;
432 }
433
434
435 if( rf_cmd[new_sn + 4]== '2') {
436 rf_cmd[new_sn + 4]= '1' ;
437 } else {
438 rf_cmd[new_sn + 4]= '2' ;
439 }
440
441
442 /* if( s== ISS_ON) { */
443
444/* rf_cmd[new_sn + 4]= '2' ; */
445/* } else { */
446/* rf_cmd[new_sn + 4]= '1' ; */
447/* } */
448
449 rf_cmd[8]= 0 ;
450
451 if ((ret_read_tmp= commRF(fd, rf_cmd)) < 0)
452 return ret_read_tmp ;
453
454
455 *cur_s1LL= *cur_s2LR= *cur_s3RL= *cur_s4RR= ISS_OFF ;
456
457 if(rf_cmd[4]== '2' ) {
458 *cur_s1LL= ISS_ON ;
459 }
460
461 if(rf_cmd[5]== '2' ) {
462 *cur_s2LR= ISS_ON ;
463 }
464
465 if(rf_cmd[6]== '2' ) {
466 *cur_s3RL= ISS_ON ;
467 }
468
469 if(rf_cmd[7]== '2' ) {
470 *cur_s4RR= ISS_ON ;
471 }
472 return 0 ;
473}
474
475
476int updateRFMaxPosition(int fd, double *value) {
477
478 float temp ;
479 char rf_cmd[32] ;
480 char vl_tmp[6] ;
481 int ret_read_tmp ;
482 char waste[1] ;
483
484 if(*value== MAXTRAVEL_READOUT) {
485
486 strcpy(rf_cmd, "FL000000" ) ;
487
488 } else {
489
490 rf_cmd[0]= 'F' ;
491 rf_cmd[1]= 'L' ;
492 rf_cmd[2]= '0' ;
493
494 if(*value > 9999) {
495 sprintf( vl_tmp, "%5d", (int) *value) ;
496
497 } else if(*value > 999) {
498
499 sprintf( vl_tmp, "0%4d", (int) *value) ;
500
501 } else if(*value > 99) {
502 sprintf( vl_tmp, "00%3d", (int) *value) ;
503
504 } else if(*value > 9) {
505 sprintf( vl_tmp, "000%2d", (int) *value) ;
506 } else {
507 sprintf( vl_tmp, "0000%1d", (int) *value) ;
508 }
509 rf_cmd[3]= vl_tmp[0] ;
510 rf_cmd[4]= vl_tmp[1] ;
511 rf_cmd[5]= vl_tmp[2] ;
512 rf_cmd[6]= vl_tmp[3] ;
513 rf_cmd[7]= vl_tmp[4] ;
514 rf_cmd[8]= 0 ;
515 }
516
517 if ((ret_read_tmp= commRF(fd, rf_cmd)) < 0)
518 return ret_read_tmp;
519
520
521 if (sscanf(rf_cmd, "FL%1c%5f", waste, &temp) < 1)
522 return -1;
523
524 *value = (double) temp ;
525
526 return 0;
527}
528
529int updateRFSetPosition(int fd, double *value) {
530
531 char rf_cmd[32] ;
532 char vl_tmp[6] ;
533 int ret_read_tmp ;
534
535 rf_cmd[0]= 'F' ;
536 rf_cmd[1]= 'S' ;
537 rf_cmd[2]= '0' ;
538
539 if(*value > 9999) {
540 sprintf( vl_tmp, "%5d", (int) *value) ;
541 } else if(*value > 999) {
542 sprintf( vl_tmp, "0%4d", (int) *value) ;
543 } else if(*value > 99) {
544 sprintf( vl_tmp, "00%3d", (int) *value) ;
545 } else if(*value > 9) {
546 sprintf( vl_tmp, "000%2d", (int) *value) ;
547 } else {
548 sprintf( vl_tmp, "0000%1d", (int) *value) ;
549 }
550 rf_cmd[3]= vl_tmp[0] ;
551 rf_cmd[4]= vl_tmp[1] ;
552 rf_cmd[5]= vl_tmp[2] ;
553 rf_cmd[6]= vl_tmp[3] ;
554 rf_cmd[7]= vl_tmp[4] ;
555 rf_cmd[8]= 0 ;
556
557 if ((ret_read_tmp= commRF(fd, rf_cmd)) < 0)
558 return ret_read_tmp;
559
560 return 0;
561}
Note: See TracBrowser for help on using the repository browser.