我的loop（）函数现在看起来像，更换为测量最大最小杆位的Map功能。我们也会改变函数类型，变为long型，以支持负数。
long rcthr, rcyaw, rcpit, rcroll; // Variables to store rc input rcthr = channels[2]; rcyaw = map(channels[3], 1068, 1915, -150, 150); rcpit = map(channels[1], 1077, 1915, -45, 45); rcroll = map(channels[0], 1090, 1913, -45, 45);
俯仰在杆位向前是是负的，翻滚或偏航在杆位向左是负的。如果实际操控中不对，请调整到正确位置。
你现在应该输出这些值，并在串行接口上面观察数据。理想状态下，当杆位在中间的时候，这些值应该为零（除了 thr 变量）。调试最大最小值，直到合适为止。这些数据会有一些扰动（在真值上下浮动）因为摇杆是模拟信号输出，所以会有1度到2度的偏差。一旦你能够将你的四旋翼飞起来的时候，你就会考虑回到这里介绍均值滤波器。
确保向前倾斜，滚动和偏航为左时，数值为负数——如果不是的，请将Map函数前改为负函数。当然你也要确保当你增加油门值时，实际油门值也会增加。
Controlling the motors 控制电机
电机是由电调来控制的，他们的工作脉宽大约有1000us和2000us和无线接收机一样——脉宽为1000us意味着无，而脉宽为2000us意味着有。电调被设定为接受50Hz的信号，但是大多数电机将5-10个储存的值平均后，再发送给电机。这种模式也可以在四旋翼上使用，如果将平均滤波效应最小化，四旋翼会表现的更好。因此，APM底层库的脉冲工作在490Hz，这意味着5到10个脉冲将会快速被平均，很大程度上减小了滤波效应。
在setup（）函数中，可以这样输出： hal.rcout->set_freq(0xF, 490); hal.rcout->enable_mask(0xFF);
在你引用之后，让我们定义一下每个Mapping输出的电机名字——mapping 的输出口名与四旋翼（Arducopter）使用的一样，只不过是从零开始而已。（编程中从0开始命名，所以四个电机的命名是0到3，四旋翼四个电机则是1到4） #define MOTOR_FL 2 // Front left #define MOTOR_FR 0 // Front right #define MOTOR_BL 1 // back left #define MOTOR_BR 3 // back right 你的loop函数里，读取无线电信号后，将这个无线电阀值直接输出给电机 hal.rcout->write(MOTOR_FR, rcthr); 你现在可以编程你的四旋翼并实验他，注意一定是不带螺旋桨的。 缓慢的提高阀值和前右电机的旋转速度。再说一次，如果所有电机带了螺旋桨，都会旋转起来，但是四旋翼还是会坠毁。我们需要的是稳妥的做法，四旋翼的电机，电调，支撑架等等，都会略有不同，意味着施加在每个电机上的力略有不同，这意味着四旋翼永远不会真正意义上的水平。
*以下内容出于安全原因而省略*
Determining Orientation
确定位置、方向
接下来就是定向了，或者说确定四旋翼的姿态。我们可以这样，通过飞行指令改变电机的速度。有两个传感器来定向，加速计和陀螺仪。加速计测量每一个方向的加速情况，（重力是加速的原因之一，通过重力我们能确定地面方向）陀螺仪则测量角速度。（例如没一个轴的旋转速度。）然而，加速度计对振动非常敏感，不是特别灵敏；而陀螺仪是高度灵敏，且抗振动，但容易浮动。（静止时也会显示1到2度每秒的角速度）所以，我们使用一个将两个传感器数据结合起来的融合算法，并得到两全其美的结果 - 这样的算法的不再本文的讨论范围之内。该算法通常是一个卡尔曼滤波器，或在ArduPilot的情况下，直接余弦矩阵（DCM）。我提供了DCM的链接，如果你有一个数学背景并对此感兴趣 ； 其余的人，我们不需要知道其具体细节。
值得庆幸的是，我们可以使用MPU6050传感器芯片，该芯片含有加速计和陀螺仪并有一个内置的数字运动处理单元（传感器融合）。它将所有值融合起来，并给我提供了一个四元数值。四元数值是一种不同的提供定位方式（相对于欧拉角：航向偏转），如果你已经进行了三维编程并且对此十分熟悉，你会发现四元数值有一定优势。 为了方便起见，我们更倾向于使用欧拉角并且使用欧拉角编程而不是四元数值。
此段代码就是接合MPU6050传感器的代码
在 setup()函数中
// Disable barometer to stop it corrupting bushal.gpio->pinMode(40,GPIO_OUTPUT);hal.gpio->write(40, 1); // Initialise MPU6050 sensorins. init(AP_InertialSensor::COLD_START, AP_InertialSensor::RATE_100HZ, NULL); // Initialise MPU6050's internal sensor fusion (akaDigitalMotionProcessing) hal.scheduler->suspend_timer_procs(); // stop bus collisions ns.dmp_init(); ins.push_gyro_offsets_to_dmp(); hal.scheduler->resume_timer_procs();
现在让我们来读取传感器数据，在loop（）函数开头加入这一行，这使得程序强行等待直到传感器有新的数据，不会改变电机的速度，直到我们得到新的数据。
while (ins.num_samples_available() == 0); **将延迟50Ms从loop（）函数中移除，我们不再需要了。**
现在我们从传感器中得到偏航/俯仰/滚动的数据，并将它们从弧度转化为角度。 ins.update(); ins.quaternion.to_euler(&roll, &pitch, &yaw); roll = ToDeg(roll) ; pitch = ToDeg(pitch) ; yaw = ToDeg(yaw) ; 我们将它从串行管理接口输出 hal.console->printf_P( PSTR(\ R:%4.1f Y:%4.1f\\n\ pitch, roll, yaw ); 你需要为这个print语句设定一个阀值，例如：确保它每循环20次只打印一次（提示：使用一个计数器）。否则，串口线带宽将被占满。
四处移动你的直升机，并确保生成正确的数值！
Acrobatic / Rate mode control
特技/速率模式控制
特技/速率模式是指你的发射机枝告诉四旋翼以一个特定的利率（如50deg/sec）转动，当您的遥控杆居中时四停止翻滚。相对于稳定模式，摇杆返回中心位置将使四旋翼恢复水平。这是一种需要实践以学习如何飞行的模式；但首先要实现稳定飞行模式，因为我们所需要的是稳定控制，这是是特技/速度控制的的最高层。
所以，我们的目标是让摇杆指示达到某一个翻转速率，而且四旋翼就努力按照这个速率
运动。因此，如果操作员要求在俯仰以50度/秒偏转，我们目前不偏转，那么我们需要加快后方马达和减速前面的马达。现在的问题是，我们要以速度来加速或减速？为了确定这一点，你需要了解比例积分微分（PID）控制器，我们需要广泛使用的。虽然有点黑科技的，原理是相当简单的。让我们假设我们的四旋翼现在在俯仰方向上旋转，所以实际速率为0，让我们进一步假设操作手希望四旋翼在15度/秒旋转，所以期望=15，现在我们可以这样说，我们得到的速率和实际速率之间的差（error）是：
error = desired - actual = 15 - 0 = 15
差（error）=期望值—实际值=15—0=15
现在将差给我，我们将他和与其相关的Kp相乘，产生一个数值我们用来加速或减速电机。所以我们可以说电机是这样改变的：
frontMotors = throttle - error*Kp rearMotors = throttle + error*Kp
前电机=阀值—差*Kp 后电机=阀值+差*Kp
当电动机加速四旋翼会开始转动，并且差（error）误会减少，从而导致前、后马达的速度差减少。这是我们所期望的，电机速度的差异将使四旋翼获得某一方向加速（即使飞行器处于运动状态），电机速度无差异将使其保持稳定状态（完美！！！）。信不信由你，这是我们真正需要的特技/速率模式，这个原则适用于每个轴（偏航，俯仰，滚转），并使用陀螺仪来告诉我们什,我们正在以什么样的速率旋转（实际的）。你可能会问的问题是，我应该怎么设置的Kp为？嗯，这是用于实验尝试的问题 - 我已经设置了一些能让我的450毫米四旋翼运行良好的值——坚持一下，你会得到这些代码。
如果你之前学过PID，你会知道实际上PID有两个部分：积分和微分。积分（Ki为调谐参数）基本上补偿恒定误差，有时Kp这个参数可能无法提供足够的响应，以应对所有的
状况，例如四旋翼是不平衡的，或者是风的影响。现在我们忽略了微分。
现在开始定义PID 阵列 和 全局常量 PID pids[6]; #define PID_PITCH_RATE 0 #define PID_ROLL_RATE 1 #define PID_PITCH_STAB 2 #define PID_ROLL_STAB 3 #define PID_YAW_RATE 4 #define PID_YAW_STAB 5 现在，在setup（）函数中将PID初始化为一些合理的值（你可能需要回来调整参数）。 pids[PID_PITCH_RATE].kP(0.7); // pids[PID_PITCH_RATE].kI(1); pids[PID_PITCH_RATE].imax(50); pids[PID_ROLL_RATE].kP(0.7); // pids[PID_ROLL_RATE].kI(1); pids[PID_ROLL_RATE].imax(50); pids[PID_YAW_RATE].kP(2.5); // pids[PID_YAW_RATE].kI(1); pids[PID_YAW_RATE].imax(50); pids[PID_PITCH_STAB].kP(4.5); pids[PID_ROLL_STAB].kP(4.5); pids[PID_YAW_STAB].kP(10); 暂时不管那些我注释掉的代码，让我们将四旋翼飞行正常。当然我们很难从代码中找到问题。
从陀螺仪中取得每个轴的旋转速率。 Vector3f gyro = ins.get_gyro();
陀螺仪中给出的是弧度率，我们将它转化为角速率： float gyroPitch = ToDeg(gyro.y), gyroRoll = ToDeg(gyro.x), gyroYaw = ToDeg(gyro.z);
接下来，我们要进行ACRO（高度？？）稳定控制。我们只是要做到，油门高于最低点（至少约1000pts，我的最大值是1170，最小值是1070），否则四旋翼会晃动时；油门若为零，四旋翼显然不会保持平衡。 if(rcthr > 1170)