急！根据通信协议写的飞控姿态上传函数在上位机上未得到正确数据

KevinBlake · 发表于 2016-8-1 21:00:03

最近在根据匿名给的V4.06通信协议写飞控姿态传输函数，也是第一次根据通信协议（见附件）写函数
但是运行的结果是：无论怎么转动6050.上位机观察到的roll pitch yaw几乎都是很小的角度，并且我没有把高度等量写进去，所以高度这个值也在乱跳
求助各位大虾，我的代码哪里有不对的地方，应该如何修改呢？

代码如下：

[mw_shl_code=c,true]void ANO_DT_Send_Status(s16 rol,s16 pit,s16 yaw)
{
u8 _cnt=0;
u16 _temp;

data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;

_temp = rol;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = pit;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = yaw;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);

data_to_send[3] = _cnt-4;

u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send;
data_to_send[_cnt++] = sum;

ANO_DT_Send_Data(data_to_send, _cnt);
}
[/mw_shl_code]