source: BAORadio/libindi/libindi/drivers/focuser/robofocus.cpp @ 646

Last change on this file since 646 was 646, checked in by frichard, 12 years ago
File size: 40.7 KB
Line 
1/*
2    RoboFocus
3    Copyright (C) 2006 Markus Wildi (markus.wildi@datacomm.ch)
4                  2011 Jasem Mutlaq (mutlaqja@ikarustech.com)
5
6    This library is free software; you can redistribute it and/or
7    modify it under the terms of the GNU Lesser General Public
8    License as published by the Free Software Foundation; either
9    version 2.1 of the License, or (at your option) any later version.
10
11    This library is distributed in the hope that it will be useful,
12    but WITHOUT ANY WARRANTY; without even the implied warranty of
13    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
14    Lesser General Public License for more details.
15
16    You should have received a copy of the GNU Lesser General Public
17    License along with this library; if not, write to the Free Software
18    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
19
20*/
21
22#include "robofocus.h"
23#include "indicom.h"
24
25#include <termios.h>
26#include <string.h>
27#include <sys/time.h>
28
29#define RF_MAX_CMD   9
30#define RF_TIMEOUT   15
31#define RF_STEP_RES  5
32
33#define BACKLASH_READOUT 99999
34#define MAXTRAVEL_READOUT 99999
35
36#define currentSpeed            SpeedN[0].value
37#define currentPosition         PositionN[0].value
38#define currentTemperature      TemperatureN[0].value
39#define currentBacklash         SetBacklashN[0].value
40#define currentDuty             SettingsN[0].value
41#define currentDelay            SettingsN[1].value
42#define currentTicks            SettingsN[2].value
43#define currentRelativeMovement RelMovementN[0].value
44#define currentAbsoluteMovement AbsMovementN[0].value
45#define currentSetBacklash      SetBacklashN[0].value
46#define currentMinPosition      MinMaxPositionN[0].value
47#define currentMaxPosition      MinMaxPositionN[1].value
48#define currentMaxTravel        MaxTravelN[0].value
49
50std::auto_ptr<RoboFocus> roboFocus(0);
51
52 void ISInit()
53 {
54    static int isInit =0;
55
56    if (isInit == 1)
57        return;
58
59     isInit = 1;
60     if(roboFocus.get() == 0) roboFocus.reset(new RoboFocus());
61     //IEAddTimer(POLLMS, ISPoll, NULL);
62
63 }
64
65 void ISGetProperties(const char *dev)
66 {
67         ISInit();
68         roboFocus->ISGetProperties(dev);
69 }
70
71 void ISNewSwitch(const char *dev, const char *name, ISState *states, char *names[], int num)
72 {
73         ISInit();
74         roboFocus->ISNewSwitch(dev, name, states, names, num);
75 }
76
77 void ISNewText(        const char *dev, const char *name, char *texts[], char *names[], int num)
78 {
79         ISInit();
80         roboFocus->ISNewText(dev, name, texts, names, num);
81 }
82
83 void ISNewNumber(const char *dev, const char *name, double values[], char *names[], int num)
84 {
85         ISInit();
86         roboFocus->ISNewNumber(dev, name, values, names, num);
87 }
88
89 void ISNewBLOB (const char *dev, const char *name, int sizes[], int blobsizes[], char *blobs[], char *formats[], char *names[], int n)
90 {
91   INDI_UNUSED(dev);
92   INDI_UNUSED(name);
93   INDI_UNUSED(sizes);
94   INDI_UNUSED(blobsizes);
95   INDI_UNUSED(blobs);
96   INDI_UNUSED(formats);
97   INDI_UNUSED(names);
98   INDI_UNUSED(n);
99 }
100
101 void ISSnoopDevice (XMLEle *root)
102 {
103     ISInit();
104     roboFocus->ISSnoopDevice(root);
105 }
106
107
108RoboFocus::RoboFocus()
109{
110
111}
112
113RoboFocus::~RoboFocus()
114{
115
116}
117
118bool RoboFocus::initProperties()
119{
120    INDI::Focuser::initProperties();
121
122    // No speed for robofocus
123    FocusSpeedN[0].min = FocusSpeedN[0].max = FocusSpeedN[0].value = 1;
124
125    IUUpdateMinMax(FocusSpeedNP);
126
127    /* Port */
128    IUFillText(&PortT[0], "PORT", "Port", "/dev/ttyUSB0");
129    IUFillTextVector(&PortTP, PortT, 1, deviceName(), "DEVICE_PORT", "Ports", MAIN_CONTROL_TAB, IP_RW, 0, IPS_IDLE);
130
131
132    /* Focuser temperature */
133    IUFillNumber(&TemperatureN[0], "TEMPERATURE", "Celsius", "%6.2f", 0, 65000., 0., 10000.);
134    IUFillNumberVector(&TemperatureNP, TemperatureN, 1, deviceName(), "FOCUS_TEMPERATURE", "Temperature", MAIN_CONTROL_TAB, IP_RO, 0, IPS_IDLE);
135
136    /* Settings of the Robofocus */
137    IUFillNumber(&SettingsN[0], "Duty cycle", "Duty cycle", "%6.0f", 0., 255., 0., 1.0);
138    IUFillNumber(&SettingsN[1], "Step Delay", "Step delay", "%6.0f", 0., 255., 0., 1.0);
139    IUFillNumber(&SettingsN[2], "Motor Steps", "Motor steps per tick", "%6.0f", 0., 255., 0., 1.0);
140    IUFillNumberVector(&SettingsNP, SettingsN, 3, deviceName(), "FOCUS_SETTINGS", "Settings", OPTIONS_TAB, IP_RW, 0, IPS_IDLE);
141
142    /* Power Switches of the Robofocus */
143    IUFillSwitch(&PowerSwitchesS[0], "1", "Switch 1", ISS_OFF);
144    IUFillSwitch(&PowerSwitchesS[1], "2", "Switch 2", ISS_OFF);
145    IUFillSwitch(&PowerSwitchesS[2], "3", "Switch 3", ISS_OFF);
146    IUFillSwitch(&PowerSwitchesS[3], "4", "Switch 4", ISS_ON);
147    IUFillSwitchVector(&PowerSwitchesSP, PowerSwitchesS, 4, deviceName(), "SWTICHES", "Power", OPTIONS_TAB, IP_RW, ISR_1OFMANY, 0, IPS_IDLE);
148
149    /* Robofocus should stay within these limits */
150    IUFillNumber(&MinMaxPositionN[0], "MINPOS", "Minimum Tick", "%6.0f", 1., 65000., 0., 100. );
151    IUFillNumber(&MinMaxPositionN[1], "MAXPOS", "Maximum Tick", "%6.0f", 1., 65000., 0., 55000.);
152    IUFillNumberVector(&MinMaxPositionNP, MinMaxPositionN, 2, deviceName(), "FOCUS_MINMAXPOSITION", "Extrama", OPTIONS_TAB, IP_RW, 0, IPS_IDLE);
153
154    IUFillNumber(&MaxTravelN[0], "MAXTRAVEL", "Maximum travel", "%6.0f", 1., 64000., 0., 10000.);
155    IUFillNumberVector(&MaxTravelNP, MaxTravelN, 1, deviceName(), "FOCUS_MAXTRAVEL", "Max. travel", OPTIONS_TAB, IP_RW, 0, IPS_IDLE );
156
157    /* Set Robofocus position register to the this position */
158    IUFillNumber(&SetRegisterPositionN[0], "SETPOS", "Position", "%6.0f", 0, 64000., 0., 0. );
159    IUFillNumberVector(&SetRegisterPositionNP, SetRegisterPositionN, 1, deviceName(), "FOCUS_REGISTERPOSITION", "Set register", OPTIONS_TAB, IP_RW, 0, IPS_IDLE);
160
161    /* Backlash */
162    IUFillNumber(&SetBacklashN[0], "SETBACKLASH", "Backlash", "%6.0f", -255., 255., 0., 0.);
163    IUFillNumberVector(&SetBacklashNP, SetBacklashN, 1, deviceName(), "FOCUS_BACKLASH", "Set Register", OPTIONS_TAB, IP_RW, 0, IPS_IDLE);
164
165    /* Robofocus` position (RO) */
166    IUFillNumber(&PositionN[0], "POSITION", "Tick", "%6.0f", 0, 65000., 0., 10000.);
167    IUFillNumberVector(&PositionNP, PositionN, 1, deviceName(), "FOCUS_POSITION", "Position", MAIN_CONTROL_TAB, IP_RO, 0, IPS_IDLE);
168
169
170    /* Relative and absolute movement */
171    IUFillNumber(&RelMovementN[0], "RELMOVEMENT", "Ticks", "%6.0f", -65000., 65000., 0., 100. );
172    IUFillNumberVector(&RelMovementNP, RelMovementN, 1, deviceName(), "FOCUS_RELMOVEMENT", "Relative GoTo", MAIN_CONTROL_TAB, IP_RW, 0, IPS_IDLE);
173
174    IUFillNumber(&AbsMovementN[0], "ABSMOVEMENT", "Tick", "%6.0f", 0, 65000., 0., 10000.);
175    IUFillNumberVector(&AbsMovementNP, AbsMovementN, 1, deviceName(), "FOCUS_ABSMOVEMENT", "Absolute GoTo", MAIN_CONTROL_TAB, IP_RW, 0, IPS_IDLE);
176
177
178    addDebugControl();
179
180    return true;
181
182}
183
184void RoboFocus::ISGetProperties(const char *dev)
185{
186    INDI::Focuser::ISGetProperties(dev);
187
188    defineText(&PortTP);
189
190
191}
192
193bool RoboFocus::updateProperties()
194{
195    INDI::Focuser::updateProperties();
196
197    if (isConnected())
198    {
199        defineNumber(&TemperatureNP);
200        defineSwitch(&PowerSwitchesSP);
201        defineNumber(&SettingsNP);
202        defineNumber(&MinMaxPositionNP);
203        defineNumber(&MaxTravelNP);
204        defineNumber(&SetRegisterPositionNP);
205        defineNumber(&SetBacklashNP);
206        defineNumber(&PositionNP);
207        defineNumber(&RelMovementNP);
208        defineNumber(&AbsMovementNP);
209    }
210    else
211    {
212
213        deleteProperty(TemperatureNP.name);
214        deleteProperty(SettingsNP.name);
215        deleteProperty(PowerSwitchesSP.name);
216        deleteProperty(MinMaxPositionNP.name);
217        deleteProperty(MaxTravelNP.name);
218        deleteProperty(SetRegisterPositionNP.name);
219        deleteProperty(SetBacklashNP.name);
220        deleteProperty(PositionNP.name);
221        deleteProperty(RelMovementNP.name);
222        deleteProperty(AbsMovementNP.name);
223
224    }
225
226    return true;
227
228}
229
230bool RoboFocus::Connect()
231{
232    int connectrc=0;
233    char errorMsg[MAXRBUF];
234    char firmeware[]="FV0000000";
235
236
237    if (isDebug())
238        IDLog("connecting to %s\n",PortT[0].text);
239
240    if ( (connectrc = tty_connect(PortT[0].text, 9600, 8, 0, 1, &PortFD)) != TTY_OK)
241    {
242        tty_error_msg(connectrc, errorMsg, MAXRBUF);
243
244        if (isDebug())
245            IDLog("Failed to connect o port %s. Error: %s", PortT[0].text, errorMsg);
246
247        IDMessage(deviceName(), "Failed to connect to port %s. Error: %s", PortT[0].text, errorMsg);
248
249        return false;
250
251    }
252
253    if((updateRFFirmware(firmeware)) < 0)
254    {
255      /* This would be the end*/
256        IDMessage( deviceName(), "Unknown error while reading Robofocus firmware\n");
257        return false;
258    }
259
260    IDMessage(deviceName(), "Robofocus is online. Getting focus parameters...");
261
262    GetFocusParams();
263
264    IDMessage(deviceName(), "RoboFocus paramaters readout complete, focuser ready for use.");
265
266    return true;
267}
268
269bool RoboFocus::Disconnect()
270{
271    tty_disconnect(PortFD);
272    IDMessage(deviceName(), "RoboFocus is offline.");
273    return true;
274}
275
276const char * RoboFocus::getDefaultName()
277{
278    return "RoboFocus";
279}
280
281unsigned char RoboFocus::CheckSum(char *rf_cmd)
282{
283
284  char substr[255] ;
285  unsigned char val= 0 ;
286  int i= 0 ;
287
288  for(i=0; i < 8; i++) {
289
290    substr[i]=rf_cmd[i] ;
291  }
292
293  val = CalculateSum( substr) ;
294  if( val==  (unsigned char) rf_cmd[8])
295  {
296
297  } else {
298
299      if (isDebug())
300          fprintf(stderr,  "Wrong (%s,%d), %x != %x\n",  rf_cmd, strlen(rf_cmd), val, (unsigned char) rf_cmd[8]) ;
301  }
302
303  return val ;
304}
305
306unsigned char RoboFocus::CalculateSum(char *rf_cmd)
307{
308
309  unsigned char val= 0 ;
310  int i=0 ;
311
312  for(i=0; i < 8; i++) {
313
314    val = val + (unsigned char) rf_cmd[i] ;
315  }
316
317  return val % 256 ;
318}
319
320int RoboFocus::SendCommand(char *rf_cmd)
321{
322
323  int nbytes_written=0, nbytes_read=0, check_ret=0, err_code=0;
324  char rf_cmd_cks[32],robofocus_error[MAXRBUF];
325
326  unsigned char val= 0 ;
327
328  val = CalculateSum( rf_cmd );
329
330  for(int i=0; i < 8; i++)
331  {
332
333    rf_cmd_cks[i]= rf_cmd[i] ;
334  }
335
336  rf_cmd_cks[8]=  (unsigned char) val ;
337  rf_cmd_cks[9]= 0 ;
338
339  if (isDebug())
340  {
341   fprintf(stderr, "WRITE: ") ;
342   for(int i=0; i < 9; i++)
343   {
344
345     fprintf(stderr, "0x%2x ", (unsigned char) rf_cmd_cks[i]) ;
346   }
347   fprintf(stderr, "\n") ;
348  }
349
350
351  tcflush(PortFD, TCIOFLUSH);
352
353   if  ( (err_code = tty_write(PortFD, rf_cmd_cks, RF_MAX_CMD, &nbytes_written) != TTY_OK))
354   {
355        tty_error_msg(err_code, robofocus_error, MAXRBUF);
356        if (isDebug())
357            IDLog("TTY error detected: %s\n", robofocus_error);
358        return -1;
359   }
360
361   nbytes_read= ReadResponse(rf_cmd, RF_MAX_CMD, RF_TIMEOUT) ;
362
363   if (nbytes_read < 1)
364     return nbytes_read;
365
366   check_ret= CheckSum(rf_cmd) ;
367
368   rf_cmd[ nbytes_read - 1] = 0 ;
369   return 0;
370
371}
372
373int RoboFocus::ReadResponse(char *buf, int nbytes, int timeout)
374{
375
376  char robofocus_error[MAXRBUF];
377  int bytesRead = 0;
378  int totalBytesRead = 0;
379  int err_code;
380
381
382  while (totalBytesRead < nbytes)
383  {
384      //IDLog("Nbytes: %d\n", nbytes);
385
386      if ( (err_code = tty_read(PortFD, buf+totalBytesRead, nbytes-totalBytesRead, timeout, &bytesRead)) != TTY_OK)
387      {
388              tty_error_msg(err_code, robofocus_error, MAXRBUF);
389              if (isDebug())
390              {
391                  IDLog("TTY error detected: %s\n", robofocus_error);
392                  IDMessage(deviceName(), "TTY error detected: %s\n", robofocus_error);
393              }
394              return false;
395      }
396
397      //IDLog("Bytes Read: %d\n", bytesRead);
398
399      if (bytesRead < 0 )
400      {
401          //IDLog("Bytesread < 1\n");
402          return -1;
403      }
404
405      if((  buf[0]== 'I')||  (  buf[0]== 'O'))
406      {
407
408        //fprintf(stderr, "Moving\n") ;
409
410        for(int i=0; i < 9; i++)
411        {
412            // Catch the F in stream of I's and O's
413            // The robofocus reply might get mixed with the movement strings. For example:
414            // 0x49 0x49 0x49 0x49 0x49 0x46 0x44 0x30 0x32
415            // The 0x46 above is the START for the rofofocus response string, but it's lost unless we catch it here
416            if (buf[i] == 0x46)
417            {
418                totalBytesRead = RF_MAX_CMD - i;
419                strncpy(buf, buf+totalBytesRead+1, totalBytesRead);
420                break;
421            }
422        }
423
424       // buf[0]=0 ;
425         usleep(100000) ;
426
427
428      }
429      else
430      {
431        //buf += bytesRead;
432        totalBytesRead += bytesRead;
433        //nbytes -= bytesRead;
434      }
435    }
436
437    tcflush(PortFD, TCIOFLUSH);
438
439   if (isDebug())
440   {
441       fprintf(stderr, "READ : (%s,%d), %d\n", buf, 9, totalBytesRead) ;
442        fprintf(stderr, "READ : ") ;
443        for(int i=0; i < 9; i++)
444        {
445
446         fprintf(stderr, "0x%2x ", (unsigned char)buf[i]) ;
447        }
448        fprintf(stderr, "\n") ;
449   }
450
451  return 9;
452}
453
454int RoboFocus::updateRFPosition(double *value)
455{
456
457  float temp ;
458  char rf_cmd[32] ;
459  int ret_read_tmp ;
460
461  strcpy(rf_cmd, "FG000000" ) ;
462
463  if ((ret_read_tmp= SendCommand(  rf_cmd)) < 0){
464
465    return ret_read_tmp;
466  }
467   if (sscanf(rf_cmd, "FD%6f", &temp) < 1){
468
469    return -1;
470  }
471
472  *value = (double) temp;
473
474  return 0;
475
476}
477
478int RoboFocus::updateRFTemperature(double *value)
479{
480
481  float temp ;
482  char rf_cmd[32] ;
483  int ret_read_tmp ;
484
485  strcpy(rf_cmd, "FT000000" ) ;
486
487  if ((ret_read_tmp= SendCommand( rf_cmd)) < 0)
488    return ret_read_tmp;
489
490
491  if (sscanf(rf_cmd, "FT%6f", &temp) < 1)
492    return -1;
493
494  *value = (double) temp/2.- 273.15;
495
496  return 0;
497}
498
499int RoboFocus::updateRFBacklash(double *value)
500{
501
502  float temp ;
503  char rf_cmd[32] ;
504  char vl_tmp[4] ;
505  int ret_read_tmp ;
506  int sign= 0 ;
507
508  if(*value== BACKLASH_READOUT) {
509
510    strcpy(rf_cmd, "FB000000" ) ;
511
512  } else {
513
514    rf_cmd[0]=  'F' ;
515    rf_cmd[1]=  'B' ;
516
517    if( *value > 0) {
518
519      rf_cmd[2]= '3' ;
520
521    } else {
522      *value= - *value ;
523      rf_cmd[2]= '2' ;
524    }
525    rf_cmd[3]= '0' ;
526    rf_cmd[4]= '0' ;
527
528    if(*value > 99) {
529      sprintf( vl_tmp, "%3d", (int) *value) ;
530
531    } else if(*value > 9) {
532      sprintf( vl_tmp, "0%2d", (int) *value) ;
533    } else {
534      sprintf( vl_tmp, "00%1d", (int) *value) ;
535    }
536    rf_cmd[5]= vl_tmp[0] ;
537    rf_cmd[6]= vl_tmp[1] ;
538    rf_cmd[7]= vl_tmp[2] ;
539  }
540
541  if ((ret_read_tmp= SendCommand( rf_cmd)) < 0)
542    return ret_read_tmp;
543
544
545  if (sscanf(rf_cmd, "FB%1d%5f", &sign, &temp) < 1)
546    return -1;
547
548  *value = (double) temp  ;
549
550  if(( sign== 2) && ( *value > 0)) {
551    *value = - (*value) ;
552  }
553  return 0;
554}
555
556int RoboFocus::updateRFFirmware(char *rf_cmd)
557{
558
559  int ret_read_tmp ;
560
561  strcpy(rf_cmd, "FV000000" ) ;
562
563  if ((ret_read_tmp= SendCommand( rf_cmd)) < 0)
564    return ret_read_tmp;
565
566  return 0;
567}
568
569int RoboFocus::updateRFMotorSettings(double *duty, double *delay, double *ticks)
570{
571
572  char rf_cmd[32] ;
573  int ret_read_tmp ;
574
575  if(( *duty== 0 ) && (*delay== 0) && (*ticks== 0) ){
576
577    strcpy(rf_cmd, "FC000000" ) ;
578
579  } else {
580
581    rf_cmd[0]=  'F' ;
582    rf_cmd[1]=  'C' ;
583    rf_cmd[2]= (char) *duty ;
584    rf_cmd[3]= (char) *delay ;
585    rf_cmd[4]= (char) *ticks ;
586    rf_cmd[5]= '0' ;
587    rf_cmd[6]= '0' ;
588    rf_cmd[7]= '0' ;
589    rf_cmd[8]=  0 ;
590
591  }
592
593  if ((ret_read_tmp= SendCommand( rf_cmd)) < 0)
594    return ret_read_tmp;
595
596  *duty=  (float) rf_cmd[2] ;
597  *delay= (float) rf_cmd[3] ;
598  *ticks= (float) rf_cmd[4] ;
599
600  return 0;
601}
602
603int RoboFocus::updateRFPositionRelativeInward(double *value)
604{
605
606  char rf_cmd[32] ;
607  int ret_read_tmp ;
608  float temp ;
609  rf_cmd[0]= 0 ;
610
611  if(*value > 9999) {
612    sprintf( rf_cmd, "FI0%5d", (int) *value) ;
613  } else if(*value > 999) {
614    sprintf( rf_cmd, "FI00%4d", (int) *value) ;
615  } else if(*value > 99) {
616    sprintf( rf_cmd, "FI000%3d", (int) *value) ;
617  } else if(*value > 9) {
618    sprintf( rf_cmd, "FI0000%2d", (int) *value) ;
619  } else {
620    sprintf( rf_cmd, "FI00000%1d", (int) *value) ;
621  }
622
623  if ((ret_read_tmp= SendCommand( rf_cmd)) < 0)
624    return ret_read_tmp;
625
626  if (sscanf(rf_cmd, "FD0%5f", &temp) < 1)
627    return -1;
628
629  *value = (double) temp  ;
630
631  return 0;
632}
633
634int RoboFocus::updateRFPositionRelativeOutward(double *value)
635{
636
637  char rf_cmd[32] ;
638  int ret_read_tmp ;
639  float temp ;
640
641  rf_cmd[0]= 0 ;
642
643  if(*value > 9999) {
644    sprintf( rf_cmd, "FO0%5d", (int) *value) ;
645  } else if(*value > 999) {
646    sprintf( rf_cmd, "FO00%4d", (int) *value) ;
647  } else if(*value > 99) {
648    sprintf( rf_cmd, "FO000%3d", (int) *value) ;
649  } else if(*value > 9) {
650    sprintf( rf_cmd, "FO0000%2d", (int) *value) ;
651  } else {
652    sprintf( rf_cmd, "FO00000%1d", (int) *value) ;
653  }
654
655  if ((ret_read_tmp= SendCommand( rf_cmd)) < 0)
656    return ret_read_tmp;
657
658  if (sscanf(rf_cmd, "FD0%5f", &temp) < 1)
659    return -1;
660
661  *value = (double) temp  ;
662
663  return 0;
664}
665
666int RoboFocus::updateRFPositionAbsolute(double *value)
667{
668
669  char rf_cmd[32] ;
670  int ret_read_tmp ;
671  float temp ;
672
673  rf_cmd[0]= 0 ;
674
675  if(*value > 9999) {
676    sprintf( rf_cmd, "FG0%5d", (int) *value) ;
677  } else if(*value > 999) {
678    sprintf( rf_cmd, "FG00%4d", (int) *value) ;
679  } else if(*value > 99) {
680    sprintf( rf_cmd, "FG000%3d", (int) *value) ;
681  } else if(*value > 9) {
682    sprintf( rf_cmd, "FG0000%2d", (int) *value) ;
683  } else {
684    sprintf( rf_cmd, "FG00000%1d", (int) *value) ;
685  }
686
687  if ((ret_read_tmp= SendCommand( rf_cmd)) < 0)
688    return ret_read_tmp;
689
690  if (sscanf(rf_cmd, "FD0%5f", &temp) < 1)
691    return -1;
692
693  *value = (double) temp  ;
694
695  return 0;
696}
697
698int RoboFocus::updateRFPowerSwitches(int s, int  new_sn, int *cur_s1LL, int *cur_s2LR, int *cur_s3RL, int *cur_s4RR)
699{
700
701  char rf_cmd[32] ;
702  char rf_cmd_tmp[32] ;
703  int ret_read_tmp ;
704  int i = 0 ;
705
706
707    /* Get first the status */
708    strcpy(rf_cmd_tmp, "FP000000" ) ;
709
710    if ((ret_read_tmp= SendCommand( rf_cmd_tmp)) < 0)
711      return ret_read_tmp ;
712
713    for(i= 0; i < 9; i++) {
714      rf_cmd[i]= rf_cmd_tmp[i] ;
715    }
716
717
718    if( rf_cmd[new_sn + 4]== '2') {
719      rf_cmd[new_sn + 4]= '1' ;
720    } else {
721      rf_cmd[new_sn + 4]= '2' ;
722    }
723
724
725 /*    if( s== ISS_ON) { */
726
727/*       rf_cmd[new_sn + 4]= '2' ; */
728/*     } else { */
729/*       rf_cmd[new_sn + 4]= '1' ; */
730/*     } */
731
732    rf_cmd[8]= 0 ;
733
734  if ((ret_read_tmp= SendCommand( rf_cmd)) < 0)
735    return ret_read_tmp ;
736
737
738  *cur_s1LL= *cur_s2LR= *cur_s3RL= *cur_s4RR= ISS_OFF ;
739
740  if(rf_cmd[4]== '2' ) {
741    *cur_s1LL= ISS_ON ;
742  }
743
744  if(rf_cmd[5]== '2' ) {
745    *cur_s2LR= ISS_ON ;
746  }
747
748  if(rf_cmd[6]== '2' ) {
749    *cur_s3RL= ISS_ON ;
750  }
751
752  if(rf_cmd[7]== '2' ) {
753    *cur_s4RR= ISS_ON ;
754  }
755  return 0 ;
756}
757
758
759int RoboFocus::updateRFMaxPosition(double *value)
760{
761
762  float temp ;
763  char rf_cmd[32] ;
764  char vl_tmp[6] ;
765  int ret_read_tmp ;
766  char waste[1] ;
767
768  if(*value== MAXTRAVEL_READOUT) {
769
770    strcpy(rf_cmd, "FL000000" ) ;
771
772  } else {
773
774    rf_cmd[0]=  'F' ;
775    rf_cmd[1]=  'L' ;
776    rf_cmd[2]=  '0' ;
777
778    if(*value > 9999) {
779      sprintf( vl_tmp, "%5d", (int) *value) ;
780
781    } else if(*value > 999) {
782
783      sprintf( vl_tmp, "0%4d", (int) *value) ;
784
785    } else if(*value > 99) {
786      sprintf( vl_tmp, "00%3d", (int) *value) ;
787
788    } else if(*value > 9) {
789      sprintf( vl_tmp, "000%2d", (int) *value) ;
790    } else {
791      sprintf( vl_tmp, "0000%1d", (int) *value) ;
792    }
793    rf_cmd[3]= vl_tmp[0] ;
794    rf_cmd[4]= vl_tmp[1] ;
795    rf_cmd[5]= vl_tmp[2] ;
796    rf_cmd[6]= vl_tmp[3] ;
797    rf_cmd[7]= vl_tmp[4] ;
798    rf_cmd[8]= 0 ;
799  }
800
801  if ((ret_read_tmp= SendCommand( rf_cmd)) < 0)
802    return ret_read_tmp;
803
804
805  if (sscanf(rf_cmd, "FL%1c%5f", waste, &temp) < 1)
806    return -1;
807
808  *value = (double) temp  ;
809
810  return 0;
811}
812
813int RoboFocus::updateRFSetPosition(double *value)
814{
815
816  char rf_cmd[32] ;
817  char vl_tmp[6] ;
818  int ret_read_tmp ;
819
820  rf_cmd[0]=  'F' ;
821  rf_cmd[1]=  'S' ;
822  rf_cmd[2]=  '0' ;
823
824  if(*value > 9999) {
825    sprintf( vl_tmp, "%5d", (int) *value) ;
826  } else if(*value > 999) {
827    sprintf( vl_tmp, "0%4d", (int) *value) ;
828  } else if(*value > 99) {
829    sprintf( vl_tmp, "00%3d", (int) *value) ;
830  } else if(*value > 9) {
831    sprintf( vl_tmp, "000%2d", (int) *value) ;
832  } else {
833    sprintf( vl_tmp, "0000%1d", (int) *value) ;
834  }
835  rf_cmd[3]= vl_tmp[0] ;
836  rf_cmd[4]= vl_tmp[1] ;
837  rf_cmd[5]= vl_tmp[2] ;
838  rf_cmd[6]= vl_tmp[3] ;
839  rf_cmd[7]= vl_tmp[4] ;
840  rf_cmd[8]= 0 ;
841
842  if ((ret_read_tmp= SendCommand( rf_cmd)) < 0)
843    return ret_read_tmp;
844
845  return 0;
846}
847
848bool RoboFocus::ISNewText (const char *dev, const char *name, char *texts[], char *names[], int n)
849{
850    if(strcmp(dev,deviceName())==0)
851    {
852        // ===================================
853        // Port Name
854        // ===================================
855        if (!strcmp(name, PortTP.name) )
856        {
857          if (IUUpdateText(&PortTP, texts, names, n) < 0)
858                return false;
859
860          PortTP.s = IPS_OK;
861          IDSetText (&PortTP, NULL);
862          return true;
863        }
864
865    }
866
867     return INDI::Focuser::ISNewText(dev, name, texts, names, n);
868}
869
870bool RoboFocus::ISNewSwitch (const char *dev, const char *name, ISState *states, char *names[], int n)
871{
872    if(strcmp(dev,deviceName())==0)
873    {
874        if (!strcmp (name, PowerSwitchesSP.name))
875        {
876          int ret= -1 ;
877          int nset= 0 ;
878          int i= 0 ;
879          int new_s= -1 ;
880          int new_sn= -1 ;
881          int cur_s1LL=0 ;
882          int cur_s2LR=0 ;
883          int cur_s3RL=0 ;
884          int cur_s4RR=0 ;
885
886          ISwitch *sp ;
887
888          PowerSwitchesSP.s = IPS_BUSY ;
889          IDSetSwitch(&PowerSwitchesSP, NULL) ;
890
891
892          for( nset = i = 0; i < n; i++) {
893            /* Find numbers with the passed names in the SettingsNP property */
894            sp = IUFindSwitch (&PowerSwitchesSP, names[i]) ;
895
896            /* If the state found is  (PowerSwitchesS[0]) then process it */
897
898            if( sp == &PowerSwitchesS[0]){
899
900
901              new_s = (states[i]) ;
902              new_sn= 0;
903              nset++ ;
904            } else if( sp == &PowerSwitchesS[1]){
905
906              new_s = (states[i]) ;
907              new_sn= 1;
908              nset++ ;
909            } else if( sp == &PowerSwitchesS[2]){
910
911              new_s = (states[i]) ;
912              new_sn= 2;
913              nset++ ;
914            } else if( sp == &PowerSwitchesS[3]){
915
916              new_s = (states[i]) ;
917              new_sn= 3;
918              nset++ ;
919            }
920          }
921          if (nset == 1)
922          {
923            cur_s1LL= cur_s2LR= cur_s3RL= cur_s4RR= 0 ;
924
925            if(( ret= updateRFPowerSwitches(new_s, new_sn, &cur_s1LL, &cur_s2LR, &cur_s3RL, &cur_s4RR)) < 0)
926            {
927
928              PowerSwitchesSP.s = IPS_ALERT;
929              IDSetSwitch(&PowerSwitchesSP, "Unknown error while reading  Robofocus power swicht settings");
930              return true;
931            }
932          }
933          else
934          {
935            /* Set property state to idle */
936            PowerSwitchesSP.s = IPS_IDLE ;
937
938            IDSetNumber(&SettingsNP, "Power switch settings absent or bogus.");
939            return true ;
940          }
941
942      }
943    }
944
945
946    return INDI::Focuser::ISNewSwitch(dev, name, states, names, n);
947}
948
949bool RoboFocus::ISNewNumber (const char *dev, const char *name, double values[], char *names[], int n)
950{
951    int nset=0,i=0;
952
953    if(strcmp(dev,deviceName())==0)
954    {
955
956        if (!strcmp (name, SettingsNP.name))
957        {
958          /* new speed */
959          double new_duty = 0 ;
960          double new_delay = 0 ;
961          double new_ticks = 0 ;
962          int ret = -1 ;
963
964          for (nset = i = 0; i < n; i++)
965          {
966            /* Find numbers with the passed names in the SettingsNP property */
967            INumber *eqp = IUFindNumber (&SettingsNP, names[i]);
968
969            /* If the number found is  (SettingsN[0]) then process it */
970            if (eqp == &SettingsN[0])
971            {
972
973              new_duty = (values[i]);
974              nset += new_duty >= 0 && new_duty <= 255;
975            } else if  (eqp == &SettingsN[1])
976            {
977
978              new_delay = (values[i]);
979              nset += new_delay >= 0 && new_delay <= 255;
980            } else if  (eqp == &SettingsN[2])
981            {
982
983              new_ticks = (values[i]);
984              nset += new_ticks >= 0 && new_ticks <= 255;
985            }
986          }
987
988          /* Did we process the three numbers? */
989          if (nset == 3)
990          {
991
992            /* Set the robofocus state to BUSY */
993            SettingsNP.s = IPS_BUSY;
994
995
996            IDSetNumber(&SettingsNP, NULL);
997
998            if(( ret= updateRFMotorSettings(&new_duty, &new_delay, &new_ticks))< 0)
999            {
1000
1001              IDSetNumber(&SettingsNP, "Changing to new settings failed");
1002              return false;
1003            }
1004
1005            currentDuty = new_duty ;
1006            currentDelay= new_delay ;
1007            currentTicks= new_ticks ;
1008
1009            SettingsNP.s = IPS_OK;
1010            IDSetNumber(&SettingsNP, "Motor settings are now  %3.0f %3.0f %3.0f", currentDuty, currentDelay, currentTicks);
1011            return true;
1012
1013          } else
1014          {
1015            /* Set property state to idle */
1016            SettingsNP.s = IPS_IDLE;
1017
1018            IDSetNumber(&SettingsNP, "Settings absent or bogus.");
1019            return false ;
1020          }
1021        }
1022
1023
1024        if (!strcmp (name, RelMovementNP.name))
1025        {
1026
1027          double cur_rpos=0 ;
1028          double new_rpos = 0 ;
1029          int nset = 0;
1030          int ret= -1 ;
1031
1032          for (nset = i = 0; i < n; i++)
1033          {
1034            /* Find numbers with the passed names in the RelMovementNP property */
1035            INumber *eqp = IUFindNumber (&RelMovementNP, names[i]);
1036
1037            /* If the number found is RelMovement (RelMovementN[0]) then process it */
1038            if (eqp == &RelMovementN[0])
1039            {
1040
1041              cur_rpos= new_rpos = (values[i]);
1042
1043              /* CHECK 2006-01-26, limmits are relative to the actual position */
1044              nset += new_rpos >= -0xffff && new_rpos <= 0xffff;
1045            }
1046
1047            if (nset == 1)
1048            {
1049
1050              /* Set the robofocus state to BUSY */
1051              RelMovementNP.s = IPS_BUSY;
1052              IDSetNumber(&RelMovementNP, NULL);
1053
1054              if((currentPosition + new_rpos < currentMinPosition) || (currentPosition + new_rpos > currentMaxPosition))
1055              {
1056
1057                RelMovementNP.s = IPS_ALERT ;
1058                IDSetNumber(&RelMovementNP, "Value out of limits %5.0f", currentPosition +  new_rpos);
1059                return false ;
1060              }
1061
1062              if( new_rpos > 0)
1063              {
1064
1065                ret= updateRFPositionRelativeOutward(&new_rpos) ;
1066              } else
1067              {
1068
1069                new_rpos= -new_rpos ;
1070                ret= updateRFPositionRelativeInward(&new_rpos) ;
1071              }
1072
1073              if( ret < 0)
1074              {
1075
1076                RelMovementNP.s = IPS_IDLE;
1077                IDSetNumber(&RelMovementNP, "Read out of the relative movement failed, trying to recover position.");
1078                if((ret= updateRFPosition(&currentPosition)) < 0) {
1079
1080
1081                  PositionNP.s = IPS_ALERT;
1082                  IDSetNumber(&PositionNP, "Unknown error while reading  Robofocus position: %d", ret);
1083                  return false;
1084                }
1085                PositionNP.s = IPS_ALERT;
1086                IDSetNumber(&PositionNP, "Robofocus position recovered %5.0f", currentPosition);
1087
1088                IDMessage( deviceName(), "Robofocus position recovered resuming normal operation");
1089
1090                /* We have to leave here, because new_rpos is not set */
1091                return false ;
1092              }
1093
1094              RelMovementNP.s = IPS_OK;
1095              currentRelativeMovement= cur_rpos ;
1096              IDSetNumber(&RelMovementNP, NULL) ;
1097
1098              AbsMovementNP.s = IPS_OK;
1099              currentAbsoluteMovement=  new_rpos - cur_rpos ;
1100              IDSetNumber(&AbsMovementNP, NULL) ;
1101
1102              PositionNP.s = IPS_OK;
1103              currentPosition= new_rpos ;
1104              IDSetNumber(&PositionNP,  "Last position was %5.0f", currentAbsoluteMovement);
1105              return true;
1106
1107           } else {
1108
1109              RelMovementNP.s = IPS_IDLE;
1110              IDSetNumber(&RelMovementNP, "Need exactly one parameter.");
1111
1112              return  false;
1113            }
1114          }
1115        }
1116
1117
1118
1119        if (!strcmp (name, AbsMovementNP.name))
1120        {
1121
1122          double new_apos = 0 ;
1123          int nset = 0;
1124          int ret= -1 ;
1125
1126          for (nset = i = 0; i < n; i++)
1127          {
1128            /* Find numbers with the passed names in the AbsMovementNP property */
1129            INumber *eqp = IUFindNumber (&AbsMovementNP, names[i]);
1130
1131            /* If the number found is AbsMovement (AbsMovementN[0]) then process it */
1132            if (eqp == &AbsMovementN[0]){
1133
1134              new_apos = (values[i]);
1135
1136              /* limits are absolute to the actual position */
1137              nset += new_apos >= 0 && new_apos <= 0xffff;
1138            }
1139
1140            if (nset == 1)
1141            {
1142
1143              /* Set the robofocus state to BUSY */
1144              AbsMovementNP.s = IPS_BUSY;
1145              IDSetNumber(&AbsMovementNP, NULL);
1146
1147              if((new_apos < currentMinPosition) || (new_apos > currentMaxPosition))
1148              {
1149
1150                AbsMovementNP.s = IPS_ALERT ;
1151                IDSetNumber(&AbsMovementNP, "Value out of limits  %5.0f", new_apos);
1152                return false ;
1153              }
1154
1155              if(( ret= updateRFPositionAbsolute(&new_apos)) < 0)
1156              {
1157
1158                AbsMovementNP.s = IPS_IDLE;
1159                IDSetNumber(&AbsMovementNP, "Read out of the absolute movement failed %3d, trying to recover position.", ret);
1160
1161                if((ret= updateRFPosition(&currentPosition)) < 0)
1162                {
1163
1164                  PositionNP.s = IPS_ALERT;
1165                  IDSetNumber(&PositionNP, "Unknown error while reading  Robofocus position: %d.", ret);
1166
1167                  return false;
1168                }
1169
1170                PositionNP.s = IPS_OK;
1171                IDSetNumber(&PositionNP, "Robofocus position recovered %5.0f", currentPosition);
1172                IDMessage( deviceName(), "Robofocus position recovered resuming normal operation");
1173                /* We have to leave here, because new_apos is not set */
1174                return false;
1175              }
1176
1177              currentAbsoluteMovement=  currentPosition ;
1178              AbsMovementNP.s = IPS_OK;
1179              IDSetNumber(&AbsMovementNP, NULL) ;
1180
1181              PositionNP.s = IPS_OK;
1182              currentPosition= new_apos ;
1183              IDSetNumber(&PositionNP,  "Absolute position was  %5.0f", currentAbsoluteMovement);
1184              return true;
1185
1186            } else
1187            {
1188
1189              AbsMovementNP.s = IPS_IDLE;
1190              IDSetNumber(&RelMovementNP, "Need exactly one parameter.");
1191
1192              return false;
1193            }
1194          }
1195        }
1196
1197
1198
1199        if (!strcmp (name, SetBacklashNP.name))
1200        {
1201
1202          double new_back = 0 ;
1203          int nset = 0;
1204          int ret= -1 ;
1205
1206          for (nset = i = 0; i < n; i++)
1207          {
1208            /* Find numbers with the passed names in the SetBacklashNP property */
1209            INumber *eqp = IUFindNumber (&SetBacklashNP, names[i]);
1210
1211            /* If the number found is SetBacklash (SetBacklashN[0]) then process it */
1212            if (eqp == &SetBacklashN[0]){
1213
1214              new_back = (values[i]);
1215
1216              /* limits */
1217              nset += new_back >= -0xff && new_back <= 0xff;
1218            }
1219
1220            if (nset == 1) {
1221
1222              /* Set the robofocus state to BUSY */
1223              SetBacklashNP.s = IPS_BUSY;
1224              IDSetNumber(&SetBacklashNP, NULL);
1225
1226              if(( ret= updateRFBacklash(&new_back)) < 0) {
1227
1228                SetBacklashNP.s = IPS_IDLE;
1229                IDSetNumber(&SetBacklashNP, "Setting new backlash failed.");
1230
1231                return false ;
1232              }
1233
1234              currentSetBacklash=  new_back ;
1235              SetBacklashNP.s = IPS_OK;
1236              IDSetNumber(&SetBacklashNP, "Backlash is now  %3.0f", currentSetBacklash) ;
1237              return true;
1238            } else {
1239
1240              SetBacklashNP.s = IPS_IDLE;
1241              IDSetNumber(&SetBacklashNP, "Need exactly one parameter.");
1242
1243              return false ;
1244            }
1245          }
1246        }
1247
1248
1249
1250        if (!strcmp (name, MinMaxPositionNP.name))
1251        {
1252          /* new positions */
1253          double new_min = 0 ;
1254          double new_max = 0 ;
1255
1256          for (nset = i = 0; i < n; i++)
1257          {
1258            /* Find numbers with the passed names in the MinMaxPositionNP property */
1259            INumber *mmpp = IUFindNumber (&MinMaxPositionNP, names[i]);
1260
1261            /* If the number found is  (MinMaxPositionN[0]) then process it */
1262            if (mmpp == &MinMaxPositionN[0])
1263            {
1264
1265              new_min = (values[i]);
1266              nset += new_min >= 1 && new_min <= 65000;
1267            } else if  (mmpp == &MinMaxPositionN[1])
1268            {
1269
1270              new_max = (values[i]);
1271              nset += new_max >= 1 && new_max <= 65000;
1272            }
1273          }
1274
1275          /* Did we process the two numbers? */
1276          if (nset == 2)
1277          {
1278
1279            /* Set the robofocus state to BUSY */
1280            MinMaxPositionNP.s = IPS_BUSY;
1281
1282            currentMinPosition = new_min ;
1283            currentMaxPosition= new_max ;
1284
1285
1286            MinMaxPositionNP.s = IPS_OK;
1287            IDSetNumber(&MinMaxPositionNP, "Minimum and Maximum settings are now  %3.0f %3.0f", currentMinPosition, currentMaxPosition);
1288            return true;
1289
1290          } else {
1291            /* Set property state to idle */
1292            MinMaxPositionNP.s = IPS_IDLE;
1293
1294            IDSetNumber(&MinMaxPositionNP, "Minimum and maximum limits absent or bogus.");
1295
1296            return false;
1297          }
1298        }
1299
1300
1301        if (!strcmp (name, MaxTravelNP.name))
1302        {
1303
1304          double new_maxt = 0 ;
1305          int ret = -1 ;
1306
1307          for (nset = i = 0; i < n; i++)
1308          {
1309            /* Find numbers with the passed names in the MinMaxPositionNP property */
1310            INumber *mmpp = IUFindNumber (&MaxTravelNP, names[i]);
1311
1312            /* If the number found is  (MaxTravelN[0]) then process it */
1313            if (mmpp == &MaxTravelN[0])
1314            {
1315
1316              new_maxt = (values[i]);
1317              nset += new_maxt >= 1 && new_maxt <= 64000;
1318            }
1319          }
1320          /* Did we process the one number? */
1321          if (nset == 1) {
1322
1323            IDSetNumber(&MinMaxPositionNP, NULL);
1324
1325            if(( ret= updateRFMaxPosition(&new_maxt))< 0 )
1326            {
1327              MaxTravelNP.s = IPS_IDLE;
1328              IDSetNumber(&MaxTravelNP, "Changing to new maximum travel failed");
1329              return false ;
1330            }
1331
1332            currentMaxTravel=  new_maxt ;
1333            MaxTravelNP.s = IPS_OK;
1334            IDSetNumber(&MaxTravelNP, "Maximum travel is now  %3.0f", currentMaxTravel) ;
1335            return true;
1336
1337          } else {
1338            /* Set property state to idle */
1339
1340            MaxTravelNP.s = IPS_IDLE;
1341            IDSetNumber(&MaxTravelNP, "Maximum travel absent or bogus.");
1342
1343            return false ;
1344          }
1345        }
1346
1347
1348        if (!strcmp (name, SetRegisterPositionNP.name))
1349        {
1350
1351          double new_apos = 0 ;
1352          int nset = 0;
1353          int ret= -1 ;
1354
1355          for (nset = i = 0; i < n; i++)
1356          {
1357            /* Find numbers with the passed names in the SetRegisterPositionNP property */
1358            INumber *srpp = IUFindNumber (&SetRegisterPositionNP, names[i]);
1359
1360            /* If the number found is SetRegisterPosition (SetRegisterPositionN[0]) then process it */
1361            if (srpp == &SetRegisterPositionN[0])
1362            {
1363
1364              new_apos = (values[i]);
1365
1366              /* limits are absolute */
1367              nset += new_apos >= 0 && new_apos <= 64000;
1368            }
1369
1370            if (nset == 1)
1371            {
1372
1373              if((new_apos < currentMinPosition) || (new_apos > currentMaxPosition))
1374              {
1375
1376                SetRegisterPositionNP.s = IPS_ALERT ;
1377                IDSetNumber(&SetRegisterPositionNP, "Value out of limits  %5.0f", new_apos);
1378                return false ;
1379              }
1380
1381              /* Set the robofocus state to BUSY */
1382              SetRegisterPositionNP.s = IPS_BUSY;
1383              IDSetNumber(&SetRegisterPositionNP, NULL);
1384
1385              if(( ret= updateRFSetPosition(&new_apos)) < 0)
1386              {
1387
1388                SetRegisterPositionNP.s = IPS_OK;
1389                IDSetNumber(&SetRegisterPositionNP, "Read out of the set position to %3d failed. Trying to recover the position", ret);
1390
1391                if((ret= updateRFPosition( &currentPosition)) < 0)
1392                {
1393
1394                  PositionNP.s = IPS_ALERT;
1395                  IDSetNumber(&PositionNP, "Unknown error while reading  Robofocus position: %d", ret);
1396
1397                  SetRegisterPositionNP.s = IPS_IDLE;
1398                  IDSetNumber(&SetRegisterPositionNP, "Relative movement failed.");
1399                }
1400
1401                SetRegisterPositionNP.s = IPS_OK;
1402                IDSetNumber(&SetRegisterPositionNP, NULL);
1403
1404
1405                PositionNP.s = IPS_OK;
1406                IDSetNumber(&PositionNP, "Robofocus position recovered %5.0f", currentPosition);
1407                IDMessage( deviceName(), "Robofocus position recovered resuming normal operation");
1408                /* We have to leave here, because new_apos is not set */
1409                return true ;
1410              }
1411              currentPosition= new_apos ;
1412              SetRegisterPositionNP.s = IPS_OK;
1413              IDSetNumber(&SetRegisterPositionNP, "Robofocus register set to %5.0f", currentPosition);
1414
1415              PositionNP.s = IPS_OK;
1416              IDSetNumber(&PositionNP, "Robofocus position is now %5.0f", currentPosition);
1417
1418              return true ;
1419
1420            } else
1421            {
1422
1423              SetRegisterPositionNP.s = IPS_IDLE;
1424              IDSetNumber(&SetRegisterPositionNP, "Need exactly one parameter.");
1425
1426              return false;
1427            }
1428
1429            if((ret= updateRFPosition(&currentPosition)) < 0)
1430            {
1431
1432              PositionNP.s = IPS_ALERT;
1433              IDSetNumber(&PositionNP, "Unknown error while reading  Robofocus position: %d", ret);
1434
1435              return false ;
1436            }
1437
1438            SetRegisterPositionNP.s = IPS_OK;
1439            IDSetNumber(&SetRegisterPositionNP, "Robofocus has accepted new register setting" ) ;
1440
1441            PositionNP.s = IPS_OK;
1442            IDSetNumber(&PositionNP, "Robofocus new position %5.0f", currentPosition);
1443
1444            return true;
1445          }
1446        }
1447
1448
1449
1450    }
1451
1452    return INDI::Focuser::ISNewNumber(dev, name, values, names, n);
1453
1454}
1455
1456void RoboFocus::GetFocusParams ()
1457{
1458
1459  int ret = -1 ;
1460  int cur_s1LL=0 ;
1461  int cur_s2LR=0 ;
1462  int cur_s3RL=0 ;
1463  int cur_s4RR=0 ;
1464
1465
1466      if((ret= updateRFPosition(&currentPosition)) < 0)
1467      {
1468        PositionNP.s = IPS_ALERT;
1469        IDSetNumber(&PositionNP, "Unknown error while reading  Robofocus position: %d", ret);
1470        return;
1471      }
1472
1473      PositionNP.s = IPS_OK;
1474      IDSetNumber(&PositionNP, NULL);
1475
1476      AbsMovementN[0].value = currentPosition;
1477      IDSetNumber(&AbsMovementNP, NULL);
1478
1479      if(( ret= updateRFTemperature(&currentTemperature)) < 0)
1480      {
1481        TemperatureNP.s = IPS_ALERT;
1482        IDSetNumber(&TemperatureNP, "Unknown error while reading  Robofocus temperature");
1483        return;
1484      }
1485      TemperatureNP.s = IPS_OK;
1486      IDSetNumber(&TemperatureNP, NULL);
1487
1488
1489      currentBacklash= BACKLASH_READOUT ;
1490      if(( ret= updateRFBacklash(&currentBacklash)) < 0)
1491      {
1492        SetBacklashNP.s = IPS_ALERT;
1493        IDSetNumber(&SetBacklashNP, "Unknown error while reading  Robofocus backlash");
1494        return;
1495      }
1496      SetBacklashNP.s = IPS_OK;
1497      IDSetNumber(&SetBacklashNP, NULL);
1498
1499      currentDuty= currentDelay= currentTicks=0 ;
1500
1501      if(( ret= updateRFMotorSettings(&currentDuty, &currentDelay, &currentTicks )) < 0)
1502      {
1503        SettingsNP.s = IPS_ALERT;
1504        IDSetNumber(&SettingsNP, "Unknown error while reading  Robofocus motor settings");
1505        return;
1506      }
1507
1508      SettingsNP.s = IPS_OK;
1509      IDSetNumber(&SettingsNP, NULL);
1510
1511      if(( ret= updateRFPowerSwitches(-1, -1,  &cur_s1LL, &cur_s2LR, &cur_s3RL, &cur_s4RR)) < 0)
1512      {
1513        PowerSwitchesSP.s = IPS_ALERT;
1514        IDSetSwitch(&PowerSwitchesSP, "Unknown error while reading  Robofocus power swicht settings");
1515        return;
1516      }
1517
1518      PowerSwitchesS[0].s= PowerSwitchesS[1].s= PowerSwitchesS[2].s= PowerSwitchesS[3].s= ISS_OFF ;
1519
1520      if(cur_s1LL== ISS_ON) {
1521
1522        PowerSwitchesS[0].s= ISS_ON ;
1523      }
1524      if(cur_s2LR== ISS_ON) {
1525
1526        PowerSwitchesS[1].s= ISS_ON ;
1527      }
1528      if(cur_s3RL== ISS_ON) {
1529
1530        PowerSwitchesS[2].s= ISS_ON ;
1531      }
1532      if(cur_s4RR== ISS_ON) {
1533
1534        PowerSwitchesS[3].s= ISS_ON ;
1535      }
1536      PowerSwitchesSP.s = IPS_OK ;
1537      IDSetSwitch(&PowerSwitchesSP, NULL);
1538
1539
1540      currentMaxTravel= MAXTRAVEL_READOUT ;
1541      if(( ret= updateRFMaxPosition(&currentMaxTravel)) < 0)
1542      {
1543        MaxTravelNP.s = IPS_ALERT;
1544        IDSetNumber(&MaxTravelNP, "Unknown error while reading  Robofocus maximum travel");
1545        return;
1546      }
1547      MaxTravelNP.s = IPS_OK;
1548      IDSetNumber(&MaxTravelNP, NULL);
1549
1550}
1551
1552bool RoboFocus::Move(FocusDirection dir, int speed, int duration)
1553{
1554    INDI_UNUSED(speed);
1555    double pos=0;
1556    struct timeval tv_start;
1557    struct timeval tv_finish;
1558    double dt=0;
1559    gettimeofday (&tv_start, NULL);
1560
1561    while (duration > 0)
1562    {
1563
1564        pos = RF_STEP_RES;
1565
1566    if (dir == FOCUS_INWARD)
1567         updateRFPositionRelativeInward(&pos);
1568    else
1569        updateRFPositionRelativeOutward(&pos);
1570
1571        gettimeofday (&tv_finish, NULL);
1572
1573        dt = tv_finish.tv_sec - tv_start.tv_sec + (tv_finish.tv_usec - tv_start.tv_usec)/1e6;
1574
1575
1576
1577        duration -= dt * 1000;
1578
1579      // IDLog("dt is: %g --- duration is: %d -- pos: %g\n", dt, duration, pos);
1580    }
1581
1582   return true;
1583}
1584
1585
Note: See TracBrowser for help on using the repository browser.