void Commander::Data_Exchange(void)
{
static u8 cnt = 0;
switch(cnt)
{
case 1:
f.Send_RCData = 1;
break;
case 2:
f.Send_MotoPwm = 1;
break;
case 30:
cnt = 0;
break;
default:
if(cnt%3)
f.Send_Senser = 1;
else
f.Send_Status = 1;
}
cnt++;
if(f.Send_Status){
f.Send_Status = 0;
Send_Status();
}
if(f.Send_Senser){
f.Send_Senser = 0;
Send_Senser();
}
if(f.Send_RCData){
f.Send_RCData = 0;
Send_RCData();
}
if(f.Send_MotoPwm){
f.Send_MotoPwm = 0;
Send_MotoPWM();
}
if(f.Send_PID1){
f.Send_PID1 = 0;
Send_PID1();
}
if(f.Send_PID2){
f.Send_PID2 = 0;
Send_PID2();
}
}
void Data_Exchange(void)
{
static int cnt = 0;
switch(cnt)
{
case SEND_STATES:
Send_Status();
cnt++;
break;
case SEND_SENSER:
Send_Senser();
cnt++;
break;
case SEND_PID1:
Send_PID1();
cnt++;
break;
case SEND_PID2:
Send_PID2();
cnt++;
break;
case SEND_RC:
Send_RCData();
cnt++;
break;
case SEND_MOTORPWM:
Send_MotoPWM();
cnt++;
break;
case SEND_DEBUG:
Send_DEBUG();
cnt++;
break;
default:
cnt=0;
break;
}
}
void Data_Receive_Anl(u8 *data_buf,u8 num)
{
u8 sum = 0;
u8 i;
for(i=0;i<(num-1);i++)
sum += *(data_buf+i);
if(!(sum==*(data_buf+num-1))) return; //判断sum
if(!(*(data_buf)==0xAA && *(data_buf+1)==0xAF)) return; //判断帧头
/////////////////////////////////////////////////////////////////////////////////////
if(*(data_buf+2)==0X01)
{
if(*(data_buf+4)==0X01)
Acc_CALIBRATED = 1;
if(*(data_buf+4)==0X02)
Gyro_CALIBRATED = 1;
if(*(data_buf+4)==0X03)
{
Acc_CALIBRATED = 1;
Gyro_CALIBRATED = 1;
}
}
if(*(data_buf+2)==0X02)
{
if(*(data_buf+4)==0X01)
{
Send_PID1();
Send_PID2();
}
if(*(data_buf+4)==0X02)
{
}
}
/*
if(*(data_buf+2)==0X03)
{
rc.rawData[THROTTLE] = (vs16)(*(data_buf+4)<<8)|*(data_buf+5);
rc.rawData[YAW] = (vs16)(*(data_buf+6)<<8)|*(data_buf+7);
rc.rawData[ROLL] = (vs16)(*(data_buf+8)<<8)|*(data_buf+9);
rc.rawData[PITCH] = (vs16)(*(data_buf+10)<<8)|*(data_buf+11);
}
*/
if(*(data_buf+2)==0X10) //PID1
{
fc.pid[PIDROLL].kP = (vs16)(*(data_buf+4)<<8)|*(data_buf+5);
fc.pid[PIDROLL].kI = (vs16)(*(data_buf+6)<<8)|*(data_buf+7);
fc.pid[PIDROLL].kD = (vs16)(*(data_buf+8)<<8)|*(data_buf+9);
fc.pid[PIDPITCH].kP = (vs16)(*(data_buf+10)<<8)|*(data_buf+11);
fc.pid[PIDPITCH].kI = (vs16)(*(data_buf+12)<<8)|*(data_buf+13);
fc.pid[PIDPITCH].kD = (vs16)(*(data_buf+14)<<8)|*(data_buf+15);
fc.pid[PIDYAW].kP = (vs16)(*(data_buf+16)<<8)|*(data_buf+17);
fc.pid[PIDYAW].kI = (vs16)(*(data_buf+18)<<8)|*(data_buf+19);
fc.pid[PIDYAW].kD = (vs16)(*(data_buf+20)<<8)|*(data_buf+21);
Send_Check(sum);
}
if(*(data_buf+2)==0X11) //PID2
{
fc.pid[PIDALT].kP = (vs16)(*(data_buf+4)<<8)|*(data_buf+5);
fc.pid[PIDALT].kI = (vs16)(*(data_buf+6)<<8)|*(data_buf+7);
fc.pid[PIDALT].kD = (vs16)(*(data_buf+8)<<8)|*(data_buf+9);
fc.pid[PIDLEVEL].kP = (vs16)(*(data_buf+10)<<8)|*(data_buf+11);
fc.pid[PIDLEVEL].kI = (vs16)(*(data_buf+12)<<8)|*(data_buf+13);
fc.pid[PIDLEVEL].kD = (vs16)(*(data_buf+14)<<8)|*(data_buf+15);
fc.pid[PIDMAG].kP = (vs16)(*(data_buf+16)<<8)|*(data_buf+17);
fc.pid[PIDMAG].kI = (vs16)(*(data_buf+18)<<8)|*(data_buf+19);
fc.pid[PIDMAG].kD = (vs16)(*(data_buf+20)<<8)|*(data_buf+21);
Send_Check(sum);
}
if(*(data_buf+2)==0X12) //PID3
{
Send_Check(sum);
SAVE_PID();
}
if(*(data_buf+2)==0X12) //PID4
{
Send_Check(sum);
}
if(*(data_buf+2)==0X12) //PID5
{
Send_Check(sum);
}
if(*(data_buf+2)==0X12) //PID6
{
Send_Check(sum);
}
if(*(data_buf+2)==0X16) //OFFSET
{
}
/////////////////////////////////////////////////////////////////////////////////////////////////
if(*(data_buf+2)==0x18)
{
}
}
