void Rc_Fun(T_RC_Data *rc_in,T_RC_Control *rc_ct)
{
static u8 cnt_arm=0,cnt_fun=0;
if(rc_in->THROTTLE < RC_FUN_MIN && rc_in->YAW < RC_FUN_MIN)
{
cnt_arm++;
if(cnt_arm==200)
{
cnt_arm=0;
rc_ct->ARMED = 1;
}
}
else if(rc_in->THROTTLE < RC_FUN_MIN && rc_in->YAW > RC_FUN_MAX)
{
cnt_arm++;
if(cnt_arm==200)
{
cnt_arm=0;
rc_ct->ARMED = 0;
}
}
else
cnt_arm = 0;
if(rc_ct->ARMED==1)
return;
if(rc_in->THROTTLE < RC_FUN_MIN && rc_in->ROLL < RC_FUN_MIN)
{
cnt_fun++;
if(cnt_fun==200)
{
cnt_fun = 0;
MPU6050_CalOff_Acc();
}
}
else if(rc_in->THROTTLE < RC_FUN_MIN && rc_in->ROLL > RC_FUN_MAX)
{
cnt_fun++;
if(cnt_fun==200)
{
cnt_fun = 0;
MPU6050_CalOff_Gyr();
}
}
else
cnt_fun = 0;
}
void Data_Receive_Anl(u8 *data_buf, u8 num)
{
vs16 rc_value_temp;
u8 sum = 0;
//Sys_sPrintf(Printf_USART, data_buf, num);
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; //
/////////////////////////////////////////////////////////////////////////////////////
if (*(data_buf + 2) == 0X01)
{
if (*(data_buf + 4) == 0X01)
MPU6050_CalOff_Acc();
if (*(data_buf + 4) == 0X02)
MPU6050_CalOff_Gyr();
if (*(data_buf + 4) == 0X03)
{
MPU6050_CalOff_Acc();
MPU6050_CalOff_Gyr();
}
if (*(data_buf + 4) == 0X04)
;//Cal_Compass();
if (*(data_buf + 4) == 0X05)
;//MS5611_CalOffset();
}
if (*(data_buf + 2) == 0X02)
{
if (*(data_buf + 4) == 0X01)
{
Send.PID1 = 1;
Send.PID2 = 1;
Send.PID3 = 1;
Send.PID4 = 1;
Send.PID5 = 1;
Send.PID6 = 1;
}
if (*(data_buf + 4) == 0X02)
Send.Offset = 1;
}
if (*(data_buf + 2) == 0X10) //PID1
{
PID_ROL.P = (float)((vs16)(*(data_buf + 4 ) << 8) | *(data_buf + 5 )) / PID_ROL_P_MULTIPLYING;
PID_ROL.I = (float)((vs16)(*(data_buf + 6 ) << 8) | *(data_buf + 7 )) / PID_ROL_I_MULTIPLYING;
PID_ROL.D = (float)((vs16)(*(data_buf + 8 ) << 8) | *(data_buf + 9 )) / PID_ROL_D_MULTIPLYING;
PID_PIT.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_PIT_P_MULTIPLYING;
PID_PIT.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_PIT_I_MULTIPLYING;
PID_PIT.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_PIT_D_MULTIPLYING;
PID_YAW.P = (float)((vs16)(*(data_buf + 16) << 8) | *(data_buf + 17)) / PID_YAW_P_MULTIPLYING;
PID_YAW.I = (float)((vs16)(*(data_buf + 18) << 8) | *(data_buf + 19)) / PID_YAW_I_MULTIPLYING;
PID_YAW.D = (float)((vs16)(*(data_buf + 20) << 8) | *(data_buf + 21)) / PID_YAW_D_MULTIPLYING;
Data_Send_Check(sum);//
Data_Save(1);
Send.PID1 = 1;
}
if (*(data_buf + 2) == 0X11) //PID2
{
PID_ALT.P = (float)((vs16)(*(data_buf + 4 ) << 8) | *(data_buf + 5 )) / PID_ALT_P_MULTIPLYING ;
PID_ALT.I = (float)((vs16)(*(data_buf + 6 ) << 8) | *(data_buf + 7 )) / PID_ALT_I_MULTIPLYING ;
PID_ALT.D = (float)((vs16)(*(data_buf + 8 ) << 8) | *(data_buf + 9 )) / PID_ALT_D_MULTIPLYING ;
PID_POS.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_POS_P_MULTIPLYING ;
PID_POS.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_POS_I_MULTIPLYING ;
PID_POS.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_POS_D_MULTIPLYING ;
PID_PID_1.P = (float)((vs16)(*(data_buf + 16) << 8) | *(data_buf + 17)) / PID_PID_1_P_MULTIPLYING;
PID_PID_1.I = (float)((vs16)(*(data_buf + 18) << 8) | *(data_buf + 19)) / PID_PID_1_I_MULTIPLYING;
PID_PID_1.D = (float)((vs16)(*(data_buf + 20) << 8) | *(data_buf + 21)) / PID_PID_1_D_MULTIPLYING;
Send.PID2 = 1;
Data_Send_Check(sum);
}
if (*(data_buf + 2) == 0X12) //PID3
{
PID_PID_2.P = (float)((vs16)(*(data_buf + 4 ) << 8) | *(data_buf + 5 )) / PID_PID_2_P_MULTIPLYING;
PID_PID_2.I = (float)((vs16)(*(data_buf + 6 ) << 8) | *(data_buf + 7 )) / PID_PID_2_I_MULTIPLYING;
PID_PID_2.D = (float)((vs16)(*(data_buf + 8 ) << 8) | *(data_buf + 9 )) / PID_PID_2_D_MULTIPLYING;
PID_PID_3.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_PID_3_P_MULTIPLYING;
PID_PID_3.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_PID_3_I_MULTIPLYING;
PID_PID_3.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_PID_3_D_MULTIPLYING;
PID_PID_4.P = (float)((vs16)(*(data_buf + 16) << 8) | *(data_buf + 17)) / PID_PID_4_P_MULTIPLYING;
PID_PID_4.I = (float)((vs16)(*(data_buf + 18) << 8) | *(data_buf + 19)) / PID_PID_4_I_MULTIPLYING;
PID_PID_4.D = (float)((vs16)(*(data_buf + 20) << 8) | *(data_buf + 21)) / PID_PID_4_D_MULTIPLYING;
Send.PID3 = 1;
Data_Send_Check(sum);
Data_Save(1);
}
if (*(data_buf + 2) == 0X13) //PID4
{
PID_PID_5.P = (float)((vs16)(*(data_buf + 4 ) << 8) | *(data_buf + 5 )) / PID_PID_5_P_MULTIPLYING;
PID_PID_5.I = (float)((vs16)(*(data_buf + 6 ) << 8) | *(data_buf + 7 )) / PID_PID_5_I_MULTIPLYING;
PID_PID_5.D = (float)((vs16)(*(data_buf + 8 ) << 8) | *(data_buf + 9 )) / PID_PID_5_D_MULTIPLYING;
PID_PID_6.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_PID_6_P_MULTIPLYING;
PID_PID_6.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_PID_6_I_MULTIPLYING;
PID_PID_6.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_PID_6_D_MULTIPLYING;
PID_PID_7.P = (float)((vs16)(*(data_buf + 16) << 8) | *(data_buf + 17)) / PID_PID_7_P_MULTIPLYING;
PID_PID_7.I = (float)((vs16)(*(data_buf + 18) << 8) | *(data_buf + 19)) / PID_PID_7_I_MULTIPLYING;
PID_PID_7.D = (float)((vs16)(*(data_buf + 20) << 8) | *(data_buf + 21)) / PID_PID_7_D_MULTIPLYING;
Data_Send_Check(sum);
}
if (*(data_buf + 2) == 0X14) //PID5
{
PID_PID_8.P = (float)((vs16)(*(data_buf + 4) << 8 ) | *(data_buf + 5 )) / PID_PID_8_P_MULTIPLYING ;
PID_PID_8.I = (float)((vs16)(*(data_buf + 6) << 8 ) | *(data_buf + 7 )) / PID_PID_8_I_MULTIPLYING ;
PID_PID_8.D = (float)((vs16)(*(data_buf + 8) << 8 ) | *(data_buf + 9 )) / PID_PID_8_D_MULTIPLYING ;
PID_PID_9.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_PID_9_P_MULTIPLYING ;
PID_PID_9.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_PID_9_I_MULTIPLYING ;
PID_PID_9.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_PID_9_D_MULTIPLYING ;
PID_PID_10.P = (float)((vs16)(*(data_buf + 16) << 8) | *(data_buf + 17)) / PID_PID_10_P_MULTIPLYING;
PID_PID_10.I = (float)((vs16)(*(data_buf + 18) << 8) | *(data_buf + 19)) / PID_PID_10_I_MULTIPLYING;
PID_PID_10.D = (float)((vs16)(*(data_buf + 20) << 8) | *(data_buf + 21)) / PID_PID_10_D_MULTIPLYING;
Data_Send_Check(sum);
}
if (*(data_buf + 2) == 0X15) //PID6
{
PID_PID_11.P = (float)((vs16)(*(data_buf + 4) << 8) | *(data_buf + 5)) / PID_PID_11_P_MULTIPLYING;
PID_PID_11.I = (float)((vs16)(*(data_buf + 6) << 8) | *(data_buf + 7)) / PID_PID_11_I_MULTIPLYING;
PID_PID_11.D = (float)((vs16)(*(data_buf + 8) << 8) | *(data_buf + 9)) / PID_PID_11_D_MULTIPLYING;
PID_PID_12.P = (float)((vs16)(*(data_buf + 10) << 8) | *(data_buf + 11)) / PID_PID_12_P_MULTIPLYING;
PID_PID_12.I = (float)((vs16)(*(data_buf + 12) << 8) | *(data_buf + 13)) / PID_PID_12_I_MULTIPLYING;
PID_PID_12.D = (float)((vs16)(*(data_buf + 14) << 8) | *(data_buf + 15)) / PID_PID_12_D_MULTIPLYING;
Data_Send_Check(sum);
}
if (*(data_buf + 2) == 0X16) //OFFSET
{
AngleOffset_Rol = (float)((vs16)(*(data_buf + 4) << 8) | *(data_buf + 5)) / 1000;
AngleOffset_Pit = (float)((vs16)(*(data_buf + 6) << 8) | *(data_buf + 7)) / 1000;
// Data_Save();
}
/////////////////////////////////////////////////////////////////////////////////////////////////
if (*(data_buf + 2) == 0x03) //
{
}
Ex_Anl();
}
void Data_Receive_Anl(u8 *data_buf,u8 num)
{
vs16 rc_value_temp;
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; //ÅжÏÖ¡Í·
/////////////////////////////////////////////////////////////////////////////////////
if(*(data_buf+2)==0X01)
{
if(*(data_buf+4)==0X01)
MPU6050_CalOff_Acc();
if(*(data_buf+4)==0X02)
MPU6050_CalOff_Gyr();
if(*(data_buf+4)==0X03)
{MPU6050_CalOff_Acc();MPU6050_CalOff_Gyr();}
// if(*(data_buf+4)==0X04)
// Cal_Compass();
// if(*(data_buf+4)==0X05)
// MS5611_CalOffset();
}
if(*(data_buf+2)==0X02)
{
if(*(data_buf+4)==0X01)
{
Send_PID1 = 1;
Send_PID2 = 1;
Send_PID3 = 1;
Send_PID4 = 1;
Send_PID5 = 1;
Send_PID6 = 1;
}
if(*(data_buf+4)==0X02)
Send_Offset = 1;
}
if(*(data_buf+2)==0X10) //PID1
{
PID_ROL.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_ROL.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/1000;
PID_ROL.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_PIT.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_PIT.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/1000;
PID_PIT.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
PID_YAW.P = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17))/100;
PID_YAW.I = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19))/100;
PID_YAW.D = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21))/100;
Data_Send_Check(sum);
}
if(*(data_buf+2)==0X11) //PID2
{
PID_ALT.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_ALT.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/100;
PID_ALT.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_POS.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_POS.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/100;
PID_POS.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
PID_PID_1.P = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17))/100;
PID_PID_1.I = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19))/100;
PID_PID_1.D = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21))/100;
Data_Send_Check(sum);
}
if(*(data_buf+2)==0X12) //PID3
{
PID_PID_2.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_PID_2.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/100;
PID_PID_2.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_PID_3.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_PID_3.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/100;
PID_PID_3.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
PID_PID_4.P = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17))/100;
PID_PID_4.I = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19))/100;
PID_PID_4.D = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21))/100;
Data_Send_Check(sum);
}
if(*(data_buf+2)==0X13) //PID4
{
PID_PID_5.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_PID_5.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/100;
PID_PID_5.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_PID_6.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_PID_6.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/100;
PID_PID_6.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
PID_PID_7.P = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17))/100;
PID_PID_7.I = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19))/100;
PID_PID_7.D = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21))/100;
Data_Send_Check(sum);
}
if(*(data_buf+2)==0X14) //PID5
{
PID_PID_8.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_PID_8.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/100;
PID_PID_8.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_PID_9.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_PID_9.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/100;
PID_PID_9.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
PID_PID_10.P = (float)((vs16)(*(data_buf+16)<<8)|*(data_buf+17))/100;
PID_PID_10.I = (float)((vs16)(*(data_buf+18)<<8)|*(data_buf+19))/100;
PID_PID_10.D = (float)((vs16)(*(data_buf+20)<<8)|*(data_buf+21))/100;
Data_Send_Check(sum);
}
if(*(data_buf+2)==0X15) //PID6
{
PID_PID_11.P = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/100;
PID_PID_11.I = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/100;
PID_PID_11.D = (float)((vs16)(*(data_buf+8)<<8)|*(data_buf+9))/100;
PID_PID_12.P = (float)((vs16)(*(data_buf+10)<<8)|*(data_buf+11))/100;
PID_PID_12.I = (float)((vs16)(*(data_buf+12)<<8)|*(data_buf+13))/100;
PID_PID_12.D = (float)((vs16)(*(data_buf+14)<<8)|*(data_buf+15))/100;
Data_Send_Check(sum);
EE_SAVE_PID();
}
if(*(data_buf+2)==0X16) //OFFSET
{
AngleOffset_Rol = (float)((vs16)(*(data_buf+4)<<8)|*(data_buf+5))/1000;
AngleOffset_Pit = (float)((vs16)(*(data_buf+6)<<8)|*(data_buf+7))/1000;
}
/////////////////////////////////////////////////////////////////////////////////////////////////
if(*(data_buf+2)==0x03) //ÅжϹ¦ÄÜ×Ö,Ϊң¿ØÊý¾Ý
{
Rc_D.THROTTLE = (vs16)(*(data_buf+4)<<8)|*(data_buf+5);
Rc_D.YAW = (vs16)(*(data_buf+6)<<8)|*(data_buf+7);
Rc_D.ROLL = (vs16)(*(data_buf+8)<<8)|*(data_buf+9);
Rc_D.PITCH = (vs16)(*(data_buf+10)<<8)|*(data_buf+11);
/////////////////////////////////////////////////////////////////////////
Rc_D.AUX1 = (vs16)(*(data_buf+12)<<8)|*(data_buf+13);
Rc_D.AUX2 = (vs16)(*(data_buf+14)<<8)|*(data_buf+15);
Rc_D.AUX3 = (vs16)(*(data_buf+16)<<8)|*(data_buf+17);
PID_ROL.P = PID_PIT.P = PID_YAW.P = (float)(Rc_D.AUX1) / 100;
PID_ROL.I = PID_PIT.I = (float)(Rc_D.AUX2) / 1000;
PID_ROL.D = PID_PIT.D = (float)(Rc_D.AUX3) / 100;
PID_YAW.D = 0.05;
////////////////////////////////////////////////////////////////////////
Rc_D.AUX4 = (vs16)(*(data_buf+18)<<8)|*(data_buf+19);
// Rc_D.AUX5 = (vs16)(*(data_buf+20)<<8)|*(data_buf+21);
Rc_D.AUX6 = (vs16)(*(data_buf+22)<<8)|*(data_buf+23);
Rc_Fun(&Rc_D,&Rc_C);
// if(Rc_D.AUX6>0 && Rc_D.AUX6<5)
// {
// AUX6_Flag++;
// if(AUX6_Flag>75)
// {
// AUX6_Flag=0;
// Fun_Flag=Rc_D.AUX6;
// }
// }
// else
// AUX6_Flag=0;
}
}
