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)
{
}
}
void ANO_DT::Data_Receive_Anl(u8 *data_buf,u8 num)
{
u8 sum = 0;
for(u8 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; //判断帧头
ano.f.FAILSAFE = 0;
/////////////////////////////////////////////////////////////////////////////////////
if(*(data_buf+2)==0X01)
{
if(*(data_buf+4)==0X01)
mpu6050.Acc_CALIBRATED = 1;
if(*(data_buf+4)==0X02)
mpu6050.Gyro_CALIBRATED = 1;
if(*(data_buf+4)==0X03)
{
mpu6050.Acc_CALIBRATED = 1;
mpu6050.Gyro_CALIBRATED = 1;
}
}
if(*(data_buf+2)==0X02)
{
if(*(data_buf+4)==0X01)
{
f.send_pid1 = 1;
f.send_pid2 = 1;
f.send_pid3 = 1;
f.send_pid4 = 1;
f.send_pid5 = 1;
f.send_pid6 = 1;
}
if(*(data_buf+4)==0X02)
{
}
if(*(data_buf+4)==0XA0) //读取版本信息
{
f.send_version = 1;
}
if(*(data_buf+4)==0XA1) //恢复默认参数
{
fc.PID_Reset();
param.SAVE_PID();
}
}
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);
rc.rawData[AUX1] = (vs16)(*(data_buf+12)<<8)|*(data_buf+13);
rc.rawData[AUX2] = (vs16)(*(data_buf+14)<<8)|*(data_buf+15);
rc.rawData[AUX3] = (vs16)(*(data_buf+16)<<8)|*(data_buf+17);
rc.rawData[AUX4] = (vs16)(*(data_buf+18)<<8)|*(data_buf+19);
}
if(*(data_buf+2)==0X10) //PID1
{
fc.pid[PIDROLL].kP = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5)) / 1000;
fc.pid[PIDROLL].kI = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7)) / 1000;
fc.pid[PIDROLL].kD = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9)) / 1000;
fc.pid[PIDPITCH].kP = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11)) / 1000;
fc.pid[PIDPITCH].kI = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13)) / 1000;
fc.pid[PIDPITCH].kD = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15)) / 1000;
fc.pid[PIDYAW].kP = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17)) / 1000;
fc.pid[PIDYAW].kI = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19)) / 1000;
fc.pid[PIDYAW].kD = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21)) / 1000;
Send_Check(*(data_buf+2),sum);
}
if(*(data_buf+2)==0X11) //PID2
{
fc.pid[PIDANGLE].kP = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5)) / 1000;
fc.pid[PIDANGLE].kI = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7)) / 1000;
fc.pid[PIDANGLE].kD = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9)) / 1000;
fc.pid[PIDMAG].kP = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11)) / 1000;
fc.pid[PIDMAG].kI = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13)) / 1000;
fc.pid[PIDMAG].kD = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15)) / 1000;
fc.pid[PIDVELZ].kP = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17)) / 1000;
fc.pid[PIDVELZ].kI = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19)) / 1000;
fc.pid[PIDVELZ].kD = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21)) / 1000;
Send_Check(*(data_buf+2),sum);
}
if(*(data_buf+2)==0X12) //PID3
{
fc.pid[PIDALT].kP = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5)) / 1000;
fc.pid[PIDALT].kI = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7)) / 1000;
fc.pid[PIDALT].kD = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9)) / 1000;
Send_Check(*(data_buf+2),sum);
param.SAVE_PID();
}
if(*(data_buf+2)==0X13) //PID4
{
Send_Check(*(data_buf+2),sum);
}
if(*(data_buf+2)==0X14) //PID5
{
Send_Check(*(data_buf+2),sum);
}
if(*(data_buf+2)==0X15) //PID6
{
Send_Check(*(data_buf+2),sum);
}
}
static U32 Send_Check(PTR pThis_, PTR pParams, PTR pReturnValue, tAsyncCall *pAsync) {
PTR buffer;
int r;
tSendRecvState *pState = (tSendRecvState*)pAsync->state;
int s = INTERNALCALL_PARAM(0, int);
HEAP_PTR bufferArray = INTERNALCALL_PARAM(4, HEAP_PTR);
U32 ofs = INTERNALCALL_PARAM(8, U32);
U32 size = INTERNALCALL_PARAM(12, U32);
U32 flags = INTERNALCALL_PARAM(16, U32);
U32 *pError = INTERNALCALL_PARAM(20, U32*);
buffer = SystemArray_GetElements(bufferArray) + ofs + pState->count;
r = send(s, buffer, size, flags);
//printf("Send_Check: r=%d\n", r);
if (r >= 0) {
pState->count += r;
if (pState->count >= size) {
// All data sent
*(U32*)pReturnValue = pState->count;
*pError = 0;
return 1;
} else {
// Still more data to come
return 0;
}
} else {
int err = ERRNO;
printf("Send_Check: errno=%d\n", err);
#ifdef WIN32
if (err == WSAEINPROGRESS || err == WSAEWOULDBLOCK) {
#else
if (err == EAGAIN) {
#endif
return 0;
} else {
*(U32*)pReturnValue = pState->count;
*pError = err;
return 1;
}
}
}
tAsyncCall* System_Net_Sockets_Internal_Send(PTR pThis_, PTR pParams, PTR pReturnValue) {
U32 ok;
tAsyncCall *pAsync = TMALLOC(tAsyncCall);
tSendRecvState *pState = TMALLOC(tSendRecvState);
pAsync->sleepTime = -1;
pAsync->checkFn = Receive_Check;
pAsync->state = (PTR)pState;
pState->count = 0;
ok = Send_Check(pThis_, pParams, pReturnValue, pAsync);
if (ok) {
free(pState);
free(pAsync);
return NULL;
} else {
return pAsync;
}
}
void Commander::Data_Receive_Anl(u8 *data_buf,u8 num)
{
u8 sum = 0;
for(u8 i=0;i<(num-1);i++)
sum += *(data_buf+i);
if(!(sum==*(data_buf+num-1))) return; //判断sum
config.f.failsafe = 0;
/////////////////////////////////////////////////////////////////////////////////////
if(*(data_buf+2)==0X01)
{
if(*(data_buf+4)==0X01)
sensor.Acc_CALIBRATED = 1;
if(*(data_buf+4)==0X02)
sensor.Gyro_CALIBRATED = 1;
if(*(data_buf+4)==0X03)
{
sensor.Acc_CALIBRATED = 1;
sensor.Gyro_CALIBRATED = 1;
}
if(*(data_buf+4)==0XA0)
{
config.f.ARMED = 0; //上锁
}
if(*(data_buf+4)==0XA1)
{
config.f.ARMED = 1; //解锁
}
}
if(*(data_buf+2)==0X02)
{
if(*(data_buf+4)==0X01)
{
f.Send_PID1 = 1;
f.Send_PID2 = 1;
f.Send_PID3 = 1;
}
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);
rc.rawData[AUX1] = (vs16)(*(data_buf+12)<<8)|*(data_buf+13);
rc.rawData[AUX2] = (vs16)(*(data_buf+14)<<8)|*(data_buf+15);
rc.rawData[AUX3] = (vs16)(*(data_buf+16)<<8)|*(data_buf+17);
rc.rawData[AUX4] = (vs16)(*(data_buf+18)<<8)|*(data_buf+19);
rc.rawData[AUX5] = (vs16)(*(data_buf+20)<<8)|*(data_buf+21);
rc.rawData[AUX6] = (vs16)(*(data_buf+22)<<8)|*(data_buf+23);
}
if(*(data_buf+2)==0X10) //PID1
{
pid_t pidval;
pidval.kp = (vs16)(*(data_buf+4)<<8)|*(data_buf+5);
pidval.ki = (vs16)(*(data_buf+6)<<8)|*(data_buf+7);
pidval.kd = (vs16)(*(data_buf+8)<<8)|*(data_buf+9);
Params_setRollPid(pidval);
pidval.kp = (vs16)(*(data_buf+10)<<8)|*(data_buf+11);
pidval.ki = (vs16)(*(data_buf+12)<<8)|*(data_buf+13);
pidval.kd = (vs16)(*(data_buf+14)<<8)|*(data_buf+15);
Params_setPitchPid(pidval);
pidval.kp = (vs16)(*(data_buf+16)<<8)|*(data_buf+17);
pidval.ki = (vs16)(*(data_buf+18)<<8)|*(data_buf+19);
pidval.kd = (vs16)(*(data_buf+20)<<8)|*(data_buf+21);
Params_setYawPid(pidval);
Params_Save();
Send_Check(sum);
}
if(*(data_buf+2)==0X11) //PID2
{
pid_t pidval;
pidval.kp = (vs16)(*(data_buf+4)<<8)|*(data_buf+5);
pidval.ki = (vs16)(*(data_buf+6)<<8)|*(data_buf+7);
pidval.kd = (vs16)(*(data_buf+8)<<8)|*(data_buf+9);
Params_setAltPid(pidval);
pidval.kp = (vs16)(*(data_buf+10)<<8)|*(data_buf+11);
pidval.ki = (vs16)(*(data_buf+12)<<8)|*(data_buf+13);
pidval.kd = (vs16)(*(data_buf+14)<<8)|*(data_buf+15);
Params_setLevelPid(pidval);
pidval.kp = (vs16)(*(data_buf+16)<<8)|*(data_buf+17);
pidval.ki = (vs16)(*(data_buf+18)<<8)|*(data_buf+19);
pidval.kd = (vs16)(*(data_buf+20)<<8)|*(data_buf+21);
Params_setMagPid(pidval);
Params_Save();
Send_Check(sum);
}
if(*(data_buf+2)==0X12) //PID3
{
Send_Check(sum);
}
if(*(data_buf+2)==0X13) //PID4
{
Send_Check(sum);
}
if(*(data_buf+2)==0X14) //PID5
{
Send_Check(sum);
}
if(*(data_buf+2)==0X15) //PID6
{
Send_Check(sum);
}
if(*(data_buf+2)==0X16) //OFFSET
{
}
/////////////////////////////////////////////////////////////////////////////////////////////////
if(*(data_buf+2)==0x18)
{
}
}
void ANO_DT::Data_Receive_Anl(u8 *data_buf,u8 num)
{
u8 sum = 0;
for(u8 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; //判断帧头
ano.f.failsafe = 0;
/////////////////////////////////////////////////////////////////////////////////////
if(*(data_buf+2)==0X01)
{
if(*(data_buf+4)==0X01)
mpu6050.Acc_CALIBRATED = 1;//校准加速度
if(*(data_buf+4)==0X02)
mpu6050.Gyro_CALIBRATED = 1;//校准陀螺
if(*(data_buf+4)==0X03)
{
mpu6050.Acc_CALIBRATED = 1;
mpu6050.Gyro_CALIBRATED = 1;
}
}
if(*(data_buf+2)==0X02)
{
if(*(data_buf+4)==0X01)
{
f.Send_PID1 = 1;
f.Send_PID2 = 1;
f.Send_PID3 = 1;
}
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);
rc.rawData[AUX1] = (vs16)(*(data_buf+12)<<8)|*(data_buf+13);
rc.rawData[AUX2] = (vs16)(*(data_buf+14)<<8)|*(data_buf+15);
rc.rawData[AUX3] = (vs16)(*(data_buf+16)<<8)|*(data_buf+17);
rc.rawData[AUX4] = (vs16)(*(data_buf+18)<<8)|*(data_buf+19);
}
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);
param.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)
{
}
}
