Subversion Repositories Projects

Rev

Rev 130 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
127 ligi 1
#include "fc.h"
2
 
3
 
4
unsigned char TxBuffer[150];
5
unsigned char _TxBuffer[150];
6
 
140 ligi 7
unsigned char RxBuffer[150];
130 ligi 8
char PrintableRxBuffer[150];
127 ligi 9
 
130 ligi 10
 
11
 
127 ligi 12
int mk_socket;
13
 
130 ligi 14
int status;
127 ligi 15
 
16
void AddCRC(unsigned int wieviele)
17
{
18
  unsigned int tmpCRC = 0,i;
19
  for(i = 0; i < wieviele;i++)
20
    {
21
      tmpCRC += TxBuffer[i];
22
    }
23
  tmpCRC %= 4096;
24
  TxBuffer[i++] = '=' + tmpCRC / 64;
25
  TxBuffer[i++] = '=' + tmpCRC % 64;
26
  TxBuffer[i++] = '\r';
27
}
28
 
29
 
30
void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)
31
{
32
  int status =0;
33
  unsigned int pt = 0;
34
  unsigned char a,b,c;
35
  unsigned char ptr = 0;
36
 
37
  TxBuffer[pt++] = '#';               // Startzeichen
38
  TxBuffer[pt++] = modul;             // Adresse (a=0; b=1,...)
39
  TxBuffer[pt++] = cmd;                 // Commando
40
 
41
  while(len)
42
    {
43
      if(len) { a = snd[ptr++]; len--;} else a = 0;
44
      if(len) { b = snd[ptr++]; len--;} else b = 0;
45
      if(len) { c = snd[ptr++]; len--;} else c = 0;
46
      TxBuffer[pt++] = '=' + (a >> 2);
47
      TxBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
48
      TxBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
49
      TxBuffer[pt++] = '=' + ( c & 0x3f);
50
    }
51
 
52
 
53
 
54
  AddCRC(pt);
55
  printf("Sending to MK %d \n" , pt);
56
 
57
  status = send(mk_socket,"\r" , 1, 0);
58
 
59
 
60
  //  for (c=0;c<pt+2;c++)
61
  // {
62
  status = write(mk_socket,&TxBuffer , pt+3);
130 ligi 63
 
64
 
127 ligi 65
  //   printf("Send to MK %d \n" , TxBuffer[c] );
66
  // }
130 ligi 67
  /* int i=0;
68
  while(TxBuffer[i] !='\r' && i<150)
127 ligi 69
    {
70
    //     TxBuffer[i]='#';
130 ligi 71
    status = send(mk_socket,TxBuffer[i] , 1, 0);
127 ligi 72
    printf(" +%d%c ",i,TxBuffer[i]);
73
    i++;
74
    }
75
 
130 ligi 76
  //  status = send(s,"\r" , 1, 0);
127 ligi 77
  */
130 ligi 78
   status = send(mk_socket,"\r" , 1, 0);
79
   status = send(mk_socket,"\n" , 1, 0);
127 ligi 80
  printf("\n");
81
}
82
 
83
 
130 ligi 84
int rx_last_length;
85
 
140 ligi 86
 
87
void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max)
88
{
89
 unsigned char a,b,c,d;
90
 unsigned char ptr = 0;
91
 unsigned char x,y,z;
92
 while(len)
93
  {
94
   a = RxBuffer[ptrIn++] - '=';
95
   b = RxBuffer[ptrIn++] - '=';
96
   c = RxBuffer[ptrIn++] - '=';
97
   d = RxBuffer[ptrIn++] - '=';
98
   if(ptrIn > max - 2) break;    
99
 
100
   x = (a << 2) | (b >> 4);
101
   y = ((b & 0x0f) << 4) | (c >> 2);
102
   z = ((c & 0x03) << 6) | d;
103
 
104
   if(len--) ptrOut[ptr++] = x; else break;
105
   if(len--) ptrOut[ptr++] = y; else break;
106
   if(len--) ptrOut[ptr++] = z; else break;
107
  }
108
 
109
}
110
 
130 ligi 111
int read_from_mk()
112
{      
113
  char in_char='#';
114
  int count=0;
115
  int r=0;
140 ligi 116
  int i=0;
117
 
130 ligi 118
  printf("starting read\n");
119
  while(in_char!='\n')
120
    {
121
      //      printf("b read\n");
122
      count=read(mk_socket,&in_char,1);
123
      //printf("a read %d\n",count);
124
      if (count!=-1)
125
        {
126
          //  printf("%c\n",in_char);
127
          RxBuffer[r]=in_char;
128
 
129
          if (in_char!=0)
130
            PrintableRxBuffer[r++]=in_char;
131
          else
132
            PrintableRxBuffer[r++]='0';
133
        }
134
 
135
    }
136
  rx_last_length=r;
137
  PrintableRxBuffer[r++]='\0'; // terminate 
138
  printf("done --->%s\n",PrintableRxBuffer);
139
 
140
  if (RxBuffer[2]=='D')
140 ligi 141
    {
142
      debug_sets++;
143
      Decode64((unsigned char *) &DebugOut,sizeof(DebugOut),3,rx_last_length);
144
      printf("decoded FC Debug data height:%d\n",DebugOut.Analog[5]);
145
    }
130 ligi 146
  return 1;
147
}
148
 
149
 
127 ligi 150
int connect_mk_bluetooth(char dest[18])
151
{
152
 
153
  struct sockaddr_rc addr ;
154
 
155
  // allocate a socket
156
  mk_socket = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
157
 
158
  // set the connection parameters (who to connect to)
159
  addr.rc_family = AF_BLUETOOTH;
160
  addr.rc_channel = 1;
161
  str2ba( dest, &addr.rc_bdaddr );
162
 
163
  // connect to server
164
  status = connect(mk_socket, (struct sockaddr *)&addr, sizeof(addr));
165
 
130 ligi 166
  printf("connection status %d\n",status);
127 ligi 167
  return status;
168
 
169
}
170
 
171
 
172
 
130 ligi 173
 
127 ligi 174
int connect_mk_localhost_socket(int port)
175
{
176
 
177
  int status;
178
  struct sockaddr_in sa;
179
 
180
  sa.sin_family = AF_INET;
181
  sa.sin_addr.s_addr = htonl(0x0);
182
  sa.sin_port = htons(port);
183
 
184
  // allocate a socket
185
  //  s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
186
  mk_socket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
187
  //);
188
 
189
  // set the connection parameters (who to connect to)
190
 
191
  // connect to server
192
  //  status = connect(s, (struct sockaddr *)&addr, sizeof(addr));
193
  status = connect(mk_socket,(struct sockaddr*) &sa, sizeof(struct sockaddr_in));
194
 
130 ligi 195
  printf("connection status %d\n",status);
127 ligi 196
  return status;
197
 
198
}
199