EEPW论坛

做到姿态解算，水平方向的姿态都已经准确了，除了yaw会漂移，另外两个是准确的。

现在做PID出问题了，找不到原因

void PID_Control()
{
	rol_err_last2 = rol_err_last1;
	pit_err_last2 = pit_err_last1;
	yaw_err_last2 = yaw_err_last1;

	rol_err_last1 = rol_err;
	pit_err_last1 = pit_err;
	yaw_err_last1 = yaw_err;
	
	rol_err = angle_x - angle_x_tar;//rollÎó²î
	pit_err = angle_y - angle_y_tar;//pitchÎó²î
	yaw_err = angle_z - angle_z_tar;//yawÎó²î

	//Èý¸öPIDµÄÔöÁ¿Êä³ö
	rol_out += Kp*(rol_err - rol_err_last1) + Ki*(rol_err) + Kd*(rol_err - 2*rol_err_last1 + rol_err_last2);
	pit_out += Kp*(pit_err - pit_err_last1) + Ki*(pit_err) + Kd*(pit_err - 2*pit_err_last1 + pit_err_last2);
	yaw_out += Kp*(yaw_err - yaw_err_last1) + Ki*(yaw_err) + Kd*(yaw_err - 2*yaw_err_last1 + yaw_err_last2);

	if (rol_out < -400) rol_out = -400;
	else if (rol_out > 400) rol_out = 400;

	if (pit_out < -400) pit_out = -400;
	else if (pit_out > 400) pit_out = 400;

	if (yaw_out < -200) yaw_out = -200;
	else if (yaw_out > 200) yaw_out = 200;
	
	/////////////////////////////////////////////////////////
	yaw_out = 0;
	
	if (gas>200)
	{
		//µþ¼ÓÊä³öµ½Êµ¼ÊÓÍÃÅ
		moto1 = gas - rol_out - pit_out + yaw_out;
		moto2 = gas - rol_out + pit_out - yaw_out;
		moto3 = gas + rol_out - pit_out - yaw_out;
		moto4 = gas + rol_out + pit_out + yaw_out;

		if (moto1 < 0) moto1 = 0;
		else if (moto1 > 1000) moto1 = 1000;
		if (moto2 < 0) moto2 = 0;
		else if (moto2 > 1000) moto2 = 1000;
		if (moto3 < 0) moto3 = 0;
		else if (moto3 > 1000) moto3 = 1000;
		if (moto4 < 0) moto4 = 0;
		else if (moto4 > 1000) moto4 = 1000;
	}
	else
	{
		moto1 = 0;
		moto2 = 0;
		moto3 = 0;
		moto4 = 0;
	}
	
	motor_speed((uint16_t)moto1,(uint16_t)moto2,(uint16_t)moto3,(uint16_t)moto4);
}

而且一边偏了之后，不关油门继续上升的话桨叶就打到地面了。

按理说，如果偏向一边，PID是可以将姿态调整回来的，可是现在是它会保持偏着，这是为何？

我取的参数是Kp=1.00,Ki=0.02,Kd=0

不是姿态校准不准的问题，如果只是校准不准，不会往一边越偏越厉害