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) { } }