// Set constants, given PID controller void set_pid_constants(double P, double I, double D, double Alpha, Controller_PID *pid) { PID_Reset(pid); pid->Const_P = P; pid->Const_I = I; pid->Const_D = D; pid->Alpha = Alpha; }
void PID_Init(void) { pid_X.KP = 0.0001; //0.24; pid_X.KI = 0.0005; //0.9; pid_X.KD = 0.0001; //0.04; pid_X.SetPoint = LEAN_FORWARD_ANGLE;// Angle of upper body pid_X.LastUpdate = 0; PID_Reset(); }
void pidctrl::Init() { yawRate = 80; //重置PID参数 PID_Reset(); motor_dev = rt_device_find("motors"); if(motor_dev == RT_NULL) { rt_kprintf("can not find motors!\n"); return; } rt_device_open(motor_dev,RT_DEVICE_FLAG_RDWR); }
ANO_FlyControl::ANO_FlyControl() { //重置PID参数 PID_Reset(); }
//锁定 rt_err_t arm(rt_int32_t addtion) { rt_err_t err; if (armed) return RT_EOK; PID_Reset(&p_angle_pid); PID_Reset(&p_rate_pid); PID_Reset(&r_angle_pid); PID_Reset(&r_rate_pid); PID_Reset(&y_angle_pid); PID_Reset(&y_rate_pid); PID_Reset(&h_v_pid); PID_Reset(&h_d_pid); PID_Reset(&x_d_pid); PID_Reset(&x_v_pid); PID_Reset(&y_d_pid); PID_Reset(&y_v_pid); if ((err = check_safe(SAFE_MPU6050 | addtion)) == RT_EOK) { yaw_exp = ahrs.degree_yaw; armed = RT_TRUE; rt_kprintf("armed.\n"); return RT_EOK; } else { rt_kprintf("pre armed fail: 0x%02X.\n", err); return err; } }