void Flow_Mavlink(unsigned char data)
{
/*
红色的是起始标志位（stx），在v1.0版本中以“FE”作为起始标志。这个标志位在mavlink消息帧接收端进行消息解码时有用处。
第二个格子代表的是灰色部分（payload，称作有效载荷，要用的数据在有效载荷里面）的字节长度（len），范围从0到255之间。在mavlink消息帧接收端可以用它和实际收到的有效载荷的长度比较，以验证有效载荷的长度是否正确。
第三个格子代表的是本次消息帧的序号（seq），每次发完一个消息，这个字节的内容会加1，加到255后会从0重新开始。这个序号用于mavlink消息帧接收端计算消息丢失比例用的，相当于是信号强度。
第四个格子代表了发送本条消息帧的设备的系统编号（sys），使用PIXHAWK刷PX4固件时默认的系统编号为1，用于mavlink消息帧接收端识别是哪个设备发来的消息。
第五个格子代表了发送本条消息帧的设备的单元编号（comp），使用PIXHAWK刷PX4固件时默认的单元编号为50，用于mavlink消息帧接收端识别是设备的哪个单元发来的消息（暂时没什么用） 。
第六个格子代表了有效载荷中消息包的编号（msg），注意它和序号是不同的，这个字节很重要，mavlink消息帧接收端要根据这个编号来确定有效载荷里到底放了什么消息包并根据编号选择对应的方式来处理有效载荷里的信息包。
26*/
static unsigned char s_flow=0,data_cnt=0;
unsigned char cnt_offset=0;
unsigned char get_one_fame=0;
char floattobyte[4];
switch(s_flow)
{
case 0: if(data==0xFE)
s_flow=1;
break;
case 1: if(data==0x1A)
{ s_flow=2;}
else
s_flow=0;
break;
case 2:
if(data_cnt<4)
{s_flow=2; FLOW_STATE[data_cnt++]=data;}
else
{data_cnt=0;s_flow=3;flow_buf[data_cnt++]=data;}
break;
case 3:
if(FLOW_STATE[3]==100)
{
if(data_cnt<26)
{s_flow=3; flow_buf[data_cnt++]=data;}
else
{data_cnt=0;s_flow=4;}
if(data_cnt<26) break;
}
else
{
data_cnt=0;s_flow=0;break;
}
case 4:get_one_fame=1;s_flow=0;data_cnt=0;break;
default:s_flow=0;data_cnt=0;break;
}//--end of s_uart
if(get_one_fame)
{
MyFlow.time_sec=(((unsigned long)flow_buf[7])<<64)|((unsigned long)flow_buf[6]<<56)|((unsigned long)flow_buf[5]<<48)|((unsigned long)flow_buf[4]<<40)
|((unsigned long)flow_buf[3]<<32)|((unsigned long)flow_buf[2]<<16)|((unsigned long)flow_buf[1]<<8)|((unsigned long)flow_buf[0]);
floattobyte[0]=flow_buf[8];
floattobyte[1]=flow_buf[9];
floattobyte[2]=flow_buf[10];
floattobyte[3]=flow_buf[11];
MyFlow.flow_comp_x =BytesToFloat((unsigned char *)floattobyte);
floattobyte[0]=flow_buf[12];
floattobyte[1]=flow_buf[13];
floattobyte[2]=flow_buf[14];
floattobyte[3]=flow_buf[15];
MyFlow.flow_comp_y =BytesToFloat((unsigned char *)floattobyte);
floattobyte[0]=flow_buf[16];
floattobyte[1]=flow_buf[17];
floattobyte[2]=flow_buf[18];
floattobyte[3]=flow_buf[19];
MyFlow.hight=BytesToFloat((unsigned char *)floattobyte);//ground_distance float Ground distance in m. Positive value: distance known. Negative value: Unknown distance
MyFlow.flow_x=(short)((flow_buf[20])|(flow_buf[21]<<8));
MyFlow.flow_y=(short)((flow_buf[22])|(flow_buf[23]<<8));
MyFlow.id=flow_buf[24];
MyFlow.quality=flow_buf[25]; //Optical flow quality / confidence. 0: bad, 255: maximum quality
}
}
void Connection::CommunicationInterpretation(const char *pData)
{
//CLog("type %d", pData[1]);
switch (pData[1]) {
case CommunicationType_Search:
{
int dataLength = pData[2] + pData[3] * 0x0100;
int ipLength = dataLength - 2;
unsigned short port = pData[4] + pData[5] * 0x0100;
char ip[ipLength];
for (int i = 0; i < ipLength; i++) {
ip[i] = pData[6 + i];
}
bool isMyIp = (0 == strcmp(CSocket::GetIP(), (const char *)&ip));
bool isMyPort = m_port == port;
if (!isMyIp || !isMyPort) {
SearchResponse(ip, port);
}
}
break;
case CommunicationType_SearchResponse:
{
RemoveRetryConnection(CommunicationType_Search);
int dataLength = pData[2] + pData[3] * 0x0100;
if (1 == dataLength) {
// とりあえず無視。
}
else {
m_clientNo = pData[4] + pData[5] * 0x0100;
m_serverPort = pData[6] + pData[7] * 0x0100;
int ipLength = dataLength - 4;
m_pServerIP = new char[ipLength];
for (int i = 0; i < ipLength; i++) {
m_pServerIP[i] = pData[8 + i];
}
const char *pSelfIP = CSocket::GetIP();
char *pIP = new char[strlen(pSelfIP)];
memcpy(pIP, pSelfIP, strlen(pSelfIP));
m_pClients[SERVER_NO] = new ConnectionClient(m_pServerIP, m_serverPort);
m_pClients[m_clientNo] = new ConnectionClient(pIP, m_port);
m_pDelegate->DidDetectServer();
}
}
break;
case CommunicationType_Entry:
{
int dataLength = pData[2] + pData[3] * 0x0100;
int clientNo = pData[4] + pData[5] * 0x0100;
if (m_clientNo != clientNo) {
bool isInvalidClientNo = (0 > clientNo || MAX_MULTI_PLAYER < clientNo || (IsServer() && 0 == m_pClients[clientNo]) || IsClient() && 0 != m_pClients[clientNo]);
if (!isInvalidClientNo) {
int floatSize = (dataLength - 2) / 2;
CPoint point = CPoint(BytesToFloat(&pData[6]), BytesToFloat(&pData[6 + floatSize]));
ToLittleEndian((char *)&point.x, floatSize);
ToLittleEndian((char *)&point.y, floatSize);
if (IsClient()) {
m_pClients[clientNo] = new ConnectionClient();
}
m_pDelegate->NewPlayerEntry(clientNo, point);
}
if (IsServer()) {
EntryResponse(!isInvalidClientNo, clientNo);
}
}
}
break;
case CommunicationType_EntryResponse:
{
m_isReady = true;
int dataLength = pData[2] + pData[3] * 0x0100;
//int isEntry = pData[4];
int floatSize = pData[5];
if (2 < dataLength) {
int temporaryLength = 0;
while (dataLength > (temporaryLength + 2)) {
int clientNo = pData[6 + temporaryLength] + pData[7 + temporaryLength] * 0x0100;
if (m_clientNo == clientNo) {
continue;
}
CPoint point;
memcpy(&point.x, &pData[8 + temporaryLength], floatSize);
memcpy(&point.y, &pData[8 + temporaryLength + floatSize], floatSize);
m_pDelegate->NewPlayerEntry(clientNo, point);
if (IsClient()) {
m_pClients[clientNo] = new ConnectionClient();
}
temporaryLength += 2 + (floatSize * 2);
}
}
m_pDelegate->DidStartClient(m_clientNo);
}
break;
case CommunicationType_Move:
{
int dataLength = pData[2] + pData[3] * 0x0100;
int clientNo = pData[4] + pData[5] * 0x0100;
if (m_clientNo != clientNo) {
bool isInvalidClientNo = (0 > clientNo || MAX_MULTI_PLAYER < clientNo || 0 == m_pClients[clientNo]);
if (!isInvalidClientNo) {
int floatSize = (dataLength - 2) / 2;
CPoint point = CPoint(BytesToFloat(&pData[6]), BytesToFloat(&pData[6 + floatSize]));
ToLittleEndian((char *)&point.x, floatSize);
ToLittleEndian((char *)&point.y, floatSize);
m_pDelegate->UpdatePlayerPosition(clientNo, point);
}
}
}
break;
case CommunicationType_Exit:
{
int clientNo = pData[4] + pData[5] * 0x0100;
if (m_clientNo != clientNo) {
bool isInvalidClientNo = (0 > clientNo || MAX_MULTI_PLAYER < clientNo || 0 == m_pClients[clientNo]);
if (!isInvalidClientNo) {
m_pDelegate->ExitPlayer(clientNo);
delete m_pClients[clientNo];
m_pClients[clientNo] = 0;
}
}
}
break;
default:
break;
}
}
