{ // *** MINIMUM THROTTLE TO DO CORRECTIONS MAKE THIS 20pts ABOVE YOUR MIN THR STICK ***/ long pitch_output = pids[PID_PITCH_RATE].get_pid(gyroPitch - rcpit, 1); long roll_output = pids[PID_ROLL_RATE].get_pid(gyroRoll - rcroll, 1); long yaw_output = pids[PID_YAW_RATE].get_pid(gyroYaw - rcyaw, 1); hal.rcout->write(MOTOR_FL, rcthr - roll_output - pitch_output); hal.rcout->write(MOTOR_BL, rcthr - roll_output + pitch_output); hal.rcout->write(MOTOR_FR, rcthr + roll_output - pitch_output); hal.rcout->write(MOTOR_BR, rcthr + roll_output + pitch_output); } else { // MOTORS OFF hal.rcout->write(MOTOR_FL, 1000); hal.rcout->write(MOTOR_BL, 1000); hal.rcout->write(MOTOR_FR, 1000); hal.rcout->write(MOTOR_BR, 1000); for(int i=0; i<6; i++) // reset PID integrals whilst on the ground pids[i].reset_I(); }

现在在你的手中要将油门提高20%，然后向前/向后，向左/向右偏转四旋翼，并保证螺旋桨的速度增加或减少。如果四旋翼向一边倾斜， 那一边的螺旋桨就要加速相反一边的则要减速。如果不这样，改变相应的电机信号，（例如，如果俯仰角是错的，在错误的俯仰角之前没有改变信号，四旋翼就会翻滚）。

你可以这样测试你选择的数据，同步你的PID数据，用绳子将四旋翼固定在一个轴上，测试没一个轴的翻转。这是一个非常有用的经验去了解PID是如何运作的，但是不是必须的。这是我评定合适PID数据的示范——我让他以50度每秒翻转。

Video: Rate PIDs only with quad fixed on one axis

四旋翼固定在一个轴线上，测试合适PIDs数据。

现在，我们需要添加偏航（左右航向）的支持。正如你所知，两侧的电机向相反的方向旋转给我们偏航的控制。所以我们要成对的增速或减速，以维持航向正常。

hal.rcout->write(MOTOR_FL, rcthr - roll_output - pitch_output -yaw_output); hal.rcout->write(MOTOR_BL, rcthr - roll_output + pitch_output + yaw_output); hal.rcout->write(MOTOR_FR, rcthr + roll_output - pitch_output + yaw_output); hal.rcout->write(MOTOR_BR, rcthr + roll_output + pitch_output - yaw_output);

这是一个更难的测试项目。你需要提高油门这样才能使它盘旋一点。如果偏航的信号错了四旋翼将自旋。

现在你能让你的四旋翼能够离地几秒钟。如果你觉得适应ACRO模式，你甚至可以驾驶它-请记住这是ACRO模式，不是在arducopter在那里进行自动找平。

如果你的四旋翼飞的偏软，或振荡，那么你需要调整你的Kp常数。如果偏软或如果振荡或如果偏硬，说明你使用了错误的方式 ，尝试打印出PID输出电机命令以纠错，和转动的调试四旋翼（不带电池的连接）。

Stablilised Control

稳定控制

稳定模式与速率模式相近， 只不过其代码高于速率模式：

现在 ，操作手杆位希望四旋翼保持某一个角度，而不是旋转模式。所以我们可以说，如果操作手的杆位是在中心的话，四旋翼现在应该保持20度的俯角，然后：

error = desiredAngle - actualAngle = 0 - 20 = -20

差=希望的角度-实际角度=0—20=—20

在这个实例中，我们需要将差与Kp相乘，得到我们需要的角速率。你一定能注意到，Kp值因为稳定的原因被设定为4.5。所以，如果我们我有-20度的角差的话，就要输出-20*4.5=-90的PID（负数仅意味着方向）。这意味着四旋翼会接收一个每秒-90度的速率恢复水平位置——我们只需要将这个数据早先设置在控制器里。我们的四旋翼会开始倾向水平了，角度差会减少，输出目标速率也会减少，所以四旋翼开始高速，然后低速的回复到水平位置正是我们所希望的。

// our new stab pids float pitch_stab_output = constrain(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250); float roll_stab_output = constrain(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250); float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid((float)rcyaw - yaw, 1), -360, 360); // rate pids from earlier long pitch_output = (long) constrain(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500, 500); long roll_output = (long) constrain(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500); long yaw_output = (long) constrain(pids[PID_YAW_RATE].get_pid(yaw_stab_output - gyroYaw, 1), -500, 500);

现在你的四旋翼应该能够悬停，虽然它可能不稳定或振荡。所以，如果它飞行的动作不太大——现在是时候调整那些PID，主要集中在利率（KP特别）-所指出应当可以。对于速率I级的主题，设定使他们有~ 1俯仰/滚动角和无偏航角。

请注意，偏航并没有像预期的那样，偏航被你的摇杆锁定——所以你会发现摇杆向左偏转45度 航向也会左四旋转45度，当你回到摇杆中心，四旋翼也会返回其偏航中心。目前我们的代码就是这样的，我们可以去除航向稳定控制器，让摆杆直接控制偏航率，但同时如果又一阵风向四旋翼袭来，它将偏航可能漂移而且不会恢复正常。所以，当操作手使用偏航摇杆时我们的直接进入速度控制器，当他放下时，我们使用稳定控制器锁航向以防他偏离。

由于偏航值从180到180，我们需要一个区间定义，横摆角达到181或-181度时将执行一个保护操作。所以在你的代码定义的顶部：

#define wrap_180(x) (x < -180 ? x+360 : (x > 180 ? x - 360: x))

如果你仔细查看你会发现，如果他是-180度，就加360度；如果他是180度就加-360度，我们就让他这样做。

定义全局或稳定变量 float yaw_target = 0; 现在在主loop函数里，我们需要设定操作手控制摇杆时航向是速率控制，无操作时是稳定控制—锁定航向。

float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360); if(abs(rcyaw) > 5) { // if pilot commanding yaw yaw_stab_output = rcyaw; // feed to rate controller (overwriting stab controller output) yaw_target = yaw; // update yaw target } 当四旋翼在地面上没有油门时，你现在也可以设定你的航向目标。 你也可以将其与不分也这么做。

是的，现在航向应该表现正常。但如果你注意的话，你可能会注意到，偏航过几十秒会抖动一下。这可能不会打扰你了，原因是发生是因为虽然你的偏航摇杆还是在中位，无线电抖动则使四旋翼并不总是收到0信号 -它在真值之间抖动造成偏航改变。此外，该mpu6050的偏航传感器也会随着时间的推移（1-2deg /秒）而漂移——你需要用指南针来补偿这种漂移（如果你真的在乎修复它，大多数人没有注意到）。

Final Product - video and full code 最终产品—视频和全部代码

恭喜，你已经一个多直升机创建你的第一个飞行控制器代码！你会发现它比标准的arducopter代码更具挑战性，很多，这是因为arducopter有很多对操作员的输入处理使它更容易飞。提高你的油门到80%和四旋翼将如火箭般直飞向天空，比arducopter实现的更快。警告-不要提高你的油门太接近100%，以留有空间使得控制器能够改变电机速度使得它恢复水平和翻转（你相当容易的实现一个自动降低油门的程序）。