WebSVN - Projects - Diff - Rev 130 and 140


#include "fc.h"
 
 
unsigned char TxBuffer[150];
unsigned char _TxBuffer[150];
 
unsigned char RxBuffer[150];
char PrintableRxBuffer[150];
 
 
 
int mk_socket;
 
int status;
 
void AddCRC(unsigned int wieviele)
{
  unsigned int tmpCRC = 0,i;
  for(i = 0; i < wieviele;i++)
    {
      tmpCRC += TxBuffer[i];
    }
  tmpCRC %= 4096;
  TxBuffer[i++] = '=' + tmpCRC / 64;
  TxBuffer[i++] = '=' + tmpCRC % 64;
  TxBuffer[i++] = '\r';
}
 
 
void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)
{
  int status =0;
  unsigned int pt = 0;
  unsigned char a,b,c;
  unsigned char ptr = 0;
 
  TxBuffer[pt++] = '#';               // Startzeichen
  TxBuffer[pt++] = modul;             // Adresse (a=0; b=1,...)
  TxBuffer[pt++] = cmd;                 // Commando
 
  while(len)
    {
      if(len) { a = snd[ptr++]; len--;} else a = 0;
      if(len) { b = snd[ptr++]; len--;} else b = 0;
      if(len) { c = snd[ptr++]; len--;} else c = 0;
      TxBuffer[pt++] = '=' + (a >> 2);
      TxBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
      TxBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
      TxBuffer[pt++] = '=' + ( c & 0x3f);
    }
 
 
 
  AddCRC(pt);
  printf("Sending to MK %d \n" , pt);
 
  status = send(mk_socket,"\r" , 1, 0);
 
 
  //  for (c=0;c<pt+2;c++)
  // {
  status = write(mk_socket,&TxBuffer , pt+3);
 
 
  //   printf("Send to MK %d \n" , TxBuffer[c] );
  // }
  /* int i=0;
  while(TxBuffer[i] !='\r' && i<150)
    {
    //     TxBuffer[i]='#';
    status = send(mk_socket,TxBuffer[i] , 1, 0);
    printf(" +%d%c ",i,TxBuffer[i]);
    i++;
    }
 
  //  status = send(s,"\r" , 1, 0);
  */
   status = send(mk_socket,"\r" , 1, 0);
   status = send(mk_socket,"\n" , 1, 0);
  printf("\n");
}
 
 
int rx_last_length;
 
 
void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max)
{
 unsigned char a,b,c,d;
 unsigned char ptr = 0;
 unsigned char x,y,z;
 while(len)
  {
   a = RxBuffer[ptrIn++] - '=';
   b = RxBuffer[ptrIn++] - '=';
   c = RxBuffer[ptrIn++] - '=';
   d = RxBuffer[ptrIn++] - '=';
   if(ptrIn > max - 2) break;    
 
   x = (a << 2) | (b >> 4);
   y = ((b & 0x0f) << 4) | (c >> 2);
   z = ((c & 0x03) << 6) | d;
 
   if(len--) ptrOut[ptr++] = x; else break;
   if(len--) ptrOut[ptr++] = y; else break;
   if(len--) ptrOut[ptr++] = z; else break;
  }
 
}
 
int read_from_mk()
{      
  char in_char='#';
  int count=0;
  int r=0;
  int i=0;
 
  printf("starting read\n");
  while(in_char!='\n')
    {
      //      printf("b read\n");
      count=read(mk_socket,&in_char,1);
      //printf("a read %d\n",count);
      if (count!=-1)
        {
          //  printf("%c\n",in_char);
          RxBuffer[r]=in_char;
     
          if (in_char!=0)
            PrintableRxBuffer[r++]=in_char;
          else
            PrintableRxBuffer[r++]='0';
        }
 
    }
  rx_last_length=r;
  PrintableRxBuffer[r++]='\0'; // terminate 
  printf("done --->%s\n",PrintableRxBuffer);
 
  if (RxBuffer[2]=='D')
    {
      debug_sets++;
      Decode64((unsigned char *) &DebugOut,sizeof(DebugOut),3,rx_last_length);
      printf("decoded FC Debug data height:%d\n",DebugOut.Analog[5]);
    }
  return 1;
}
 
 
int connect_mk_bluetooth(char dest[18])
{
 
  struct sockaddr_rc addr ;
 
  // allocate a socket
  mk_socket = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
 
  // set the connection parameters (who to connect to)
  addr.rc_family = AF_BLUETOOTH;
  addr.rc_channel = 1;
  str2ba( dest, &addr.rc_bdaddr );
 
  // connect to server
  status = connect(mk_socket, (struct sockaddr *)&addr, sizeof(addr));
 
  printf("connection status %d\n",status);
  return status;
 
}
 
 
 
 
int connect_mk_localhost_socket(int port)
{
 
  int status;
  struct sockaddr_in sa;
 
  sa.sin_family = AF_INET;
  sa.sin_addr.s_addr = htonl(0x0);
  sa.sin_port = htons(port);
 
  // allocate a socket
  //  s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
  mk_socket = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
  //);
 
  // set the connection parameters (who to connect to)
 
  // connect to server
  //  status = connect(s, (struct sockaddr *)&addr, sizeof(addr));
  status = connect(mk_socket,(struct sockaddr*) &sa, sizeof(struct sockaddr_in));
 
  printf("connection status %d\n",status);
  return status;
 
}
 
 

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

Subversion Repositories Projects

(root)/Riddim/fc.c - Rev 130 → 140