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

Last change on this file since 614 was 490, checked in by campagne, 14 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.