void RadioPanel::updateDisplay()
{
static unsigned char buf[23];
if (!handle)
{
return;
}
VESSEL *vessel = oapiGetFocusInterface(); // Get current vessel
if (vessel!=NULL) // check if pointer is valid
{
buf[0]=0x00;
// parse nav1
parseNavData(vessel, 0, &(buf[1]));
// parse nav2
parseNavData(vessel, 1, &(buf[11]));
int res = hid_send_feature_report(handle, buf, 23);
}
}
/**
* receive navigation data
* 接收导航数据线程函数
*
* @解释：根据开发手册P41，需要完成类似于三次握手协议，先发一个激活数据报，
* 然后配置无人机，才能让无人机向基站发送数据报
* 可以使用WireShark2 检测数据报是否发到本机上
* @author: blyde
* @update: 2015.7.11
**/
int Ardrone::receiveNavData()
{
if( !isInit_)
return C_ERROR;
SOCKET socketNav = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
sockaddr_in navSin;
navSin.sin_family = AF_INET;
navSin.sin_port = htons(NAVDATA_PORT);
navSin.sin_addr.s_addr = inet_addr(ARDRONE_IP);
int lenNavSin = sizeof(navSin);
// 激活指令发往NAVDATA_PORT 端口
const char trigger[4] = { 0x01, 0x00, 0x00, 0x00 };
int ret = sendto(socketNav, trigger, strlen(trigger), 0, (sockaddr *)&navSin, lenNavSin);
if (ret == SOCKET_ERROR)
{
std::cout<<"__send trigger flag error"<<std::endl;
return C_ERROR;
}
printf_s("Sent trigger flag to UDP port : %d \n", NAVDATA_PORT);
// 配置指令发往AT_PORT 端口
char initCmd[100];
sprintf_s(initCmd, "AT*CONFIG=%d,\"general:navdata_demo\",\"TRUE\"\r", nextSeq());
ret = send_at_cmd(initCmd);
if ( ret == C_ERROR)
{
std::cout<<"__send initCmd error"<<std::endl;
return C_ERROR;
}
// 接收数据包
MemoryLibrary::Buffer navDataBuffer; // 二进制数据缓冲区
char recv[1024] = { 0 }; // 数据包接收数组
int lenRecv = 0;
int delay = 0;
std::cout<<"__receiveNavData start "<<std::endl;
while ( true)
{
// 获取导航数据包
lenRecv = recvfrom(socketNav, recv, 1024, 0, (struct sockaddr*)&navSin, &lenNavSin);
delay++;
if (delay >= 5)
{
delay = 0;
printf("received %d bytes\n", lenRecv);
// 分析数据包，得到导航数据
navDataBuffer.Set(recv, lenRecv);
ret = parseNavData(navDataBuffer);
if ( ret == C_ERROR)
{
std::cout<<"__parseNavData() error"<<std::endl;
return C_ERROR;
}
}
}
}
