四轴程序学习apmpx4ardupoilotapm 1.docx

四轴程序学习apmpx4ardupoilotapm 1.docx

《四轴程序学习apmpx4ardupoilotapm 1.docx》由会员分享，可在线阅读，更多相关《四轴程序学习apmpx4ardupoilotapm 1.docx（18页珍藏版）》请在冰豆网上搜索。

四轴程序学习apmpx4ardupoilotapm1

四旋翼飞行器搭建教程

（译自————加里斯.欧文）

本文将带你通过建立自己的飞行控制器（飞空软件），同时教你工作的具体细节。

这些信息很难找到，特别是那些本身就不是航天工程师的人！

就我个人而言，我用了六个月，因为我花费了太多的时间查找bug和调试bug，但通过这篇文章你可以短期收获同样的经验。

我会教你避开陷阱，这样你就不会像我一样浪费时间。

第一个关键是你对硬件的选择。

我选择从零开始建立自己系统，在这一阶段的时候我都不知道RC（remotecontrolled遥控;radiocoding无线电编码;）和飞行器是如何飞行的，这是一个巨大错误。

开始我以为，通过自行购买附加电路，芯片和传感器能省很多钱，结果最终我花了一大笔钱！

放过自己吧，直接去购买ardupilot2.5控制板，组装你的直升机，了解遥控，了解飞行原理，然后回到这里。

这个板子本质上是只是一个连有一些传感器Arduino（开源主控板,可查

本项目（ardupilot）由3DRobotics提供赞助，这意味着他们销售所设计的硬件获利，并将所得利润回馈社区。

该软硬件是完全开源的，所有人可以免费复制下载。

你可以直接从他们那里购买，或者从Hobbyking（namedHKPilot）andRCTimer（namedArduFlyer）.购买相同的拷贝件。

在这篇文章中，我将假定您有ardupilot硬件——其本质上上是附传感器Arduino。

如果你选择忽视我的建议，并且建立自己的硬件，或使用Arduino电路板，那么您需要更换的底层代码（HAL库）。

我也会以为你在X配置（x型四旋翼），+/X（两种四旋翼配置）和六/八旋翼飞行器之间切换（只是不同的电机的组合），配置的改变不会让它在本文有任何实质性的区别。

理想的情况是，你已经飞行过加载了arducopter代码的四旋翼，因此你应该将电机连接如下所示的位置和旋转方向。

我也要假设你有一些arduino的经验，或至少是C/C++的经验。

Arduino库不是特别智能的或适合，所以我们将使用一些更加合适的的ardupilot库。

然而，我们会尽量少使用、最小幅度的使用，以提供DIY式解决方案的支持（这就是为什么你在这里毕竟）。

我们将要使用的第一和主要库是ardupilot硬件抽象层（HAL）库。

这个库试图隐藏了底层的区别，例如你如何读取和写入引脚和其它一些东西——其优势是，软件可移植到新的硬件，只需更换硬件抽象层。

在ardupilot的情况下，有两个硬件平台，APM和PX4，每一个都有自己的底层库允许ardupilot代码运行。

如果您稍后决定在RaspberryPi上运行你的代码，你只需要改变硬件层代码。

硬件层是由如下几个部分组成：

RCInput无线输入——阅读RC无线电rcinput-。

RCOutput无线输出——用于控制电动机和其它输出rcoutput-。

Scheduler计划表——运行特定的任务在固定时间间隔调度程序-。

console控制台——提供访问串行端口。

I2C，SPI——总线驱动程序（用于连接传感器小电路板网络）

GPIO——一般目的的输入/输出——允许直接访问Arduino引脚，但是在我们的案例中，主要的发光二极管

如何下载：

你需要下载Arduino的IDEardupilot版。

还需要将库文件放在你的延伸文件夹中。

还要确保你选择你的板类型像Arduino的菜单这样：

ReadingtheRadioInputs

读取无线电输入

我们的飞行控制系统要读取无线电输入信号（飞行指令），测量四旋翼的实时的姿态（偏航/俯仰/滚动），改变电机的转速——以适应四旋翼按照我们所期望的方式飞行。

所以让我们开始通过无线电开始学习。

RC接收机有几个输出，有好几个通道（或称为棒/开关/旋钮）。

无线电输出的50Hz脉冲，这个脉冲的宽度是由无线电发射机的摇杆位置决定的。

通常情况下，脉冲持续1000us到2000us长并有18000us到19000us的间隙，所以表示0的油门会产生一个1000us的脉冲，表示完全的油门则有2000us长。

可悲的是，大多数的接收机是不准确的，我们通常要测量每个杆位的最小或最大值脉冲宽度（这我们接下来将要做的）。

Ardupilot的底层库，为我们做了测量这些脉冲宽度的痛苦工作。

如果你自己编写这些代码，你需要中断其他工作并用定时器来测量无线电信号——analogreadArduino不适合做这项工作，因为它占用了处理器全部的计算容量，虽然它可以测量，但是同时也阻止我们其他事。

（即Arduino是单线程软件，无法同时做两项工作）实现一个测量信号的工作并不难，让这种程序持续1个小时甚至更长是很普通的事，我们不是只做这么简单的程序。

下面是一些使用APM底层库，测量通道“值”的代码的简单例子。

通道“值”就是测量毫秒级的脉冲宽度。

#include

#include

#include

#include

#include

#include

#include

#include

constAP_HAL:

:

HAL&hal=AP_HAL_AVR_APM2;//Hardwareabstractionlayer

voidsetup（）

{

}

voidloop（）

{uint16_tchannels[8];//arrayforrawchannelvalues

//ReadRCchannelsandstoreinchannelsarray

hal.rcin->read（channels,8）;

//Copyfromchannelsarraytosomethinghumanreadable-arrayentry0=input1,etc.

uint16_trcthr,rcyaw,rcpit,rcroll;//Variablestostorercinputrcthr=channels[2];

rcyaw=channels[3];

rcpit=channels[1];

rcroll=channels[0];

hal.console->printf_P（

PSTR（"individualreadTHR%dYAW%dPIT%dROLL%d\r\n"）,

rcthr,rcyaw,rcpit,rcroll）;

hal.scheduler->delay（50）;//Wait50ms

}

AP_HAL_MAIN（）;//specialmacrothatreplace'soneofArduino'stosetupthecode（e.g.ensureloop（）iscalledinaloop）.

创建一个新的草稿脚本，并将该脚本上传到Ardupilot硬件上。

使用串行记录仪纪录下每个通道的最大和最小值。

（同时将脉冲调至极限）

现在让我们来衡量脉冲的“值”，以显示他们所含有的意义。

我们要使用一种称为Map的功能，这是一种确定一个数值在某一区间将其判读为另一“值”的功能。

例如，有一个50的数值，在0——100之间，而我们想要将他放入0到500这个区间来衡量，Map的功能将会定义其为250，并返回250这个值。

Map功能会在你引用并定义（#include&defines）之后被启用（从Arduino库中引用）

longmap（longx,longin_min,longin_max,longout_min,longout_max）

{

return（x-in_min）*（out_max-out_min）/（in_max-in_min）+out_min;

}

这样使用Map功能：

result=map（VALUE,FROM_MIN,FROM_MAX,TO_MIN,TO_MAX）.

你会感到仍然没有接触到油门，毫无疑问你会对于自己没有参与建立油门的判读而感到高兴，这个阀值是由电调生成的（如果你是按照我的建议做的）。

俯仰和翻滚在正负45度之间会被测量，当然你也可以设定为正负150度。

我的loop（）函数现在看起来像，更换为测量最大最小杆位的Map功能。

我们也会改变函数类型，变为long型，以支持负数。

longrcthr,rcyaw,rcpit,rcroll;//Variablestostorercinput

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函数前改为负函数。

当然你也要确保当你增加油门值时，实际油门值也会增加。

Controllingthemotors

控制电机

电机是由电调来控制的，他们的工作脉宽大约有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）

#defineMOTOR_FL2//Frontleft

#defineMOTOR_FR0//Frontright

#defineMOTOR_BL1//backleft

#defineMOTOR_BR3//backright

你的loop函数里，读取无线电信号后，将这个无线电阀值直接输出给电机

hal.rcout->write（MOTOR_FR,rcthr）;

你现在可以编程你的四旋翼并实验他，注意一定是不带螺旋桨的。

缓慢的提高阀值和前右电机的旋转速度。

再说一次，如果所有电机带了螺旋桨，都会旋转起来，但是四旋翼还是会坠毁。

我们需要的是稳妥的做法，四旋翼的电机，电调，支撑架等等，都会略有不同，意味着施加在每个电机上的力略有不同，这意味着四旋翼永远不会真正意义上的水平。

*以下内容出于安全原因而省略*

DeterminingOrientation

确定位置、方向

接下来就是定向了，或者说确定四旋翼的姿态。

我们可以这样，通过飞行指令改变电机的速度。

有两个传感器来定向，加速计和陀螺仪。

加速计测量每一个方向的加速情况，（重力是加速的原因之一，通过重力我们能确定地面方向）陀螺仪则测量角速度。

（例如没一个轴的旋转速度。

）然而，加速度计对振动非常敏感，不是特别灵敏；而陀螺仪是高度灵敏，且抗振动，但容易浮动。

（静止时也会显示1到2度每秒的角速度）所以，我们使用一个将两个传感器数据结合起来的融合算法，并得到两全其美的结果-这样的算法的不再本文的讨论范围之内。

该算法通常是一个卡尔曼滤波器，或在ArduPilot的情况下，直接余弦矩阵（DCM）。

我提供了DCM的链接，如果你有一个数学背景并对此感兴趣；其余的人，我们不需要知道其具体细节。

值得庆幸的是，我们可以使用MPU6050传感器芯片，该芯片含有加速计和陀螺仪并有一个内置的数字运动处理单元（传感器融合）。

它将所有值融合起来，并给我提供了一个四元数值。

四元数值是一种不同的提供定位方式（相对于欧拉角：

航向偏转），如果你已经进行了三维编程并且对此十分熟悉，你会发现四元数值有一定优势。

为了方便起见，我们更倾向于使用欧拉角并且使用欧拉角编程而不是四元数值。

此段代码就是接合MPU6050传感器的代码

在setup（）函数中

//Disablebarometertostopitcorruptingbushal.gpio->pinMode（40,GPIO_OUTPUT）;hal.gpio->write（40,1）;

//InitialiseMPU6050sensorins.

init（AP_InertialSensor:

:

COLD_START,AP_InertialSensor:

:

RATE_100HZ,NULL）;

//InitialiseMPU6050'sinternalsensorfusion（akaDigitalMotionProcessing）

hal.scheduler->suspend_timer_procs（）;//stopbuscollisions

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（"P:

%4.1fR:

%4.1fY:

%4.1f\n"）,

pitch,

roll,

yaw

）;

你需要为这个print语句设定一个阀值，例如：

确保它每循环20次只打印一次（提示：

使用一个计数器）。

否则，串口线带宽将被占满。

四处移动你的直升机，并确保生成正确的数值！

Acrobatic/Ratemodecontrol

特技/速率模式控制

特技/速率模式是指你的发射机枝告诉四旋翼以一个特定的利率（如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阵列和全局常量

PIDpids[6];

#definePID_PITCH_RATE0

#definePID_ROLL_RATE1

#definePID_PITCH_STAB2

#definePID_ROLL_STAB3

#definePID_YAW_RATE4

#definePID_YAW_STAB5

现在，在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）;

暂时不管那些我注释掉的代码，让我们将四旋翼飞行正常。

当然我们很难从代码中找到问题。

从陀螺仪中取得每个轴的旋转速率。

Vector3fgyro=ins.get_gyro（）;

陀螺仪中给出的是弧度率，我们将它转化为角速率：

floatgyroPitch=ToDeg（gyro.y）,gyroRoll=ToDeg（gyro.x）,gyroYaw=ToDeg（gyro.z）;

接下来，我们要进行ACRO（高度？

？

）稳定控制。

我们只是要做到，油门高于最低点（至少约1000pts，我的最大值是1170，最小值是1070），否则四旋翼会晃动时；油门若为零，四旋翼显然不会保持平衡。

if（rcthr>1170）

{

//***MINIMUMTHROTTLETODOCORRECTIONSMAKETHIS20ptsABOVEYOURMINTHRSTICK***/

longpitch_output=pids[PID_PITCH_RATE].get_pid（gyroPitch-rcpit,1）;

longroll_output=pids[PID_ROLL_RATE].get_pid（gyroRoll-rcroll,1）;

longyaw_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

{//MOTORSOFF

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（inti=0;i<6;i++）//resetPIDintegralswhilstontheground

pids[i].reset_I（）;

}

现在在你的手中要将油门提高20%，然后向前/向后，向左/向右偏转四旋翼，并保证螺旋桨的速度增加或减少。

如果四旋翼向一边倾斜，那一边的螺旋桨就要加速相反一边的则要减速。

如果不这样，改变相应的电机信号，（例如，如果俯仰角是错的，在错误的俯仰角之前没有改变信号，四旋翼就会翻滚）。

你可以这样测试你选择的数据，同步你的PID数据，用绳子将四旋翼固定在一个轴上，测试没一个轴的翻转。

这是一个非常有用的经验去了解PID是如何运作的，但是不是必须的。

这是我评定合适PID数据的示范——我让他以50度每秒翻转。

Video:

RatePIDsonlywithquadfixedononeaxis

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

现在，我们需要添加偏航（左右航向）的支持。

正如你所知，两侧的电机向相反的方向旋转给我们偏航的控