← Back to branch summary

~ubuntu-branches/debian/jessie/stellarium/jessie

« back to all changes in this revision

Viewing changes to src/Telescope.cpp

Tags: upstream-0.9.0
Import upstream version 0.9.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/Telescope.cpp
 
1
/*
 
2
 * Author and Copyright of this file and of the stellarium telescope feature:
 
3
 * Johannes Gajdosik, 2006
 
4
 *
 
5
 * This program is free software; you can redistribute it and/or
 
6
 * modify it under the terms of the GNU General Public License
 
7
 * as published by the Free Software Foundation; either version 2
 
8
 * of the License, or (at your option) any later version.
 
9
 * 
 
10
 * This program 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
 
13
 * GNU General Public License for more details.
 
14
 * 
 
15
 * You should have received a copy of the GNU General Public License
 
16
 * along with this program; if not, write to the Free Software
 
17
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 
18
 */
 
19
 
 
20
#include "Telescope.hpp"
 
21
#include "StelUtils.hpp"
 
22
#include "Translator.hpp"
 
23
 
 
24
#include <sstream>
 
25
#include <iostream>
 
26
#include <iomanip>
 
27
 
 
28
#include <math.h>
 
29
 
 
30
#ifdef WIN32
 
31
  #include <windows.h> // GetSystemTimeAsFileTime
 
32
  #include <winsock2.h>
 
33
  #define ERRNO WSAGetLastError()
 
34
  #define STRERROR(x) x
 
35
  #undef EAGAIN
 
36
  #define EAGAIN WSAEWOULDBLOCK
 
37
  #undef EINTR
 
38
  #define EINTR WSAEINTR
 
39
  #undef EINPROGRESS
 
40
  #define EINPROGRESS WSAEINPROGRESS
 
41
  static u_long ioctlsocket_arg = 1;
 
42
  #define SET_NONBLOCKING_MODE(s) ioctlsocket(s,FIONBIO,&ioctlsocket_arg)
 
43
  #define SOCKLEN_T int
 
44
  #define close closesocket
 
45
  #define IS_INVALID_SOCKET(fd) (fd==INVALID_SOCKET)
 
46
#else
 
47
  #include <netdb.h>
 
48
  #include <netinet/in.h>
 
49
  #include <sys/socket.h>
 
50
  #include <sys/time.h>
 
51
  #include <unistd.h>
 
52
  #include <fcntl.h>
 
53
  #include <errno.h>
 
54
  #include <string.h> // strerror
 
55
  #define ERRNO errno
 
56
  #define STRERROR(x) strerror(x)
 
57
  #define SET_NONBLOCKING_MODE(s) fcntl(s,F_SETFL,O_NONBLOCK)
 
58
  #define SOCKLEN_T socklen_t
 
59
  #define SOCKET int
 
60
  #define IS_INVALID_SOCKET(fd) (fd<0)
 
61
  #define INVALID_SOCKET (-1)
 
62
#endif
 
63
 
 
64
struct PrintRaDec {
 
65
  PrintRaDec(const unsigned int ra_int,const int dec_int)
 
66
    :ra_int(ra_int),dec_int(dec_int) {}
 
67
  const unsigned int ra_int;
 
68
  const int dec_int;
 
69
};
 
70
 
 
71
template<class T>
 
72
T &operator<<(T &o,const PrintRaDec &x) {
 
73
  unsigned int h = x.ra_int;
 
74
  int d = (int)floor(0.5+x.dec_int*(360*3600*1000/4294967296.0));
 
75
  char dec_sign;
 
76
  if (d >= 0) {
 
77
    if (d > 90*3600*1000) {
 
78
      d =  180*3600*1000 - d;
 
79
      h += 0x80000000;
 
80
    }
 
81
    dec_sign = '+';
 
82
  } else {
 
83
    if (d < -90*3600*1000) {
 
84
      d = -180*3600*1000 - d;
 
85
      h += 0x80000000;
 
86
    }
 
87
    d = -d;
 
88
    dec_sign = '-';
 
89
  }
 
90
  h = (unsigned int)floor(0.5+h*(24*3600*10000/4294967296.0));
 
91
  const int ra_ms = h % 10000; h /= 10000;
 
92
  const int ra_s = h % 60; h /= 60;
 
93
  const int ra_m = h % 60; h /= 60;
 
94
  h %= 24;
 
95
  const int dec_ms = d % 1000; d /= 1000;
 
96
  const int dec_s = d % 60; d /= 60;
 
97
  const int dec_m = d % 60; d /= 60;
 
98
  o << "ra = "
 
99
    << setfill(' ') << setw(2) << h << 'h'
 
100
    << setfill('0') << setw(2) << ra_m << 'm'
 
101
    << setfill('0') << setw(2) << ra_s << '.'
 
102
    << setfill('0') << setw(4) << ra_ms
 
103
    << " dec = "
 
104
    << ((d<10)?" ":"") << dec_sign << d << 'd'
 
105
    << setfill('0') << setw(2) << dec_m << 'm'
 
106
    << setfill('0') << setw(2) << dec_s << '.'
 
107
    << setfill('0') << setw(3) << dec_ms
 
108
    << setfill(' ');
 
109
  return o;
 
110
}
 
111
 
 
112
class TelescopeDummy : public Telescope {
 
113
public:
 
114
  TelescopeDummy(const string &name,const string &params) : Telescope(name) {
 
115
    desired_pos[0] = XYZ[0] = 1.0;
 
116
    desired_pos[1] = XYZ[1] = 0.0;
 
117
    desired_pos[2] = XYZ[2] = 0.0;
 
118
  }
 
119
private:
 
120
  bool isConnected(void) const {return true;}
 
121
  bool hasKnownPosition(void) const {return true;}
 
122
  Vec3d getObsJ2000Pos(const Navigator *nav=0) const {return XYZ;}
 
123
  void prepareSelectFds(fd_set&,fd_set&,int&) {
 
124
    XYZ = XYZ*31.0+desired_pos;
 
125
    const double lq = XYZ.lengthSquared();
 
126
    if (lq > 0.0) XYZ *= (1.0/sqrt(lq));
 
127
    else XYZ = desired_pos;
 
128
  }
 
129
  void telescopeGoto(const Vec3d &j2000_pos) {
 
130
    desired_pos = j2000_pos;
 
131
    desired_pos.normalize();
 
132
  }
 
133
  Vec3d XYZ; // j2000 position
 
134
  Vec3d desired_pos;
 
135
};
 
136
 
 
137
 
 
138
class TelescopeTcp : public Telescope {
 
139
public:
 
140
  TelescopeTcp(const string &name,const string &params);
 
141
  ~TelescopeTcp(void) {hangup();}
 
142
private:
 
143
  bool isConnected(void) const
 
144
    {return (!IS_INVALID_SOCKET(fd) && !wait_for_connection_establishment);}
 
145
  Vec3d getObsJ2000Pos(const Navigator *nav=0) const;
 
146
  void prepareSelectFds(fd_set &read_fds,fd_set &write_fds,int &fd_max);
 
147
  void handleSelectFds(const fd_set &read_fds,const fd_set &write_fds);
 
148
  void telescopeGoto(const Vec3d &j2000_pos);
 
149
  bool isInitialized(void) const {return (ntohs(address.sin_port)!=0);}
 
150
  void performReading(void);
 
151
  void performWriting(void);
 
152
private:
 
153
  void hangup(void);
 
154
  struct sockaddr_in address;
 
155
  SOCKET fd;
 
156
  bool wait_for_connection_establishment;
 
157
  long long int end_of_timeout;
 
158
  char read_buff[120];
 
159
  char *read_buff_end;
 
160
  char write_buff[120];
 
161
  char *write_buff_end;
 
162
  int time_delay;
 
163
  struct Position {
 
164
    long long int server_micros;
 
165
    long long int client_micros;
 
166
    Vec3d pos;
 
167
    int status;
 
168
  };
 
169
  Position positions[16];
 
170
  Position *position_pointer;
 
171
  Position *const end_position;
 
172
  virtual bool hasKnownPosition(void) const
 
173
    {return (position_pointer->client_micros!=0x7FFFFFFFFFFFFFFFLL);}
 
174
};
 
175
 
 
176
Telescope *Telescope::create(const string &url) {
 
177
  string::size_type i = url.find(':');
 
178
  if (i == string::npos) {
 
179
    cout << "Telescope::create(" << url << "): bad url: \"" << url
 
180
         << "\", ':' missing" << endl;
 
181
    return 0;
 
182
  }
 
183
  const string name = url.substr(0,i);
 
184
  if (i+2 > url.length()) {
 
185
    cout << "Telescope::create(" << url << "): bad url: \"" << url
 
186
         << "\", too short" << endl;
 
187
    return 0;
 
188
  }
 
189
  string::size_type j = url.find(':',i+1);
 
190
  if (j == string::npos) {
 
191
    cout << "Telescope::create(" << url << "): bad url: \"" << url
 
192
         << "\", 2nd ':' missing" << endl;
 
193
    return 0;
 
194
  }
 
195
  const string type = url.substr(i+1,j-i-1);
 
196
  if (j+2 > url.length()) {
 
197
    cout << "Telescope::create(" << url << "): bad url: \"" << url
 
198
         << "\", too short" << endl;
 
199
    return 0;
 
200
  }
 
201
  const string params = url.substr(j+1);
 
202
  cout << "Telescope::create(" << url << "): trying to create telescope \""
 
203
       << name << "\" of type \"" << type << "\" with parameters \""
 
204
       << params << '"' << endl;
 
205
  Telescope *rval = 0;
 
206
  if (type == "Dummy") {
 
207
    rval = new TelescopeDummy(name,params);
 
208
  } else if (type == "TCP") {
 
209
    rval = new TelescopeTcp(name,params);
 
210
  } else {
 
211
    cout << "Telescope::create(" << url << "): unknown telescop type \""
 
212
         << type << '"' << endl;
 
213
  }
 
214
  if (rval && !rval->isInitialized()) {
 
215
    delete rval;
 
216
    rval = 0;
 
217
  }
 
218
  return rval;
 
219
}
 
220
 
 
221
 
 
222
Telescope::Telescope(const string &name) : name(name) {
 
223
  std::wostringstream oss;
 
224
  oss << name.c_str();
 
225
  nameI18n = oss.str();
 
226
}
 
227
 
 
228
wstring Telescope::getInfoString(const Navigator *nav) const {
 
229
  const Vec3d j2000_pos = getObsJ2000Pos(nav);
 
230
  double dec_j2000, ra_j2000;
 
231
  StelUtils::rect_to_sphe(&ra_j2000,&dec_j2000,j2000_pos);
 
232
  const Vec3d equatorial_pos = nav->j2000_to_earth_equ(j2000_pos);
 
233
  double dec_equ, ra_equ;
 
234
  StelUtils::rect_to_sphe(&ra_equ,&dec_equ,equatorial_pos);
 
235
  std::wostringstream oss;
 
236
  oss << nameI18n << endl
 
237
      << _("J2000") << L" " << _("RA/DE: ")
 
238
      << StelUtils::radToHmsWstr(ra_j2000,false)
 
239
      << L"/" << StelUtils::radToDmsWstr(dec_j2000,false) << endl
 
240
      << _("Equ of date") << L" " << _("RA/DE: ")
 
241
      << StelUtils::radToHmsWstr(ra_equ)
 
242
      << L"/" << StelUtils::radToDmsWstr(dec_equ);
 
243
  return oss.str();
 
244
}
 
245
 
 
246
wstring Telescope::getShortInfoString(const Navigator*) const {
 
247
  return nameI18n;
 
248
}
 
249
 
 
250
 
 
251
 
 
252
long long int GetNow(void) {
 
253
#ifdef WIN32
 
254
  FILETIME file_time;
 
255
  GetSystemTimeAsFileTime(&file_time);
 
256
  return (*((__int64*)(&file_time))/10) - 86400000000LL*134774;
 
257
#else
 
258
  struct timeval tv;
 
259
  gettimeofday(&tv,0);
 
260
  return tv.tv_sec * 1000000LL + tv.tv_usec;
 
261
#endif
 
262
}
 
263
 
 
264
TelescopeTcp::TelescopeTcp(const string &name,const string &params)
 
265
             :Telescope(name),fd(INVALID_SOCKET),
 
266
              end_position(positions+(sizeof(positions)/sizeof(positions[0]))) {
 
267
  hangup();
 
268
  address.sin_port = htons(0);
 
269
  string::size_type i = params.find(':');
 
270
  if (i == string::npos) {
 
271
    cout << "TelescopeTcp::TelescopeTcp(" << name << ',' << params << "): "
 
272
            "bad params: ':' missing" << endl;
 
273
    return;
 
274
  }
 
275
  const string host = params.substr(0,i);
 
276
  if (i+2 > params.length()) {
 
277
    cout << "TelescopeTcp::TelescopeTcp(" << name << ',' << params << "): "
 
278
            "bad params: too short"
 
279
         << endl;
 
280
    return;
 
281
  }
 
282
  string::size_type j = params.find(':',i+1);
 
283
  if (j == string::npos) {
 
284
    cout << "TelescopeTcp::TelescopeTcp(" << name << ',' << params << "): "
 
285
            "bad params: ':' missing" << endl;
 
286
    return;
 
287
  }
 
288
  int port;
 
289
  if (1!=sscanf(params.substr(i+1,j-i-1).c_str(),"%d",&port) ||
 
290
      port<=0 || port>0xFFFF) {
 
291
    cout << "TelescopeTcp::TelescopeTcp(" << name << ',' << params << "): "
 
292
            "bad port" << endl;
 
293
    return;
 
294
  }
 
295
  if (1!=sscanf(params.substr(j+1).c_str(),"%d",&time_delay) ||
 
296
      time_delay<=0 || time_delay>10000000) {
 
297
    cout << "TelescopeTcp::TelescopeTcp(" << name << ',' << params << "): "
 
298
            "bad time_delay" << endl;
 
299
    return;
 
300
  }
 
301
  struct hostent *hep = gethostbyname(host.c_str());
 
302
  if (hep == 0) {
 
303
    cout << "TelescopeTcp::TelescopeTcp(" << name << ',' << params << "): "
 
304
            "unknown host" << endl;
 
305
    return;
 
306
  }
 
307
  if (hep->h_length != 4) {
 
308
    cout << "TelescopeTcp::TelescopeTcp(" << name << ',' << params << "): "
 
309
            "only IPv4 implemented"
 
310
         << endl;
 
311
    return;
 
312
  }
 
313
  memset(&address,0,sizeof(struct sockaddr_in));
 
314
  memcpy(&(address.sin_addr),hep->h_addr,4);
 
315
  address.sin_port = htons(port);
 
316
  address.sin_family = AF_INET;
 
317
  end_of_timeout = -0x8000000000000000LL;
 
318
 
 
319
  for (position_pointer = positions;
 
320
       position_pointer < end_position;
 
321
       position_pointer++) {
 
322
    position_pointer->server_micros = 0x7FFFFFFFFFFFFFFFLL;
 
323
    position_pointer->client_micros = 0x7FFFFFFFFFFFFFFFLL;
 
324
    position_pointer->pos[0] = 0.0;
 
325
    position_pointer->pos[1] = 0.0;
 
326
    position_pointer->pos[2] = 0.0;
 
327
    position_pointer->status = 0;
 
328
  }
 
329
  position_pointer = positions;
 
330
}
 
331
 
 
332
void TelescopeTcp::hangup(void) {
 
333
  if (!IS_INVALID_SOCKET(fd)) {
 
334
    close(fd);
 
335
    fd = INVALID_SOCKET;
 
336
  }
 
337
  read_buff_end = read_buff;
 
338
  write_buff_end = write_buff;
 
339
  wait_for_connection_establishment = false;
 
340
  for (position_pointer = positions;
 
341
       position_pointer < end_position;
 
342
       position_pointer++) {
 
343
    position_pointer->server_micros = 0x7FFFFFFFFFFFFFFFLL;
 
344
    position_pointer->client_micros = 0x7FFFFFFFFFFFFFFFLL;
 
345
    position_pointer->pos[0] = 0.0;
 
346
    position_pointer->pos[1] = 0.0;
 
347
    position_pointer->pos[2] = 0.0;
 
348
    position_pointer->status = 0;
 
349
  }
 
350
  position_pointer = positions;
 
351
}
 
352
 
 
353
void TelescopeTcp::telescopeGoto(const Vec3d &j2000_pos) {
 
354
  if (isConnected()) {
 
355
    if (write_buff_end-write_buff+20 < (int)sizeof(write_buff)) {
 
356
      const double ra = atan2(j2000_pos[1],j2000_pos[0]);
 
357
      const double dec = atan2(j2000_pos[2],
 
358
              sqrt(j2000_pos[0]*j2000_pos[0]+j2000_pos[1]*j2000_pos[1]));
 
359
      unsigned int ra_int = (unsigned int)floor(
 
360
                               0.5 +  ra*(((unsigned int)0x80000000)/M_PI));
 
361
      int dec_int = (int)floor(0.5 + dec*(((unsigned int)0x80000000)/M_PI));
 
362
//      cout << "TelescopeTcp(" << name << ")::telescopeGoto: "
 
363
//              "queuing packet: " << PrintRaDec(ra_int,dec_int) << endl;
 
364
        // length of packet:
 
365
      *write_buff_end++ = 20;
 
366
      *write_buff_end++ = 0;
 
367
        // type of packet:
 
368
      *write_buff_end++ = 0;
 
369
      *write_buff_end++ = 0;
 
370
        // client_micros:
 
371
      long long int now = GetNow();
 
372
      *write_buff_end++ = now;now>>=8;
 
373
      *write_buff_end++ = now;now>>=8;
 
374
      *write_buff_end++ = now;now>>=8;
 
375
      *write_buff_end++ = now;now>>=8;
 
376
      *write_buff_end++ = now;now>>=8;
 
377
      *write_buff_end++ = now;now>>=8;
 
378
      *write_buff_end++ = now;now>>=8;
 
379
      *write_buff_end++ = now;
 
380
        // ra:
 
381
      *write_buff_end++ = ra_int;ra_int>>=8;
 
382
      *write_buff_end++ = ra_int;ra_int>>=8;
 
383
      *write_buff_end++ = ra_int;ra_int>>=8;
 
384
      *write_buff_end++ = ra_int;
 
385
        // dec:
 
386
      *write_buff_end++ = dec_int;dec_int>>=8;
 
387
      *write_buff_end++ = dec_int;dec_int>>=8;
 
388
      *write_buff_end++ = dec_int;dec_int>>=8;
 
389
      *write_buff_end++ = dec_int;
 
390
    } else {
 
391
      cout << "TelescopeTcp(" << name << ")::telescopeGoto: "
 
392
              "communication is too slow, I will ignore this command" << endl;
 
393
    }
 
394
  }
 
395
}
 
396
 
 
397
void TelescopeTcp::performWriting(void) {
 
398
  const int to_write = write_buff_end - write_buff;
 
399
  const int rc = send(fd,write_buff,to_write,0);
 
400
  if (rc < 0) {
 
401
    if (ERRNO != EINTR && ERRNO != EAGAIN) {
 
402
      cout << "TelescopeTcp(" << name << ")::performWriting: "
 
403
              "send failed: " << STRERROR(ERRNO) << endl;
 
404
      hangup();
 
405
    }
 
406
  } else if (rc > 0) {
 
407
    if (rc >= to_write) {
 
408
        // everything written
 
409
      write_buff_end = write_buff;
 
410
    } else {
 
411
        // partly written
 
412
      memmove(write_buff,write_buff+rc,to_write-rc);
 
413
      write_buff_end -= rc;
 
414
    }
 
415
  }
 
416
}
 
417
 
 
418
void TelescopeTcp::performReading(void) {
 
419
  const int to_read = read_buff + sizeof(read_buff) - read_buff_end;
 
420
  const int rc = recv(fd,read_buff_end,to_read,0);
 
421
  if (rc < 0) {
 
422
    if (ERRNO != EINTR && ERRNO != EAGAIN) {
 
423
      cout << "TelescopeTcp(" << name << ")::performReading: "
 
424
              "recv failed: " << STRERROR(ERRNO) << endl;
 
425
      hangup();
 
426
    }
 
427
  } else if (rc == 0) {
 
428
    cout << "TelescopeTcp(" << name << ")::performReading: "
 
429
            "server has closed the connection" << endl;
 
430
    hangup();
 
431
  } else {
 
432
    read_buff_end += rc;
 
433
    char *p = read_buff;
 
434
    while (read_buff_end-p >= 2) {
 
435
      const int size = (int)(                ((unsigned char)(p[0])) |
 
436
                              (((unsigned int)(unsigned char)(p[1])) << 8) );
 
437
      if (size > (int)sizeof(read_buff) || size < 4) {
 
438
        cout << "TelescopeTcp(" << name << ")::performReading: "
 
439
                "bad packet size: " << size << endl;
 
440
        hangup();
 
441
        return;
 
442
      }
 
443
      if (size > read_buff_end-p) {
 
444
          // wait for complete packet
 
445
        break;
 
446
      }
 
447
      const int type = (int)(                ((unsigned char)(p[2])) |
 
448
                              (((unsigned int)(unsigned char)(p[3])) << 8) );
 
449
        // dispatch:
 
450
      switch (type) {
 
451
        case 0: {
 
452
          if (size < 24) {
 
453
            cout << "TelescopeTcp(" << name << ")::performReading: "
 
454
                    "type 0: bad packet size: " << size << endl;
 
455
            hangup();
 
456
            return;
 
457
          }
 
458
          const long long int server_micros = (long long int)
 
459
                 (  ((unsigned long long int)(unsigned char)(p[ 4])) |
 
460
                   (((unsigned long long int)(unsigned char)(p[ 5])) <<  8) |
 
461
                   (((unsigned long long int)(unsigned char)(p[ 6])) << 16) |
 
462
                   (((unsigned long long int)(unsigned char)(p[ 7])) << 24) |
 
463
                   (((unsigned long long int)(unsigned char)(p[ 8])) << 32) |
 
464
                   (((unsigned long long int)(unsigned char)(p[ 9])) << 40) |
 
465
                   (((unsigned long long int)(unsigned char)(p[10])) << 48) |
 
466
                   (((unsigned long long int)(unsigned char)(p[11])) << 56) );
 
467
          const unsigned int ra_int =
 
468
                    ((unsigned int)(unsigned char)(p[12])) |
 
469
                   (((unsigned int)(unsigned char)(p[13])) <<  8) |
 
470
                   (((unsigned int)(unsigned char)(p[14])) << 16) |
 
471
                   (((unsigned int)(unsigned char)(p[15])) << 24);
 
472
          const int dec_int =
 
473
            (int)(  ((unsigned int)(unsigned char)(p[16])) |
 
474
                   (((unsigned int)(unsigned char)(p[17])) <<  8) |
 
475
                   (((unsigned int)(unsigned char)(p[18])) << 16) |
 
476
                   (((unsigned int)(unsigned char)(p[19])) << 24) );
 
477
          const int status =
 
478
            (int)(  ((unsigned int)(unsigned char)(p[20])) |
 
479
                   (((unsigned int)(unsigned char)(p[21])) <<  8) |
 
480
                   (((unsigned int)(unsigned char)(p[22])) << 16) |
 
481
                   (((unsigned int)(unsigned char)(p[23])) << 24) );
 
482
 
 
483
          position_pointer++;
 
484
          if (position_pointer >= end_position) position_pointer = positions;
 
485
          position_pointer->server_micros = server_micros;
 
486
          position_pointer->client_micros = GetNow();
 
487
          const double ra  =  ra_int * (M_PI/(unsigned int)0x80000000);
 
488
          const double dec = dec_int * (M_PI/(unsigned int)0x80000000);
 
489
          const double cdec = cos(dec);
 
490
          position_pointer->pos[0] = cos(ra)*cdec;
 
491
          position_pointer->pos[1] = sin(ra)*cdec;
 
492
          position_pointer->pos[2] = sin(dec);
 
493
          position_pointer->status = status;
 
494
//          cout << "TelescopeTcp(" << name << ")::performReading: "
 
495
////                  "Server Time: " << server_micros
 
496
//               << PrintRaDec(ra_int,dec_int)
 
497
//               << endl;
 
498
        } break;
 
499
        default:
 
500
          cout << "TelescopeTcp(" << name << ")::performReading: "
 
501
                  "ignoring unknown packet, type: " << type << endl;
 
502
          break;
 
503
      }
 
504
      p += size;
 
505
    }
 
506
    if (p >= read_buff_end) {
 
507
        // everything handled
 
508
      read_buff_end = read_buff;
 
509
    } else {
 
510
        // partly handled
 
511
      memmove(read_buff,p,read_buff_end-p);
 
512
      read_buff_end -= (p-read_buff);
 
513
    }
 
514
  }
 
515
}
 
516
 
 
517
Vec3d TelescopeTcp::getObsJ2000Pos(const Navigator*) const {
 
518
  if (position_pointer->client_micros == 0x7FFFFFFFFFFFFFFFLL) {
 
519
    return Vec3d(0,0,0);
 
520
  }
 
521
  const long long int now = GetNow() - time_delay;
 
522
  const Position *p = position_pointer;
 
523
  do {
 
524
    const Position *pp = p;
 
525
    if (pp == positions) pp = end_position;
 
526
    pp--;
 
527
    if (pp->client_micros == 0x7FFFFFFFFFFFFFFFLL) break;
 
528
    if (pp->client_micros <= now && now <= p->client_micros) {
 
529
      if (pp->client_micros != p->client_micros) {
 
530
        Vec3d rval = p->pos * (now - pp->client_micros)
 
531
                   + pp->pos * (p->client_micros - now);
 
532
        double f = rval.lengthSquared();
 
533
        if (f > 0.0) {
 
534
          return (1.0/sqrt(f))*rval;
 
535
        }
 
536
      }
 
537
      break;
 
538
    }
 
539
    p = pp;
 
540
  } while (p != position_pointer);
 
541
  return p->pos;
 
542
}
 
543
 
 
544
void TelescopeTcp::prepareSelectFds(fd_set &read_fds,fd_set &write_fds,
 
545
                                    int &fd_max) {
 
546
  if (IS_INVALID_SOCKET(fd)) {
 
547
      // try reconnecting
 
548
    const long long int now = GetNow();
 
549
    if (now < end_of_timeout) return;
 
550
    end_of_timeout = now + 5000000;
 
551
    fd = socket(AF_INET,SOCK_STREAM,0);
 
552
    if (IS_INVALID_SOCKET(fd)) {
 
553
      cout << "TelescopeTcp(" << name << ")::prepareSelectFds: "
 
554
              "socket() failed: " << STRERROR(ERRNO) << endl;
 
555
      return;
 
556
    }
 
557
    if (SET_NONBLOCKING_MODE(fd) != 0) {
 
558
      cout << "TelescopeTcp(" << name << ")::prepareSelectFds: "
 
559
              "could not set nonblocking mode: " << STRERROR(ERRNO) << endl;
 
560
      hangup();
 
561
      return;
 
562
    }
 
563
    if (connect(fd,(struct sockaddr*)(&address),sizeof(address)) != 0) {
 
564
      if (ERRNO != EINPROGRESS && ERRNO != EAGAIN) {
 
565
        cout << "TelescopeTcp(" << name << ")::prepareSelectFds: "
 
566
                "connect() failed: " << STRERROR(ERRNO) << endl;
 
567
        hangup();
 
568
        return;
 
569
      }
 
570
      wait_for_connection_establishment = true;
 
571
//      cout << "TelescopeTcp(" << name << ")::prepareSelectFds: "
 
572
//              "waiting for connection establishment" << endl;
 
573
    } else {
 
574
      wait_for_connection_establishment = false;
 
575
      cout << "TelescopeTcp(" << name << ")::prepareSelectFds: "
 
576
              "connection established" << endl;
 
577
        // connection established, wait for next call of prepareSelectFds
 
578
    }
 
579
  } else {
 
580
      // socked is already connected
 
581
    if (fd_max < (int)fd) fd_max = (int)fd;
 
582
    if (wait_for_connection_establishment) {
 
583
      const long long int now = GetNow();
 
584
      if (now > end_of_timeout) {
 
585
        end_of_timeout = now + 1000000;
 
586
        cout << "TelescopeTcp(" << name << ")::prepareSelectFds: "
 
587
                "connect timeout" << endl;
 
588
        hangup();
 
589
        return;
 
590
      }
 
591
      FD_SET(fd,&write_fds);
 
592
    } else {
 
593
      if (write_buff_end > write_buff) FD_SET(fd,&write_fds);
 
594
      FD_SET(fd,&read_fds);
 
595
    }
 
596
  }
 
597
}
 
598
 
 
599
void TelescopeTcp::handleSelectFds(const fd_set &read_fds,
 
600
                                   const fd_set &write_fds) {
 
601
  if (!IS_INVALID_SOCKET(fd)) {
 
602
    if (wait_for_connection_establishment) {
 
603
      if (FD_ISSET(fd,&write_fds)) {
 
604
        wait_for_connection_establishment = false;
 
605
        int err = 0;
 
606
        SOCKLEN_T length = sizeof(err);
 
607
        if (getsockopt(fd,SOL_SOCKET,SO_ERROR,(char*)(&err),&length) != 0) {
 
608
          cout << "TelescopeTcp(" << name << ")::handleSelectFds: "
 
609
                  "getsockopt failed" << endl;
 
610
          hangup();
 
611
        } else {
 
612
          if (err != 0) {
 
613
            cout << "TelescopeTcp(" << name << ")::handleSelectFds: "
 
614
                    "connect failed: " << STRERROR(err) << endl;
 
615
            hangup();
 
616
          } else {
 
617
            cout << "TelescopeTcp(" << name << ")::handleSelectFds: "
 
618
                    "connection established" << endl;
 
619
          }
 
620
        }
 
621
      }
 
622
    } else { // connection already established
 
623
      if (FD_ISSET(fd,&write_fds)) {
 
624
        performWriting();
 
625
      }
 
626
      if (!IS_INVALID_SOCKET(fd) && FD_ISSET(fd,&read_fds)) {
 
627
        performReading();
 
628
      }
 
629
    }
 
630
  }
 
631
}
 
632
 
 
633
 
 
634