void ANO_Param::Init(void)
{
if(READ_FirstInitFlag()!= FirstInitFlag) //板子从未初始化
{
SAVE_PID();
}
READ_CONF();
SAVE_FirstInitFlag();
}
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)
{
}
}
