void Control(T_float_angle *att_in,T_int16_xyz *gyr_in, T_RC_Data *rc_in, u8 armed)
{ //加速度 角速度
static u8 first =1;
T_float_angle angle;
#ifndef USE_USART1
if(first)
{
init_pid();
first=0;
}
#endif
//**************roll轴指向***********************************************************
angle.rol = att_in->rol - (rc_in->ROLL-1500)/25;//误差
if(rc_in->THROTTLE<1200)
rol_i=0;
else
rol_i += angle.rol;//误差和
LIMIT(-2000, rol_i,2000);//积分限幅
PID_ROL.pout = PID_ROL.P * angle.rol; //比例项 = 比例*误差
PID_ROL.dout = -PID_ROL.D * gyr_in->Y; //微分项 = 微分*变化量
//PID_ROL.iout = PID_ROL.I * PID_ROL.dout; //积分项 = 积分*误差和
PID_ROL.iout = PID_ROL.I * rol_i;
PID_ROL.OUT = (-PID_ROL.pout)-PID_ROL.iout +PID_ROL.dout;//
//**************pith轴指向***********************************************************
angle.pit = att_in->pit + (rc_in->PITCH-1500)/25;
if(rc_in->THROTTLE<1200)
pit_i=0;
else
pit_i += angle.pit;//误差和
LIMIT(-2000, pit_i,2000);
PID_PIT.pout = PID_PIT.P * angle.pit; //比例项 = 比例*误差
PID_PIT.dout = PID_PIT.D * gyr_in->X; //微分项 = 微分*变化量
PID_PIT.iout = PID_PIT.I * pit_i; //积分项 = 积分*误差和
//PID_PIT.iout = PID_PIT.I *PID_PIT.dout ;
PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
//**************yaw轴指向***********************************************************
if(rc_in->YAW<1400||rc_in->YAW>1600)
{gyr_in->Z=gyr_in->Z+(rc_in->YAW-1500)*2;}
if(rc_in->THROTTLE<1200)
yaw_p=0;
else
yaw_p+=gyr_in->Z*0.0609756f*0.002f;// +(Rc_Get.YAW-1500)*30
if(yaw_p>20)
yaw_p=20;
if(yaw_p<-20)
yaw_p=-20;
PID_YAW.pout = PID_YAW.P*yaw_p;
PID_YAW.dout = PID_YAW.D * gyr_in->Z;
PID_YAW.iout = PID_YAW.I *PID_YAW.dout ;
PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;
//**************电机控制***********************************************************
if(rc_in->THROTTLE>1200&&armed)//armed判断解锁
{ //横滚 //俯仰 //航向
// Moto_PWM_1 = rc_in->THROTTLE - 1000 - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
// Moto_PWM_2 = rc_in->THROTTLE - 1000 - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
// Moto_PWM_3 = rc_in->THROTTLE - 1000 + PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
// Moto_PWM_4 = rc_in->THROTTLE - 1000 + PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
Moto_PWM_1 = rc_in->THROTTLE - 1000 + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;//反逻辑 电机下沉端用加号
Moto_PWM_2 = rc_in->THROTTLE - 1000 + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
Moto_PWM_3 = rc_in->THROTTLE - 1000 - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
Moto_PWM_4 = rc_in->THROTTLE - 1000 - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
}
else
{
Moto_PWM_1 = 0;
Moto_PWM_2 = 0;
Moto_PWM_3 = 0;
Moto_PWM_4 = 0;
}
//**************更新PWM***********************************************************
Moto_PwmRflash(Moto_PWM_1,Moto_PWM_2,Moto_PWM_3,Moto_PWM_4);
}
void Control(T_float_angle *att_in,T_int16_xyz *gyr_in, T_RC_Data *rc_in, u8 armed)
{
T_float_angle angle;
angle.rol = att_in->rol - (rc_in->ROLL-1504)/12;
angle.pit = att_in->pit + (rc_in->PITCH-1501)/12;
rol_i += angle.rol;
if(rol_i>2000)
rol_i=2000;
if(rol_i<-2000)
rol_i=-2000;
PID_ROL.pout = PID_ROL.P * angle.rol;
PID_ROL.dout = -PID_ROL.D * gyr_in->X;
PID_ROL.iout = PID_ROL.I * PID_ROL.dout;
pit_i += angle.pit;
if(pit_i>2000)
pit_i=2000;
if(pit_i<-2000)
pit_i=-2000;
PID_PIT.pout = PID_PIT.P * angle.pit;
PID_PIT.dout = PID_PIT.D * gyr_in->Y;
PID_PIT.iout = PID_PIT.I * pit_i;
if(rc_in->YAW<1480||rc_in->YAW>1520)
{
gyr_in->Z=gyr_in->Z+(rc_in->YAW-1500)*10;
}
yaw_p+=gyr_in->Z*0.0609756f*0.002f;// +(Rc_Get.YAW-1500)*30
if(yaw_p>20)
yaw_p=20;
if(yaw_p<-20)
yaw_p=-20;
PID_YAW.pout=PID_YAW.P*yaw_p;
PID_YAW.dout = PID_YAW.D * gyr_in->Z;
if(rc_in->THROTTLE<1025)
{
pit_i=0;
rol_i=0;
yaw_p=0;
}
PID_ROL.OUT = (-PID_ROL.pout)-PID_ROL.iout +PID_ROL.dout;//
PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;
if(rc_in->THROTTLE>1050&&armed)
{
Moto_PWM_1 = rc_in->THROTTLE - PID_ROL.OUT - PID_PIT.OUT; //+ PID_YAW.OUT;
Moto_PWM_2 = rc_in->THROTTLE + PID_ROL.OUT - PID_PIT.OUT; //- PID_YAW.OUT;
Moto_PWM_3 = rc_in->THROTTLE + PID_ROL.OUT + PID_PIT.OUT; //+ PID_YAW.OUT;
Moto_PWM_4 = rc_in->THROTTLE - PID_ROL.OUT + PID_PIT.OUT; //- PID_YAW.OUT;
//将PWM输出限定在1000~2000以内
if(Moto_PWM_1>2000) Moto_PWM_1 = 2000;
if(Moto_PWM_2>2000) Moto_PWM_2 = 2000;
if(Moto_PWM_3>2000) Moto_PWM_3 = 2000;
if(Moto_PWM_4>2000) Moto_PWM_4 = 2000;
if(Moto_PWM_1<1000) Moto_PWM_1 = 1000;
if(Moto_PWM_2<1000) Moto_PWM_2 = 1000;
if(Moto_PWM_3<1000) Moto_PWM_3 = 1000;
if(Moto_PWM_4<1000) Moto_PWM_4 = 1000;
}
else
{
Moto_PWM_1 = 1000;
Moto_PWM_2 = 1000;
Moto_PWM_3 = 1000;
Moto_PWM_4 = 1000;
}
Moto_PwmRflash(Moto_PWM_1,Moto_PWM_2,Moto_PWM_3,Moto_PWM_4);
}
void Balance(T_float_angle *att_in, S_INT16_XYZ *gyr_in, S_INT16_XYZ *acc_in, T_RC_Data *Rc_in, T_Control *Ctl)
{
extern u16 Alt_ultrasonic;
Balance_Data(att_in, gyr_in, acc_in, Rc_in, Ctl);//赋值
// if(rc_in->THROTTLE>1700){Throttle_IN=1700-RC_FUN_ZERO;}
// else {Throttle_IN = rc_in->THROTTLE - RC_FUN_ZERO;}//油门-1200
time--;
Yaw_Control();
Rol_Control();
Pit_Control();
ALT_Control(ALT_Set);
if(qifei==1){
Throttle_IN = rc_in->THROTTLE - RC_FUN_ZERO;
Balance_Throttle = Throttle_OUT = Throttle_IN;
//if(Throttle_OUT>700)Throttle_OUT=700;
Throttle_OUT=Alt_ultrasonic/5+690;//730
if(Throttle_OUT>800)Throttle_OUT=800;}//800
/*****************************************************
/CONTROL
*****************************************************/
// ROL
// +
// MOTO1_PWM /|\ MOTO1_PWM
// |
// + ----------- - PITCH
// |
// MOTO1_PWM | MOTO1_PWM
// -
//
if (rc_in->THROTTLE > 1300 && ctl->ARMED && att_in->pit < 40 && att_in->rol < 40 && att_in->pit > -40 && att_in->rol > -40)
{
MOTO1_PWM = (int32_t)((int)Throttle_OUT + PID_ROL.OUT + PID_PIT.OUT- PID_YAW.OUT+PID_ALT.OUT);//
MOTO2_PWM = (int32_t)((int)Throttle_OUT - PID_ROL.OUT + PID_PIT.OUT+ PID_YAW.OUT+PID_ALT.OUT);
MOTO3_PWM = (int32_t)((int)Throttle_OUT - PID_ROL.OUT - PID_PIT.OUT- PID_YAW.OUT+PID_ALT.OUT);
MOTO4_PWM = (int32_t)((int)Throttle_OUT + PID_ROL.OUT - PID_PIT.OUT+ PID_YAW.OUT+PID_ALT.OUT);
qifei=1;
}
else
{
if(qifei==0){
att_in->yaw = 0;
pit_i = 0;
rol_i = 0;
yaw_i = 0;
alt_i = 0;
qifei = 0;
MOTO1_PWM=MOTO2_PWM=MOTO3_PWM=MOTO4_PWM=0;}
if(qifei==1&&Alt_ultrasonic>100&& ctl->ARMED&&att_in->pit < 40 && att_in->rol < 40 && att_in->pit > -40 && att_in->rol > -40){
p--;
if(p==0)
{down--;p=5;}
MOTO1_PWM = (int32_t)(down + PID_ROL.OUT + PID_PIT.OUT- PID_YAW.OUT);//
MOTO2_PWM = (int32_t)(down - PID_ROL.OUT + PID_PIT.OUT+ PID_YAW.OUT);
MOTO3_PWM = (int32_t)(down - PID_ROL.OUT - PID_PIT.OUT- PID_YAW.OUT);
MOTO4_PWM = (int32_t)(down + PID_ROL.OUT - PID_PIT.OUT+ PID_YAW.OUT);
}
if(qifei==1&&Alt_ultrasonic<100)
{
qifei=0;
MOTO1_PWM=MOTO2_PWM=MOTO3_PWM=MOTO4_PWM=0;down=800;ctl->ARMED=0;
}
}
Moto_PwmRflash(MOTO1_PWM, MOTO2_PWM, MOTO3_PWM, MOTO4_PWM);
}
void fly_control(void)
{
LED0 = ~LED0;
count_time ++;
if(count_time == GET_DATA_TIME){
// printf("read the data ...\r\n");
MPU6050_Dataanl(); //update data
PID_ROL.pout = PID_ROL.P * (roll - BALANCE_STATUS);
PID_ROL.iout = PID_ROL.I * (MPU6050_GYRO_LAST.ROLL - BALANCE_STATUS*COUNT_NUM);
PID_ROL.dout = PID_ROL.D * (roll - MPU6050_GYRO_LAST.ROLL_SECOND);
PID_PIT.pout = PID_PIT.P * (pitch - BALANCE_STATUS);
PID_PIT.iout = PID_PIT.I * (MPU6050_GYRO_LAST.PITCH - BALANCE_STATUS*COUNT_NUM);
PID_PIT.dout = PID_PIT.D * (pitch - MPU6050_GYRO_LAST.PITCH_SECOND);
PID_YAW.pout = PID_YAW.P * (roll - BALANCE_STATUS);
PID_YAW.iout = PID_YAW.I * (MPU6050_GYRO_LAST.YAW - BALANCE_STATUS*COUNT_NUM);
PID_YAW.dout = PID_YAW.D * (yaw - MPU6050_GYRO_LAST.YAW_SECOND);
PID_ROL.OUT = PID_ROL.pout + PID_ROL.iout + PID_ROL.dout;
PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;
if(Rc_Get.THROTTLE>1200)
{
/***
moto1 = Rc_Get.THROTTLE - PID_ROL.OUT;
moto2 = Rc_Get.THROTTLE + PID_PIT.OUT;
moto3 = Rc_Get.THROTTLE + PID_ROL.OUT;
moto4 = Rc_Get.THROTTLE - PID_PIT.OUT;
***/
moto1 = Rc_Get.THROTTLE - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
moto2 = Rc_Get.THROTTLE - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
moto3 = Rc_Get.THROTTLE + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;
moto4 = Rc_Get.THROTTLE + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
printf("moto1 = %d ,moto2 = %d,moto3 = %d,moto4 = %d \r\n",moto1,moto2,moto3,moto4);
printf("roll = %f ,pitch = %f ,yaw = %f \r\n",roll,pitch,yaw);
}
else
{
moto1 = Rc_Get.THROTTLE;
moto2 = Rc_Get.THROTTLE;
moto3 = Rc_Get.THROTTLE;
moto4 = Rc_Get.THROTTLE;
}
if(1 == Rc_Get.STATUS_OK){
Moto_PwmRflash(moto1,moto2,moto3,moto4);
}else {
Moto_PwmRflash(1000,1000,1000,1000);
}
MPU6050_GYRO_LAST.ROLL = 0;
MPU6050_GYRO_LAST.PITCH = 0;
MPU6050_GYRO_LAST.YAW = 0;
count_time = 0;
}
}
