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;
140 ligi 118
  int i=0;
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);
129
      printf("a read %d %d %c \n",p,count,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
 
179
  mk_socket = open(tty_filename,O_RDWR);
127 ligi 180
 
130 ligi 181
 
483 ligi 182
 
183
  struct termios termattr;
184
   speed_t baudRate;
185
 
186
      /* Make a copy of the termios structure. */
187
   tcgetattr(mk_socket, &termattr);
188
 
189
   termattr.c_iflag = IGNBRK | IGNPAR;
487 ligi 190
   termattr.c_cflag=CS8 | CREAD | CLOCAL ;
191
   termattr.c_cflag&= ~CBAUD;
483 ligi 192
 
487 ligi 193
   termattr.c_cflag|=B57600;
483 ligi 194
 
487 ligi 195
 
196
 
197
 
483 ligi 198
   tcsetattr(mk_socket, TCSANOW, &termattr);
199
 
200
 
201
   /*
202
   usleep(1000000);  
203
  char in_char='#';
204
  int count=0;
205
  int r=0;
206
  int i=0;
207
 
208
  int p=0;
209
  printf("starting read %d\n",mk_socket);
210
  while  (1)
211
    {
212
      p=0;
213
      r=0;
214
  char in_char='#';
215
  while((in_char!='\r'))//&&(r<MAX_BUFF_LEN))
216
        {
217
          //      p++;
218
          //      printf("b read\n");
219
          count=read(mk_socket,&in_char,1);
220
          // tcflush( mk_socket, TCOFLUSH );
221
 
222
          //      printf("\np !read %d %d %c \n",p,count,in_char);
223
 
224
          //count=read(mk_socket,&in_char,1);
225
          // tcflush( mk_socket, TCOFLUSH );
226
 
227
          //      printf("\np !read %d %d %d %c \n",r,p,count,in_char);
228
          printf("%d %c \n",r, in_char);
229
 
230
                  if (count==1)
231
                  {
232
                  //  printf("%c\n",in_char);
233
                    RxBuffer[r]=in_char;
234
 
235
                  if (in_char!=0)
236
                  PrintableRxBuffer[r]=in_char;
237
                  else
238
                  PrintableRxBuffer[r]='0';
239
                  r++;
240
                  }
241
          //else
242
                         //printf("%d/%d",errno,EBADF);
243
 
244
        }
245
  PrintableRxBuffer[r]=0;
246
      printf("buff> %d\n %s\n", r, PrintableRxBuffer);
247
    }
248
*/
249
  if (mk_socket<0)
250
    return 0;
251
  else
252
    return  1;
253
 
254
}
255
 
256
 
127 ligi 257
int connect_mk_localhost_socket(int port)
258
{
259
  struct sockaddr_in sa;
260
 
261
  sa.sin_family = AF_INET;
262
  sa.sin_addr.s_addr = htonl(0x0);
263
  sa.sin_port = htons(port);
264
 
265
  mk_socket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
266
 
483 ligi 267
  return  connect(mk_socket,(struct sockaddr*) &sa, sizeof(struct sockaddr_in));
127 ligi 268
 
269
}
270