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