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; } }