#include <sys\stat.h>
#include <stdio.h>
#include <stdlib.h>
//#include <unistd.h>
#include <string.h>
#include <fcntl.h>
#include <io.h>
#include <dir.h>
#include <windows.h>
#include <conio.h>
unsigned char ring;
unsigned char _data[8];
unsigned char mav_rbuf[300];
unsigned char mav_0buf[300];
unsigned char mav_wbuf[300];
unsigned char *buf;
FILE *handle1, *handle2;
unsigned long file_size;
char s[10];
//extern int _fmode;
struct ffblk ffblk;
int done;
int flagmulti;
HANDLE hCom;
unsigned long writen;
char flagauto;
//-----------------------------------------------------------------------
unsigned char m_dlin[256]= {
9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0,
0, 0, 0, 0, 20, 2, 25, 23, 30,101, 22, 26, 16, 14, 28, 32,
28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3,
13, 12, 37, 0, 0, 0, 27, 25, 0, 0, 0, 0, 0, 68, 26,185,
181, 42, 6, 4, 0, 11, 18, 0, 0, 37, 20, 35, 33, 3, 0, 0,
0, 22, 39, 37, 53, 51, 53, 51, 0, 28, 56, 42, 33, 0, 0, 0,
0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 44, 64, 84, 9,254, 16,
0, 36, 44, 64, 22, 6, 14, 12, 97, 2, 2,113, 35, 6, 79, 35,
35, 22, 13,255, 14, 18, 43, 8, 22, 14, 36, 43, 41, 0, 0, 0,
0, 0, 0, 36, 60, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0,254, 36, 30, 18, 18, 51, 9, 0};
/*
9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0,
0, 0, 0, 0, 20, 2, 25, 23, 30,101, 22, 26, 16, 14, 28, 32,
28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3,
13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46,
36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0,
0, 0, 0, 0, 26, 32, 32, 20, 32, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0};
*/
unsigned char m_crc[256]= {
50,124,137, 0,237,217,104,119, 0, 0, 0, 89, 0, 0, 0, 0,
0, 0, 0, 0,214,159,220,168, 24, 23,170,144, 67,115, 39,246,
185,104,237,244,222,212, 9,254,230, 28, 28,132,221,232, 11,153,
41, 39, 78, 0, 0, 0, 15, 3, 0, 0, 0, 0, 0,153,183, 51,
82,118,148, 21, 0,243,124, 0, 0, 38, 20,158,152,143, 0, 0,
0,106, 49, 22,143,140, 5,150, 0,231,183, 63, 54, 0, 0, 0,
0, 0, 0, 0,175,102,158,208, 56, 93,138,108, 32,185, 84, 34,
0,124,237, 4, 76,128, 56,116,134,237,203,250, 87,203,220, 25,
226, 46, 29,223, 85, 6,229,203, 1,195,109,168,181, 0, 0, 0,
0, 0, 0,154,178, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 8,204, 49,170, 44, 83, 46, 0};
/*
50,124,137, 0,237,217,104,119, 0, 0, 0, 89, 0, 0, 0, 0,
0, 0, 0, 0,214,159,220,168, 24, 23,170,144, 67,115, 39,246,
185,104,237,244,222,212, 9,254,230, 28, 28,132,221,232, 11,153,
41, 39,214,223,141, 33, 15, 3,100, 24,239,238, 30,240,183,130,
130, 0,148, 21, 0, 52,124, 0, 0, 0, 20, 0,152,143, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0,231,183, 63, 54, 0, 0, 0,
0, 0, 0, 0,175,102,158,208, 56, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0,204, 49,170, 44, 83, 46, 0};
*/
//---------------------------------------------------------------
unsigned char mav_size,mav_ring,mav_sys,mav_comp,mav_pct;
unsigned char syst,comp,conf,sats,fix;
unsigned short int vx,vy,vz,hdg,aspeed_i,gspeed_i,cmd;
unsigned long ipos,lon_l,lat_l,alt_l,ralt_l;
unsigned long *adrl;
unsigned short int *adri;
float *adrf;
float pitch_f,roll_f,yaw_f,aspeed_f,gspeed_f;
float param1_f,param2_f,param3_f,param4_f,param5_f,param6_f,param7_f;
//===========================================================================
// 頡滄曉彙番撲
//===========================================================================
//氟摘琯� �.�. 溜菴隋�� 蝠慚 菴瞄,
//甦蝕憤��錙� 皚豢� 廈錙閎靦 頑擋獗�
//�.�. 頑菊擋靦 愿 甦蝕憤��錙擋
unsigned int mav_accum;
void crc_calc(char data)
{
unsigned char tmp;
/*Accumulate one byte of data into the CRC*/
tmp = data ^ (unsigned char)(mav_accum &0xff);
tmp = tmp ^ (tmp<<4);
mav_accum = (mav_accum>>8);
mav_accum = mav_accum ^ (tmp<<8);
mav_accum = mav_accum ^ (tmp <<3);
mav_accum = mav_accum ^ (tmp>>4);
}
//===================================================================
unsigned char read_byte()
{
if(fread(_data, 1,1,handle1) !=1) //鞣錶粳 閱撼灘 8 菴瞄
{fprintf(stderr,"\nread error"); exit(1);}
ipos++;
return _data[0];
}
//==================================================================================
unsigned char GET_PACKET()
{unsigned char i,dan;
new_packet:
while(read_byte()!= 0xFE);
mav_accum=0xFFFF;
mav_size=read_byte(); crc_calc(mav_size);
mav_ring=read_byte(); crc_calc(mav_ring);
mav_sys =read_byte(); crc_calc(mav_sys);
mav_comp=read_byte(); crc_calc(mav_comp);
mav_pct=read_byte(); crc_calc(mav_pct);
if(m_dlin[mav_pct] !=mav_size) goto new_packet;
for (i=0; i<mav_size;i++)
{
dan=read_byte();
mav_rbuf[i]=dan;
crc_calc(dan);
}
crc_calc(m_crc[mav_pct]);
dan=read_byte();
if(dan!=(unsigned char) (mav_accum & 0xFF)) goto new_packet;
dan=read_byte();
if(dan!=(unsigned char)(mav_accum >> 8)) goto new_packet;
return mav_pct;
}
//=================================================================================
void MAV_SEND(char *buf ,char size)
{int i;
ring++;
buf[2]=ring;
mav_accum=0xFFFF;
for(i=1;i<size;i++)
{
crc_calc(buf[i]);
}
crc_calc(m_crc[buf[5]]);
buf[size]=mav_accum & 0xFF;
buf[size+1]=mav_accum >>8;
i=WriteFile(hCom,buf,size+2,&writen,NULL); //藝粳 摸 � 頡暸
if(i==0)
{
printf("\r\nWrite2com error %d",i);
CloseHandle(hCom);
exit(0);
}
}
//===========================================================================
int KBD_PART()
{int s;
if (kbhit())
{
s=getch();
return s;
}
else
{return 0;}
}
//======================================================================
void printhelp()
{
printf("\n'H' - this help");
printf("\n' ' - se
评论0