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