小四轴编程入门教程之二:数据的滤波和融合

在上一节,我们讲述了陀螺仪和加速度计的作用。在这一节,我们讲述如何对陀螺仪和加速度数据进行滤波和融合。

首先,我们来谈小四轴的滤波。滤波的方法有很多种,在小四轴上,对于陀螺仪和加速度计,我们一般采用不同的滤波方法。对于陀螺仪,大家最常见的就是滑动平均法。即把最新的10个或者20个数据进行平均。在次基础上,对不同的数采用不同的加权,又能变种出很多种不同的滤波方法。对小四轴初学者来说,用最简单的滑动平均法就可以。对于加速度计,我们通常采用掐尾法,即去掉低位的数据以避免由于小四轴震动而引起的误差。

接下来,我们来谈小四轴的融合。所谓融合,就是把陀螺仪测量出来的数据和加速度计测量出来的数据以一定比重混合在一起。和大家在做蛋糕过程中,加多少牛奶,多少面粉方法类似。

小四轴融合常用的方法有:四元数法,互补滤波法,卡尔曼法等。通过调整合适的参数比例,把陀螺仪测量出来的物体角度和加速度计测量出来的角度进行互相修正。

由于该节涉及到的数学知识比较多,这里只是大略提及了各个概念,而不一一进行深入分析。对于各种滤波和融合的算法C语言实现方法,大家可以直接阅读我们的源代码。

说完了数据滤波和融合,下一节就轮到:

小四轴编程入门教程之三:PID的实现

 

小四轴编程入门教程之一:陀螺仪和加速度计

在小四轴中,陀螺仪是一种用于测量小四轴旋转速度的传感器,它测量的是角速度,是指物体在单位时间内转过的角度大小。通过测量物体在X/Y/Z三个轴上的角速度,我们就可以得到物体在X/Y/Z三个轴上一段时间内转过的角度。

那么,在小四轴上,陀螺仪的作用是什么呢? 我们知道,把一个物体悬浮在空中的方法可以是给物体的四个角施加拉力。如果物体某个方向的力度不够,那么物体就会因为重力的作用朝这个方向倾斜。通过在该物体上装上陀螺仪,就可以知道物体倾斜的方向,然后据此调整物体四个角的拉力,就可以达到让物体悬浮的目的。

至此,我们基本了解了陀螺仪在小四轴中的作用。但是,有一个问题我们不能忽略,那就是误差的问题。我们在使用传感器测量角速度时候,假定我们每一毫秒读取一次传感器数据,然后把该数据当作物体在这一毫秒内转过的角度。由于物体倾斜的过程并不是匀速的,所以计算出来的角度和物体实际转过的角度是有误差的。这样一来,随着时间的增加,所累加的误差也会越来越大。这样一来,即使物体在水平状态,但是由于陀螺仪的计算误差,从运算结果来看,物体并不在水平状态。

这个时候,我们引入了加速度计。加速度计是用于测量物体的加速度的。它的一个好处就是可以借助重力加速度来帮助我们来计算一个静止物体相对于地面的倾角。当一个物体和地面完全水平的时候,物体的倾角是0。但是,由于加速度计测量的不仅仅是重力的作用,对于一个运动的物体,它的测量数据是重力和其它力共同作用的的结果。由于在空中飞行的小四轴是运动的物体,所以我们无法根据加速度计所得到的数据直接来判断小四轴是否处于水平状态。

我们知道,要把小四轴悬浮在空中,就要在它的四个角通过电机施加升力。而每个电机应该给多少升力则取决于小四轴的倾斜状态。当小四轴处于水平状态,四个电机给一样的升力,当小四轴朝某个方向跌落,则该方向的电机给予更大的升力。要做到这一点,我们就要时刻测量小四轴的倾角。

但是,根据上面的描述,我们知道无论是陀螺仪,还是加速度计,都无法准确地给出飞行中的小四轴的倾斜状态。人们想到一个方法,就是用加速计测量的数据来修正陀螺仪的数据。这就是我们下一节要讲到的:

小四轴编程入门教程之二:数据的滤波和融合

 

 

四轴飞行器的位置式PID控制和增量式PID控制

在开始本文的描述前,我们先来看看什么叫PID控制器。P是Proportional的简称,I是Integral的简称,D是Derivative的简称,合并起来,PID控制器指的就是是比例,积分和微分控制器,用公式表示如下(e是指设定值和当前值的误差,t是指当前的时间)。
公式一:pid

以离散的数字方式表示如下(其中,k表示某个时刻,e(k)就表示某个时刻的误差值)。

公式二:pid2

以四轴飞行器为例,假定我们每1ms读取一次陀螺仪和加速度的数值,并计算到姿态pitch和roll,再假定我们的目前姿态是水平状态,即pitch=0,roll=0,那么我们就可以得到某个时刻的PID计算公式如下图(其中A代表的是姿态角)。

公式三:pid3

上述公式就是我们在四轴飞行器源代码里面经常看到的编程方法,通常,我们把微分控制的姿态差直接用陀螺仪的数据来表示,因为从陀螺仪读取出来的数据代表的就是现在和上一次的角速度差,乘以1ms的时间差,即为角度差,因而公式变为。

公式四:pid4

对于简单的四轴飞行,积分不是必要的条件,而且积分使用不好的情况下,会对四轴飞行器的稳定性产生很大影响,所以对于四轴飞行器初学者来说,我们可以先去除积分项,把公式简化成下面的样子。

公式五:pid6

就这样,一个简单的四轴飞行器的PID控制公式就出现在我们面前了。把该公式变换成代码,即可以实现四轴飞行器的PID控制。

从上面的描述,我们可知,这是一个位置式的PID控制器,即我们是把当前位置和水平位置作为误差来进行PID控制,从而实现四轴飞行器的水平飞行的。增量式PID和位置式PID的不同就在于增量PID控制是把位置的变化量作为控制量。这样一来,我们可以得到下述的计算公式。

公式六:pid7

从上面公式,我们可以看出,积分项累加的结果就是当前姿态减去初始姿态,假定初始姿态为水平状态,那么姿态增量的积分就是当前的姿态。比例控制的是当前姿态相对于上一姿态的增量,根据我们之前的描述,可以用陀螺仪数据来表示。假定我们忽略微风项的控制,那么公式可以简化为:

公式七:pid8

把公式七和公式五做对比,我们可以知道,在四轴飞行器中,位置式的PD控制相当于增量式PID的PI控制。把位置式PID控制和增量式PID控制组合在一起,就可以组成串级PID控制。

上文详细描述了位置式和增量式PID控制,并引出了串级PID控制的概念,如需转载,请勿用于商业目的,并注明:该文章出自圆点博士无人机www.bspilot.com

 

 

 

四轴遥控器PPM遥控信号分析

PPM是四轴飞行器遥控器普遍使用的一种遥控信号格式。它的全称是Pulse Position Modulation,中文的意思就是脉冲位置调制。它是一种周期性信号,每个信号周期里面包含一个或者多个脉冲。

在四轴飞行器中,PPM信号每个周期时间是20ms, 即遥控信号的频率是50赫兹。在一个周期内,包含了一个同步脉冲和多个遥控信号脉冲,用来分别代表油门,方向,飞行模式等。比如在APM飞控上,用4个通道分别控制前后(Pitch),左右(Roll),旋转(Yaw)和油门(Throttle),用第5个通道来控制自稳模式,定高模式,悬停模式,无头模式,返航模式,降落模式,等六种飞行模式。

下面我们来看针对遥控器PPM信号的解析,如下图所示,每个PPM信号的周期时间是20ms。每个脉冲信号均以一个长度为0.5ms的低电平信号开始,脉冲信号的最小时间长度是1ms,最大时间长度是2ms,中间位置时间长度是1.5ms。以遥控器的左右摇杆来举个例子说明如下:当摇杆在中间位置时,脉冲时间长度为1.5ms,摇杆打到最左边,脉冲时间长度为1ms,摇杆打到最右边,脉冲时间长度为2ms。需要注意的是,同步信号的脉冲时间长度要大于2ms,用于区分它和遥控通道的不同。

ppm

如上图所示:
第一行PPM描述了一个四通道遥控器的PPM信号波形。图中共有5个低点平区间,时间长度均为0.5ms,用于代表四个遥控通道CH0-CH3的数据和一个同步码。另外,图中也有5个高点平区间。CH0-CH3的高电平时间长度在0.5ms和1ms之间变化,用于代表不同的遥控数据。
第二行PPM-1描述了CH0-CH3均为最小值下的波形,我们从图中可以看出,每个CH的脉冲时间宽度是1ms, 同步脉冲时间宽度为16ms。
第三行PPM-2描述了CH0-CH3均为中间值下的波形,我们从图中可以看出,每个CH的脉冲时间宽度是1.5ms, 同步脉冲时间宽度为14ms。
第四行PPM-3描述了CH0-CH3均为最大值下的波形,我们从图中可以看出,每个CH的脉冲时间宽度是2ms, 同步脉冲时间宽度为12ms。
第五行PPM-3描述了CH0,CH2均为最小值,CH1,CH3均为最大值下的波形,我们从图中可以看出,CH0,CH2的脉冲时间宽度是1ms, CH1,CH3的脉冲时间宽度是2ms, 同步脉冲时间宽度为14ms。

最后,我们来说说如何利用单片机来解码PPM信号。从上面的描述可知,每个脉冲均以低电平开头,所以我们可以用IO口下降沿触发中断的方式来计算两个中断之间的时间间隔。通过该时间间隔来判定同步信号和各个遥控通道的数值。

在上文提及的APM的6种飞行模式控制中,就是利用脉冲信号的时间差异来识别不同的飞行模式的,其六种飞行模式分别对应的时间长度是:0-1.230ms,1.231ms-1.360ms,1.361ms-1.490ms,1.491ms-1.620ms,1.621ms-1.749ms,1.750ms-2ms。

上文详细描述了四轴遥控器的PPM信号解码方法,如需转载,请勿用于商业目的,并注明:该文章出自圆点博士无人机www.bspilot.com

陀螺仪和加速度计MPU6050的单位换算方法

对于四轴的初学者,可能无法理解四轴源代码里面陀螺仪和加速度数据的那些数学转换方法。下面我们来具体描述下这些转换方法。

我们首先来看陀螺仪数据。在MPU6050的手册里面,提供了一个陀螺仪数据表如下:

gyro_spec

在表格里面,列出了当寄存器FS_SEL选择不同数值时的陀螺仪量程范围。 以四轴常用的FS_SEL=3为例,这时陀螺仪的量程为-2000到+2000。

由于MPU6050采用的是16位带符号数作为陀螺仪测量数据输出,即数据类型是int16。所以以16进制来表示,最小的数是FFFF,由于最高位为符号位,所以相当于-7FFFF,即-32767;最大的数是7FFF,即32767。

综合上述的描述,我们可以得到,数字-32767对应-2000度每秒的陀螺仪角速度,32767对应2000度每秒的陀螺仪角速度。把32767除以2000,就可以得到16.40, 即我们说的灵敏度。

从上面的计算可知,把从陀螺仪读出的数字除以16.40,就可以换算成陀螺仪的角速度数值。举个例子,如果我们从陀螺仪读到的数字是1000,那么对应的角速度数据是1000/16.40=61度每秒。

在四轴姿态计算中,我们通常要把角度换算成弧度。我们知道2Pi代表360度,那么1度换算成弧度就是:2Pi/360=(2*3.1415926)/360=0.0174532。用倒数表示就是:1/57.30

通过上述分析,我们就可以知道,当采用量程为-2000到+2000的范围,把我们从陀螺仪获取的数据做如下处理,就可以用于四元数的姿态解算(用gyro_x来代表从陀螺仪读到的数据): gyro_x/(16.40*57.30)=gyro_x*0.001064,单位为弧度每秒。

接下来,我们看加速度计:

acc_spec

采用和陀螺仪同样的计算方法,当AFS_SEL=3时,数字-32767对应-16g,32767对应16g。把32767除以16,就可以得到2048, 即我们说的灵敏度。把从加速度计读出的数字除以2048,就可以换算成加速度的数值。举个例子,如果我们从加速度计读到的数字是1000,那么对应的加速度数据是1000/2048=0.49g。g为加速度的单位,重力加速度定义为1g, 等于9.8米每平方秒。

上文详细描述了MPU6050传感器陀螺仪和加速度数据的单位换算方法,如需转载,请勿用于商业目的,并注明:该文章出自圆点博士无人机www.bspilot.com

四元数AHRS姿态解算和IMU姿态解算分析

AHRS是自动航向基准系统(Automatic Heading Reference System)的简称。目前,使用四元数来进行AHRS姿态解算的算法被广泛采用于四轴飞行器上。该算法源自英国Bristol大学的Ph.D Sebastian Madgwick,他在2009年开发并发布了该算法。下面我们来对该算法的代码进行详细分析。

我们首先来看IMU部分。IMU是惯性测量装置(Inertial Measurement Unit)的简称,通常包含陀螺仪和加速度计。陀螺仪测量的是角速度,即物体转动的速度,把速度和时间相乘,即可以得到某一时间段内物体转过的角度。加速度计测量的是物体的加速度,我们知道,重力加速度是一个物体受重力作用的情况下所具有的加速度。当物体处于静止状态时,加速度计测量出来的值就等于重力加速度1g, 约等于9.8米每平方秒。重力加速度g的方向总是竖直向下的,通过获得重力加速度在其X轴,Y轴上的分量,我们可以计算出物体相对于水平面的倾斜角度。典型的IMU惯性测量芯片为MPU6050,它被广泛采用在四轴飞行器上。

对于陀螺仪的角速度测量,大家比较好理解,简单来说,相当于一个人绕着一个圆圈行走,假如他的速度是1度没秒,那么通过速度乘以时间,我们就可以知道他距离起点走了多少度。

那么我们如何理解用加速度计来测量倾角呢?一个简单的例子如下: 一个单轴的加速计位于重力水平面上的时候,它在垂直方向上受到的加速度为1g,在水平方向上受到的加速度为0。当我们把它旋转一个角度的时候,就会在水平轴上产生一个加速度分量。通过它们的关系,就可以计算出该单轴加速计的倾角。

gravity

在一个三轴的加速度传感器中,可以通过下列的反正切函数运算公式来计算加速度计各个轴的倾角。

gravity_xyz

另外,我们还要注意各传感器的方向。下图是MPU6050和MPU9150的传感器方向定义,其中MPU6050只包含陀螺仪和加速计共六个轴,而MPU9150还包含磁力计,共九个轴。从MPU6050和MPU9150获得的数据可以直接在下面的算法中使用,而不需要做符号变换。

imu_dir

现在,让我们回到姿态解算代码的分析。下述代码的核心思想是:通过陀螺仪的积分来获得四轴的旋转角度,然后通过加速度计的比例和积分运算来修正陀螺仪的积分结果。下面代码中的gx,gy,gz分别代表陀螺仪在X轴,Y轴和Z轴三个轴上的分量,ax,ay,az分别代表加速度计在在X轴,Y轴和Z轴三个轴上的分量。

void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;

如果加速计各轴的数均是0,那么忽略该加速度数据。否则在加速计数据归一化处理的时候,会导致除以0的错误。
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

把加速度计的数据进行归一化处理。其中invSqrt是平方根的倒数,使用平方根的倒数而不是直接使用平方根的原因是使得下面的ax,ay,az的运算速度更快。通过归一化处理后,ax,ay,az的数值范围变成-1到+1之间。
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比,从而实现对四轴姿态的修正。
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q1 * q3 – q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 – 0.5f + q3 * q3;

使用叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。
// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz – az * halfvy);
halfey = (az * halfvx – ax * halfvz);
halfez = (ax * halfvy – ay * halfvx);

把上述计算得到的重力差进行积分运算,积分的结果累加到陀螺仪的数据中,用于修正陀螺仪数据。积分系数是Ki,如果Ki参数设置为0,则忽略积分运算。
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}

把上述计算得到的重力差进行比例运算。比例的结果累加到陀螺仪的数据中,用于修正陀螺仪数据。比例系数为Kp。
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}

通过上述的运算,我们得到了一个由加速计修正过后的陀螺仪数据。接下来要做的就是把修正过后的陀螺仪数据整合到四元数中。
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx – qc * gy – q3 * gz);
q1 += (qa * gx + qc * gz – q3 * gy);
q2 += (qa * gy – qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy – qc * gx);

把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}

在一个三维的空间内,由于重力加速度的存在,加速度计为我们提供了一个水平位置的绝对参考,但是它无法给我们提供一个方向的参考。这时候,我们首先考虑到的是指南针,磁力计就是这样的一个传感器,它给人们提供了一个正北方向的绝对参考。在四轴中常使用的地磁传感器是HMC5883L。

有了上面IMU惯性测量姿态解算的基础,我们就能很容易理解AHRS姿态解算的代码。它是在IMU惯性测量的基础上加入了地磁姿态的解算。下面代码中的gx,gy,gz分别代表陀螺仪在X轴,Y轴和Z轴三个轴上的分量,ax,ay,az分别代表加速度计在在X轴,Y轴和Z轴三个轴上的分量。mx,my,mz分别代表地磁在在X轴,Y轴和Z轴三个轴上的分量。

void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float hx, hy, bx, bz;
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
float halfex, halfey, halfez;
float qa, qb, qc;

如果地磁传感器各轴的数均是0,那么忽略该地磁数据。否则在地磁数据归一化处理的时候,会导致除以0的错误。
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
return;
}

如果加速计各轴的数均是0,那么忽略该加速度数据。否则在加速计数据归一化处理的时候,会导致除以0的错误。
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

把加速度计的数据进行归一化处理。
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

把地磁的数据进行归一化处理。
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;

预先进行四元数数据运算,以避免重复运算带来的效率问题。
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;

// Reference direction of Earth’s magnetic field
hx = 2.0f * (mx * (0.5f – q2q2 – q3q3) + my * (q1q2 – q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f – q1q1 – q3q3) + mz * (q2q3 – q0q1));
bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 – q0q2) + my * (q2q3 + q0q1) + mz * (0.5f – q1q1 – q2q2));

根据当前四元数的姿态值来估算出各重力分量Vx,Vy,Vz和各地磁分量Wx,Wy,Wz。
// Estimated direction of gravity and magnetic field
halfvx = q1q3 – q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 – 0.5f + q3q3;
halfwx = bx * (0.5f – q2q2 – q3q3) + bz * (q1q3 – q0q2);
halfwy = bx * (q1q2 – q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f – q1q1 – q2q2);

使用叉积来计算重力和地磁误差。
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex = (ay * halfvz – az * halfvy) + (my * halfwz – mz * halfwy);
halfey = (az * halfvx – ax * halfvz) + (mz * halfwx – mx * halfwz);
halfez = (ax * halfvy – ay * halfvx) + (mx * halfwy – my * halfwx);

把上述计算得到的重力和磁力差进行积分运算,
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}

把上述计算得到的重力差和磁力差进行比例运算。
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}

把由加速计和磁力计修正过后的陀螺仪数据整合到四元数中。
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx – qc * gy – q3 * gz);
q1 += (qa * gx + qc * gz – q3 * gy);
q2 += (qa * gy – qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy – qc * gx);

把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}

上文详细描述了由Madgwick提出四元数AHRS姿态解算和IMU姿态解算方法,如需转载,请勿用于商业目的,并注明:该文章出自圆点博士无人机www.bspilot.com

 

针对TI TM4C123G的PWM编程来实现四轴的电机控制

在四轴飞行器中,飞控通过PWM输出来控制电机的转速,从而实现飞行器的平稳飞行。PWM控制主要有两个参数,PWM周期和PWM占空比。在四轴上,PWM的周期在四个电机上都是相同的,在飞行过程中改变的是PWM的占空比。

Ti的TM4C123G的PWM结构和STM32略有不同。STM32一个时钟源可以同时驱动4个PWM信号,而TM4C123G一个时钟源只能同时驱动2个PWM信号。所以对于TM4C123G,我们需要启动2个PWM驱动源。这这个驱动源需要保持同样的PWM周期,但不需要绝对同步。

下面我们来看一个TM4C123G通过PWM驱动四个四轴电机的例子:

设置PWM占空比:
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0,bs004_fly_m3_out);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_1,bs004_fly_m4_out);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2,bs004_fly_m1_out);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3,bs004_fly_m2_out);

PWM信号初始化:
//etootle:PWM clock is processor clock /4=80/4=20M
ROM_SysCtlPWMClockSet(SYSCTL_PWMDIV_4);
//
//etootle: enable the GPIOB/D clk becaue PB4 and PB5 is used for PWM
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
//
//etootle:enable PWM modul M0 and M1
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM1);
//
//etootle:Set GPIO type
ROM_GPIOPadConfigSet(GPIO_PORTB_BASE, GPIO_PIN_4|GPIO_PIN_5,GPIO_STRENGTH_4MA,GPIO_PIN_TYPE_STD);
ROM_GPIOPadConfigSet(GPIO_PORTD_BASE, GPIO_PIN_0|GPIO_PIN_1,GPIO_STRENGTH_4MA,GPIO_PIN_TYPE_STD);
//
//etootle: define GPIO to PWM pin
ROM_GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_4|GPIO_PIN_5);
ROM_GPIOPinTypePWM(GPIO_PORTD_BASE, GPIO_PIN_0|GPIO_PIN_1);
//
//etootle:map GPIO to PWM
ROM_GPIOPinConfigure(GPIO_PB4_M0PWM2);
ROM_GPIOPinConfigure(GPIO_PB5_M0PWM3);
ROM_GPIOPinConfigure(GPIO_PD0_M1PWM0);
ROM_GPIOPinConfigure(GPIO_PD1_M1PWM1);
//
//etootle:PWM mode
ROM_PWMGenConfigure(PWM0_BASE, PWM_GEN_1, PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC);
ROM_PWMGenConfigure(PWM1_BASE, PWM_GEN_0, PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC);
//
//etootle:PWM period 1000-1=20K
ROM_PWMGenPeriodSet(PWM0_BASE, PWM_GEN_1, 1000);
ROM_PWMGenPeriodSet(PWM1_BASE, PWM_GEN_0, 1000);
//
//etootle:invert m2 and m4
//ROM_PWMOutputInvert(PWM0_BASE,PWM_OUT_3_BIT, true);
//ROM_PWMOutputInvert(PWM1_BASE,PWM_OUT_1_BIT, true);
//
//etootle:set pwm dulty cycle
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2,0);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3,0);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0,0);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_1,0);
//
//etootle: enable the PWM counter
ROM_PWMGenEnable(PWM0_BASE, PWM_GEN_1);
ROM_PWMGenEnable(PWM1_BASE, PWM_GEN_0);
//
//etootle:enable the pwm output
ROM_PWMOutputState(PWM0_BASE,PWM_OUT_2_BIT|PWM_OUT_3_BIT, true);
ROM_PWMOutputState(PWM1_BASE,PWM_OUT_0_BIT|PWM_OUT_1_BIT, true);

该设置已经在基于TM4C123G的四轴飞行器上验证测试通过。如需转载,请勿用于商业目的,并注明:该文章出自圆点博士无人机www.bspilot.com

基于TI Tiva TM4C123G LaunchPad的四轴姿态解算

本文分享基于TI Tiva TM4C123G LaunchPad开发板的四轴姿态解算源代码,代码使用MDK工程结构。该代码包含无线蓝牙模块使用的串口通讯,MPU6050陀螺仪模拟I2C通讯,以及采用四元数的四轴姿态解算方法三个部分。代码在16MHz的频率下运行,用户可以自行修改时钟参数以达到更快的解算速度,通过配合圆点博士小四轴上位机软件,用户也可以方便地看到解算出来的姿态。

TI TM4C123GH6PM的硬件定义如下:
1,TM4C123GH6PM的第48脚作为I2C的SDA和MPU6050的陀螺仪模块相连
2,TM4C123GH6PM的第47脚作为I2C的SCL和MPU6050的陀螺仪模块相连

Ti_M4_IMU

 

Ti TM4C123G四轴姿态解算源代码(Ti_M4_IMU源代码.zip)下载