自制小四轴

        在我们前面的章节中我们已经介绍了如何使用MPU6050传感器获取当前小四轴的姿态信息，我们可以得到其3轴的姿态角和角速度，之后就可以根据这些信息来对其进行控制。本质上我们还是采用串级PID对其进行控制，关于PID的控制方法请读者参见《多旋翼无人机-飞行控制》章节，这里我们只是简单的给出具体实现方法：

        对于姿态控制来说我们对3轴姿态角进行控制就是其主控制器，用于控制其实际处于指定的角度上，而角速度则是副控制器用于根据姿态角控制量来控制其角速度，而最终将角速度控制量经过混合控制器输出为4个电机的控制量。

        首先我们来编写一个PID控制函数用于根据状态误差生成控制量：

float ctl_pid(float devi, float devi_pre, float p, float i, float d, float* integral, float integral_limit)
{
	if (integral != NULL)
	{
		*integral += devi * i;
		ctl_value_limit(integral, integral_limit, -integral_limit);
	}

	float val_p = devi * p;
	float val_i = integral == NULL ? 0 : *integral;
	float val_d = (devi - devi_pre) * d;

	return val_p + val_i + val_d;
}

        此函数很清楚的将当前时刻误差devi、上一时刻误差devi_pre，PID参数，积分项integral和积分项限幅integral_limit作为函数的参数，并通过PID算法进行计算最终得到val_p+val_i+val_d的控制量。之后我们再来看一下如何实现PID串级控制器：

while (1)
{
		//读取姿态信息
		if (mpu6050_value(&mpu_value[0], &mpu_value[1], &mpu_value[2], &mpu_value[3], &mpu_value[4], &mpu_value[5], &mpu_value[6], &mpu_value[7], &mpu_value[8]) == 0)
		{
			for (int i = 0; i < 9; i++)
			{
				xyz_value[i] = mpu_value[i] * xyz_value_filter[i] + xyz_value_pre[i] * (1.0 - xyz_value_filter[i]);
				xyz_value_pre[i] = xyz_value[i];
			}
		}
		float x = xyz_value[0];
		float y = xyz_value[1];
		float z = xyz_value[2];
		float gx = xyz_value[3];
		float gy = xyz_value[4];
		float gz = xyz_value[5];
		float t_x = offset_x + x;
		float t_y = offset_y + y;
		float t_z = offset_z + z;
		float t_gx = offset_gx + gx;
		float t_gy = offset_gy + gy;
		float t_gz = offset_gz + gz;
		//已解锁
		if (ctl_arming)
		{
			//角速度期望转为航向角速期望
			yaw_expect_total += ctl_yaw * yaw_expect_rate;
			// [外环PID控制
			//根据角度期望计算角度误差
			float devi_pitch_angle = (-ctl_pitch) * (ctl_angle * ctl_angle_p) - t_x;
			float devi_roll_angle = (-ctl_roll) * (ctl_angle * ctl_angle_p) - t_y;
			float devi_yaw_angle = yaw_expect_total - t_z;
			// PID得到角速度期望
			float ctl_pitch_angle = ctl_pid(devi_pitch_angle, devi_pitch_angle_pre, ctl_param_pitch_roll_angle_p, 0, 0, NULL, 0);
			float ctl_roll_angle = ctl_pid(devi_roll_angle, devi_roll_angle_pre, ctl_param_pitch_roll_angle_p, 0, 0, NULL, 0);
			float ctl_yaw_angle = ctl_pid(devi_yaw_angle, devi_yaw_angle_pre, ctl_param_yaw_angle_p, 0, 0, NULL, 0);
			// 外环PID控制]
			// [内环PID控制
			//根据角速度期望计算角度误差
			float devi_pitch_rate = ctl_pitch_angle - t_gx;
			float devi_roll_rate = ctl_roll_angle - t_gy;
			float devi_yaw_rate = ctl_yaw_angle - t_gz;
			// PID得到控制量
			float ctl_pitch_rate = ctl_pid(devi_pitch_rate, devi_pitch_rate_pre, ctl_param_pitch_roll_rate_p, ctl_param_pitch_roll_rate_i, ctl_param_pitch_roll_rate_d, &ctl_integral_rate_pitch, ctl_thro);
			float ctl_roll_rate = ctl_pid(devi_roll_rate, devi_roll_rate_pre, ctl_param_pitch_roll_rate_p, ctl_param_pitch_roll_rate_i, ctl_param_pitch_roll_rate_d, &ctl_integral_rate_roll, ctl_thro);
			float ctl_yaw_rate = ctl_pid(devi_yaw_rate, devi_yaw_rate_pre, ctl_param_yaw_rate_p, ctl_param_yaw_rate_i, ctl_param_yaw_rate_d, &ctl_integral_rate_yaw, ctl_thro);
			// 内环PID控制]
			// [更新误差项
			devi_pitch_angle_pre = devi_pitch_angle;
			devi_roll_angle_pre = devi_roll_angle;
			devi_pitch_rate_pre = devi_pitch_rate;
			devi_roll_rate_pre = devi_roll_rate;
			devi_yaw_rate_pre = devi_yaw_rate;
			// 更新误差项]
			//混控
			if (ctl_thro < 0.1)
			{
				ctl_lock_zero();
				ctl_offset_z(-z, -gz, -az);
				ctl_offset_alt_press();
				ctl_mixer(0, 0, 0, 0, ctl_motor);
			}
			else
			{
				ctl_mixer(ctl_thro, ctl_pitch_rate, ctl_roll_rate, ctl_yaw_rate, ctl_motor);
			}
		}
		//未解锁
		else
		{
			//未校准时校准按钮被按下
			if (ctl_sw[0] == 1 && ctl_calibrate == 0)
			{
				//开始校准
				ctl_calibrate = 1;
			}
			//校准
			if (ctl_calibrate >= 1)
			{
				ctl_offset(-x, -y, -z, -gx, -gy, -gz, -ax, -ay, -az);
				ctl_calibrate++;
			}
			if (ctl_calibrate > 1000)
			{
				ctl_calibrate = 0;
				ctl_offset_save();
			}
			ctl_lock_zero();
			ctl_mixer(0, 0, 0, 0, ctl_motor);
		}
		ctl_output();
		tk++;
		sleep_ticks(10);
}

        这样我们就完成了串级PID控制器，最后我们再来看看混合控制器是如何实现的：

float ctl_mixer(float ctl_t, float ctl_p, float ctl_r, float ctl_y, float* ctl_motor)
{
	ctl_motor[0] = ctl_t + (ctl_p * sqrt_2_2) - (ctl_r * sqrt_2_2) + ctl_y;
	ctl_motor[1] = ctl_t + (ctl_p * sqrt_2_2) + (ctl_r * sqrt_2_2) - ctl_y;
	ctl_motor[2] = ctl_t - (ctl_p * sqrt_2_2) + (ctl_r * sqrt_2_2) + ctl_y;
	ctl_motor[3] = ctl_t - (ctl_p * sqrt_2_2) - (ctl_r * sqrt_2_2) - ctl_y;

	for (int i = 0; i < 4; i++)
	{
		ctl_value_limit(&ctl_motor[i], 1.0, 0.0);
	}
}

        由于我们是X型四旋翼机，因此我们的俯仰控制量和滚转控制量乘了一个根号2，实际上由于比例作用的原因不乘以根号2也是可以的。我们可以看到：

俯仰控制量ctl_p在电机0和电机1上是加法，在电机2和电机3上是减法；

滚转控制量ctl_r在电机0和电机3上是减法，而在电机1和电机2上是加法；

偏航控制量ctl_y在电机0和电机2上是加法，而在电机1和电机3上是减法。

        这样我们就完成了小四轴的控制程序，当然完整的程序要比我们文章中给出的代码要复杂一些，但原理上都是一样的，最后我们只需要根据实际情况来完成小四轴的PID参数调节工作，希望读者们可以顺利完成这部分内容。我们《自制小四轴》整个系列的文章就至此结束了，感谢大家的支持！

最后我们给出完整的电路设计：下载地址
最后我们给出完整的程序代码：下载地址

注：下载之后将文件的扩展名由.gz修改为.tar.gz再解压即可。