四轴 - - 程序 - 学习 - apm(px4 - - ardupoilot - - apm - )(1)

我的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)

联系客服:779662525#qq.com(#替换为@) 苏ICP备20003344号-4