在圆点博士小四轴2014版代码里，我们只使用到PD参数。
首先我们来看PID中的比例控制。跟上一节模型提到的一样，比例是针对误差的控制。
首先我们获取小四轴当前角度。
bs004_angle_cur_pitch=bs004_imu_pitch;
bs004_angle_cur_roll =bs004_imu_roll;
把当前角度和目标角度相减，就可以得到角度偏差。
bs004_angle_err_pitch=bs004_angle_cur_pitch-bs004_angle_target_pitch;
bs004_angle_err_roll=bs004_angle_cur_roll-bs004_angle_target_roll;
然后进行比例控制：
bs004_fly_m1=bs004_fly_m1
+bs004_pitch_p*bs004_angle_err_pitch
-bs004_roll_p *bs004_angle_err_roll
-bs004_yaw_p*bs004_angle_err_yaw;
bs004_fly_m2=bs004_fly_m2
-bs004_pitch_p*bs004_angle_err_pitch
-bs004_roll_p *bs004_angle_err_roll
+bs004_yaw_p*bs004_angle_err_yaw;
bs004_fly_m3=bs004_fly_m3
-bs004_pitch_p*bs004_angle_err_pitch
+bs004_roll_p *bs004_angle_err_roll
-bs004_yaw_p*bs004_angle_err_yaw;
bs004_fly_m4=bs004_fly_m4
+bs004_pitch_p*bs004_angle_err_pitch
+bs004_roll_p *bs004_angle_err_roll
+bs004_yaw_p*bs004_angle_err_yaw;
在上一节模型中，我们提到PID中的微分控制针对的是角度变化而进行的控制。
所以我们首先要得到当前角度和上一次角度的差异。
bs004_angle_dif_pitch=bs004_angle_cur_pitch-bs004_angle_last_pitch;
bs004_angle_dif_roll =bs004_angle_cur_roll-bs004_angle_last_roll;
bs004_angle_dif_yaw =bs004_angle_last_yaw-bs004_angle_cur_yaw;
然后进行微分控制：
bs004_fly_m1=bs004_fly_m1
+bs004_pitch_d*bs004_angle_dif_pitch
-bs004_roll_d *bs004_angle_dif_roll
-bs004_yaw_d*bs004_angle_dif_yaw;
bs004_fly_m2=bs004_fly_m2
-bs004_pitch_d*bs004_angle_dif_pitch
-bs004_roll_d *bs004_angle_dif_roll
+bs004_yaw_d*bs004_angle_dif_yaw;
bs004_fly_m3=bs004_fly_m3
-bs004_pitch_d*bs004_angle_dif_pitch
+bs004_roll_d *bs004_angle_dif_roll
-bs004_yaw_d*bs004_angle_dif_yaw;
bs004_fly_m4=bs004_fly_m4
+bs004_pitch_d*bs004_angle_dif_pitch
+bs004_roll_d *bs004_angle_dif_roll
+bs004_yaw_d*bs004_angle_dif_yaw;
从上面的描述我们可以看出，小四轴的PID控制还是比较简单的。