Subversion Repositories Projects

Rev

Rev 140 | Go to most recent revision | Details | Compare with Previous | 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;
190
   termattr.c_cflag=CS8 | CREAD | CLOCAL;
191
   //   termattr.speed_t=B57600;
192
 
193
 
194
   tcsetattr(mk_socket, TCSANOW, &termattr);
195
 
196
 
197
   /*
198
   usleep(1000000);  
199
  char in_char='#';
200
  int count=0;
201
  int r=0;
202
  int i=0;
203
 
204
  int p=0;
205
  printf("starting read %d\n",mk_socket);
206
  while  (1)
207
    {
208
      p=0;
209
      r=0;
210
  char in_char='#';
211
  while((in_char!='\r'))//&&(r<MAX_BUFF_LEN))
212
        {
213
          //      p++;
214
          //      printf("b read\n");
215
          count=read(mk_socket,&in_char,1);
216
          // tcflush( mk_socket, TCOFLUSH );
217
 
218
          //      printf("\np !read %d %d %c \n",p,count,in_char);
219
 
220
          //count=read(mk_socket,&in_char,1);
221
          // tcflush( mk_socket, TCOFLUSH );
222
 
223
          //      printf("\np !read %d %d %d %c \n",r,p,count,in_char);
224
          printf("%d %c \n",r, in_char);
225
 
226
                  if (count==1)
227
                  {
228
                  //  printf("%c\n",in_char);
229
                    RxBuffer[r]=in_char;
230
 
231
                  if (in_char!=0)
232
                  PrintableRxBuffer[r]=in_char;
233
                  else
234
                  PrintableRxBuffer[r]='0';
235
                  r++;
236
                  }
237
          //else
238
                         //printf("%d/%d",errno,EBADF);
239
 
240
        }
241
  PrintableRxBuffer[r]=0;
242
      printf("buff> %d\n %s\n", r, PrintableRxBuffer);
243
    }
244
*/
245
  if (mk_socket<0)
246
    return 0;
247
  else
248
    return  1;
249
 
250
}
251
 
252
 
127 ligi 253
int connect_mk_localhost_socket(int port)
254
{
255
  struct sockaddr_in sa;
256
 
257
  sa.sin_family = AF_INET;
258
  sa.sin_addr.s_addr = htonl(0x0);
259
  sa.sin_port = htons(port);
260
 
261
  mk_socket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
262
 
483 ligi 263
  return  connect(mk_socket,(struct sockaddr*) &sa, sizeof(struct sockaddr_in));
127 ligi 264
 
265
}
266