Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
127 ligi 1
#include "fc.h"
2
 
3
 
4
 
483 ligi 5
unsigned char TxBuffer[MAX_BUFF_LEN];
6
unsigned char _TxBuffer[MAX_BUFF_LEN];
127 ligi 7
 
483 ligi 8
unsigned char RxBuffer[MAX_BUFF_LEN];
9
char PrintableRxBuffer[MAX_BUFF_LEN];
130 ligi 10
 
11
 
483 ligi 12
 
127 ligi 13
int mk_socket;
14
 
130 ligi 15
int status;
127 ligi 16
 
17
void AddCRC(unsigned int wieviele)
18
{
19
  unsigned int tmpCRC = 0,i;
20
  for(i = 0; i < wieviele;i++)
21
    {
22
      tmpCRC += TxBuffer[i];
23
    }
24
  tmpCRC %= 4096;
25
  TxBuffer[i++] = '=' + tmpCRC / 64;
26
  TxBuffer[i++] = '=' + tmpCRC % 64;
27
  TxBuffer[i++] = '\r';
28
}
29
 
30
 
31
void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)
32
{
483 ligi 33
  //  return;
127 ligi 34
  int status =0;
35
  unsigned int pt = 0;
36
  unsigned char a,b,c;
37
  unsigned char ptr = 0;
38
 
39
  TxBuffer[pt++] = '#';               // Startzeichen
40
  TxBuffer[pt++] = modul;             // Adresse (a=0; b=1,...)
41
  TxBuffer[pt++] = cmd;                 // Commando
42
 
43
  while(len)
44
    {
45
      if(len) { a = snd[ptr++]; len--;} else a = 0;
46
      if(len) { b = snd[ptr++]; len--;} else b = 0;
47
      if(len) { c = snd[ptr++]; len--;} else c = 0;
48
      TxBuffer[pt++] = '=' + (a >> 2);
49
      TxBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
50
      TxBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
51
      TxBuffer[pt++] = '=' + ( c & 0x3f);
52
    }
53
 
54
 
55
 
56
  AddCRC(pt);
57
  printf("Sending to MK %d \n" , pt);
58
 
59
  status = send(mk_socket,"\r" , 1, 0);
60
 
61
 
62
  //  for (c=0;c<pt+2;c++)
63
  // {
64
  status = write(mk_socket,&TxBuffer , pt+3);
130 ligi 65
 
66
 
127 ligi 67
  //   printf("Send to MK %d \n" , TxBuffer[c] );
68
  // }
130 ligi 69
  /* int i=0;
70
  while(TxBuffer[i] !='\r' && i<150)
127 ligi 71
    {
72
    //     TxBuffer[i]='#';
130 ligi 73
    status = send(mk_socket,TxBuffer[i] , 1, 0);
127 ligi 74
    printf(" +%d%c ",i,TxBuffer[i]);
75
    i++;
76
    }
77
 
130 ligi 78
  //  status = send(s,"\r" , 1, 0);
127 ligi 79
  */
130 ligi 80
   status = send(mk_socket,"\r" , 1, 0);
81
   status = send(mk_socket,"\n" , 1, 0);
127 ligi 82
  printf("\n");
83
}
84
 
85
 
130 ligi 86
int rx_last_length;
87
 
140 ligi 88
 
89
void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max)
90
{
91
 unsigned char a,b,c,d;
92
 unsigned char ptr = 0;
93
 unsigned char x,y,z;
94
 while(len)
95
  {
96
   a = RxBuffer[ptrIn++] - '=';
97
   b = RxBuffer[ptrIn++] - '=';
98
   c = RxBuffer[ptrIn++] - '=';
99
   d = RxBuffer[ptrIn++] - '=';
100
   if(ptrIn > max - 2) break;    
101
 
102
   x = (a << 2) | (b >> 4);
103
   y = ((b & 0x0f) << 4) | (c >> 2);
104
   z = ((c & 0x03) << 6) | d;
105
 
106
   if(len--) ptrOut[ptr++] = x; else break;
107
   if(len--) ptrOut[ptr++] = y; else break;
108
   if(len--) ptrOut[ptr++] = z; else break;
109
  }
110
 
111
}
112
 
130 ligi 113
int read_from_mk()
114
{      
115
  char in_char='#';
116
  int count=0;
117
  int r=0;
522 ligi 118
  //  int i=0;
140 ligi 119
 
483 ligi 120
  int p=0;
130 ligi 121
  printf("starting read\n");
483 ligi 122
  while(in_char!='\r')
130 ligi 123
    {
483 ligi 124
      p++;
130 ligi 125
      //      printf("b read\n");
126
      count=read(mk_socket,&in_char,1);
483 ligi 127
 
128
      //      if ( count ==-1) exit(0);
488 ligi 129
      printf("a read %d %d %c %d\n",p,count,in_char,in_char);
130 ligi 130
      if (count!=-1)
131
        {
132
          //  printf("%c\n",in_char);
133
          RxBuffer[r]=in_char;
134
 
135
          if (in_char!=0)
136
            PrintableRxBuffer[r++]=in_char;
137
          else
138
            PrintableRxBuffer[r++]='0';
139
        }
140
 
141
    }
142
  rx_last_length=r;
143
  PrintableRxBuffer[r++]='\0'; // terminate 
144
  printf("done --->%s\n",PrintableRxBuffer);
145
 
146
  if (RxBuffer[2]=='D')
140 ligi 147
    {
148
      debug_sets++;
149
      Decode64((unsigned char *) &DebugOut,sizeof(DebugOut),3,rx_last_length);
150
      printf("decoded FC Debug data height:%d\n",DebugOut.Analog[5]);
151
    }
130 ligi 152
  return 1;
153
}
154
 
155
 
127 ligi 156
int connect_mk_bluetooth(char dest[18])
157
{
158
  struct sockaddr_rc addr ;
159
 
160
  // allocate a socket
161
  mk_socket = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
162
 
163
  // set the connection parameters (who to connect to)
164
  addr.rc_family = AF_BLUETOOTH;
165
  addr.rc_channel = 1;
166
  str2ba( dest, &addr.rc_bdaddr );
167
 
168
  // connect to server
169
  status = connect(mk_socket, (struct sockaddr *)&addr, sizeof(addr));
170
 
130 ligi 171
  printf("connection status %d\n",status);
127 ligi 172
  return status;
173
}
174
 
175
 
483 ligi 176
int connect_mk_tty(char* tty_filename)
177
{
178
 
488 ligi 179
  mk_socket = open(tty_filename,O_RDWR | O_NOCTTY);
127 ligi 180
 
130 ligi 181
 
483 ligi 182
 
183
  struct termios termattr;
522 ligi 184
  //   speed_t baudRate;
483 ligi 185
 
186
      /* Make a copy of the termios structure. */
187
   tcgetattr(mk_socket, &termattr);
488 ligi 188
    termattr.c_cc[VMIN]=0;
189
    termattr.c_cc[VTIME]=1;
483 ligi 190
 
488 ligi 191
    termattr.c_cflag|=CREAD|CLOCAL;
192
    termattr.c_lflag&=(~(ICANON|ECHO|ECHOE|ECHOK|ECHONL|ISIG));
193
    termattr.c_iflag&=(~(INPCK|IGNPAR|PARMRK|ISTRIP|ICRNL|IXANY));
194
    termattr.c_oflag&=(~OPOST);
195
    termattr.c_cc[VMIN]=0;
196
    termattr.c_cc[VINTR] = _POSIX_VDISABLE;
197
    termattr.c_cc[VQUIT] = _POSIX_VDISABLE;
198
    termattr.c_cc[VSTART] = _POSIX_VDISABLE;
199
    termattr.c_cc[VSTOP] = _POSIX_VDISABLE;
200
    termattr.c_cc[VSUSP] = _POSIX_VDISABLE;
201
 
483 ligi 202
 
488 ligi 203
    //data 8
204
 
205
    termattr.c_cflag&=(~CSIZE);
206
 
207
    // no par
208
    termattr.c_cflag&=(~PARENB);
209
 
210
    termattr.c_cflag |=CS8 | CREAD | CLOCAL ;
211
    termattr.c_iflag |=  IGNPAR;
212
    // stop1
213
    termattr.c_cflag&=(~CSTOPB);
214
 
215
    // Flow off
216
    termattr.c_cflag&=(~CRTSCTS);
217
    termattr.c_iflag&=(~(IXON|IXOFF|IXANY | ICRNL));
218
 
219
 
220
 
221
    cfsetospeed(&termattr,B57600);
222
    cfsetispeed(&termattr,B57600);
483 ligi 223
 
488 ligi 224
    printf("set attr: %d\n" , tcsetattr(mk_socket, TCSANOW, &termattr));
225
 
487 ligi 226
 
483 ligi 227
   /*
228
   usleep(1000000);  
229
  char in_char='#';
230
  int count=0;
231
  int r=0;
232
  int i=0;
233
 
234
  int p=0;
235
  printf("starting read %d\n",mk_socket);
236
  while  (1)
237
    {
238
      p=0;
239
      r=0;
240
  char in_char='#';
241
  while((in_char!='\r'))//&&(r<MAX_BUFF_LEN))
242
        {
243
          //      p++;
244
          //      printf("b read\n");
245
          count=read(mk_socket,&in_char,1);
246
          // tcflush( mk_socket, TCOFLUSH );
247
 
248
          //      printf("\np !read %d %d %c \n",p,count,in_char);
249
 
250
          //count=read(mk_socket,&in_char,1);
251
          // tcflush( mk_socket, TCOFLUSH );
252
 
253
          //      printf("\np !read %d %d %d %c \n",r,p,count,in_char);
254
          printf("%d %c \n",r, in_char);
255
 
256
                  if (count==1)
257
                  {
258
                  //  printf("%c\n",in_char);
259
                    RxBuffer[r]=in_char;
260
 
261
                  if (in_char!=0)
262
                  PrintableRxBuffer[r]=in_char;
263
                  else
264
                  PrintableRxBuffer[r]='0';
265
                  r++;
266
                  }
267
          //else
268
                         //printf("%d/%d",errno,EBADF);
269
 
270
        }
271
  PrintableRxBuffer[r]=0;
272
      printf("buff> %d\n %s\n", r, PrintableRxBuffer);
273
    }
274
*/
275
  if (mk_socket<0)
276
    return 0;
277
  else
278
    return  1;
279
 
280
}
281
 
282
 
127 ligi 283
int connect_mk_localhost_socket(int port)
284
{
285
  struct sockaddr_in sa;
286
 
287
  sa.sin_family = AF_INET;
288
  sa.sin_addr.s_addr = htonl(0x0);
289
  sa.sin_port = htons(port);
290
 
291
  mk_socket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
292
 
483 ligi 293
  return  connect(mk_socket,(struct sockaddr*) &sa, sizeof(struct sockaddr_in));
127 ligi 294
 
295
}
296