DIY飞控（STM32+MPU9150)，试飞成功 ！附PID及互补滤波代码
本帖最后由 zhanwang_sky 于 2014-2-12 11:09 编辑先上个成品图
简述一下飞控的硬件组成：
1、主控芯片用STM32F107VCT6，用的最小系统板
2、姿态传感器用的MPU9150
3、遥控器接收机是Futaba 6208，直接读它的S.Bus信号，关于S.Bus的详细信息请看
http://mbed.org/users/Digixx/notebook/futaba-s-bus-controlled-by-mbed/ 这里补充一下，S.Bus帧头标志应该是0x0F，而不是他说的F0。
还有需要注意的是S.Bus信号是经过反相的，即低电平为逻辑“1”，高电平为逻辑“0”，加个三极管就行了
互补滤波请参考 http://www.x-io.co.uk/ 这里有关于四元数跟互补滤波的详细资料和现成代码~~
互补滤波现成代码请看这里 http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
四元数转换成欧拉角请看 http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html这样算出来的Pitch是0~90度的，网上还有一些让Pitch在-180~0~+180之间变化的方法，我懒了 没弄~~~
以下是PID控制代码，我直接放在主函数里了
只用了PD控制，没加积分项。
MPU6050采样率设在200Hz，电调PWM频率也是200Hz（好盈飞腾）
int main(void)
{
////////////////////////////////////////
// 变量定义
// 10轴传感器初始化结构体
struct Ten_Axis_Sensors_Init_s Ten_Axis_Init_Structure;
// 当前角度及期望角度
float current_attitude, current_heading,
desired_attitude = {0.f}, desired_heading = {0.f}, desired_thro = 1000.f;
// PID参数
float a_p_gain = 4.08f, a_i_gain = 0.f, a_d_gain = 238.f,
h_p_gain = 2.f, h_i_gain = 0.f, h_d_gain = 235.f;
// PID过程变量
float last_attitude = {0.f}, last_heading = 0.f,
attitude_int_value = {0.f}, heading_int_value = 0.f,
error;
// PID调整量
float attitude_trim, heading_trim;
// 控制量
uint16_t thro;
// 状态变量
uint8_t offline = 0, sbus_rtn, n;
////////////////////////////////////////
// 函数声明
void SysTick_delay(__IO uint32_t nTime);
////////////////////////////////////////
// 配置所有外设
Horizon_Configuration();
////////////////////////////////////////
// Application
// 给电调一个持续的低油门信号
SysTick_delay(4000);
// 初始化S.Bus缓冲区
sbus_set_buffer_en(1);
// 初始化10轴传感器
Ten_Axis_Init_Structure.MPU_SMPRT = 200;
Ten_Axis_Init_Structure.MPU_LPF_DEF = MPU_LPF_GYRO_BW_98HZ;
Ten_Axis_Init_Structure.MPU_GYRO_FSR_DEF = MPU_GYRO_FSR_2000DPS;
Ten_Axis_Init_Structure.MPU_ACCEL_FSR_DEF = MPU_ACCEL_FSR_2G;
Ten_Axis_Init_Structure.MPU_GYRO_CALIB_EN = 1;
Ten_Axis_Init_Structure.MPU_ACCEL_CALIB_EN = 1;
ten_axis_sensors_init(&Ten_Axis_Init_Structure);
// 开始接收上位机指令
usart_receive(UPPER_USART, 2, G_upper_cmd);
while(1)
{
while(!G_ten_axis_data_calculable);
ten_axis_data_norm();
////////////////////
// 解姿态
if(G_Ten_Axis_Data_Structure.mag_available)
{
MadgwickAHRSupdate(G_Ten_Axis_Data_Structure.gyro * 0.01745329f, G_Ten_Axis_Data_Structure.gyro * 0.01745329f, G_Ten_Axis_Data_Structure.gyro * 0.01745329f,
G_Ten_Axis_Data_Structure.accel, G_Ten_Axis_Data_Structure.accel, G_Ten_Axis_Data_Structure.accel,
G_Ten_Axis_Data_Structure.mag, G_Ten_Axis_Data_Structure.mag, G_Ten_Axis_Data_Structure.mag);
}
else
{
MadgwickAHRSupdateIMU(G_Ten_Axis_Data_Structure.gyro * 0.01745329f, G_Ten_Axis_Data_Structure.gyro * 0.01745329f,
G_Ten_Axis_Data_Structure.gyro * 0.01745329f, G_Ten_Axis_Data_Structure.accel,
G_Ten_Axis_Data_Structure.accel, G_Ten_Axis_Data_Structure.accel);
}
current_attitude = - atan2f(2.f*(q0*q1 + q2*q3), 1.f - 2.f*(q1*q1 + q2*q2)) * 57.29578f;
current_attitude = - asinf(2.f*(q0*q2 - q3*q1)) * 57.29578f;
current_heading = atan2f(2.f*(q0*q3 + q1*q2), 1.f - 2.f*(q2*q2 + q3*q3)) * 57.29578f;
if(current_heading < 0.f)
current_heading = - current_heading;
else
current_heading = 360 - current_heading;
////////////////////
// 读遥控器信号
sbus_rtn = sbus_frame_analy();
if(!sbus_rtn && !(G_sbus_flag & 0xC))
{
offline = 0;
desired_thro = (float)(1684 - G_sbus_channel) / 1320.f * (float)(MAX_THRO_US - 1000) + 1000.f;
desired_attitude = - (float)(G_sbus_channel - 1024) / 1024.f * MAX_ATTITUDE_DEG_F;
desired_attitude = (float)(G_sbus_channel - 1024) / 1024.f * MAX_ATTITUDE_DEG_F;
// desire_angle = to be continue...
}
else if(sbus_rtn != 1)
offline = 1;
////////////////////
// PID控制
if(!offline)
{
// 姿态控制
for(n = 0; n < 2; n++)
{
error = desired_attitude - current_attitude;
attitude_int_value += error * a_i_gain;
if(attitude_int_value > ATTI_INT_LIMIT_US_F)
attitude_int_value = ATTI_INT_LIMIT_US_F;
else if(attitude_int_value < - ATTI_INT_LIMIT_US_F)
attitude_int_value = - ATTI_INT_LIMIT_US_F;
attitude_trim = (error * a_p_gain) + attitude_int_value - ((current_attitude - last_attitude) * a_d_gain);
last_attitude = current_attitude;
}
// 航向控制
error = relative_error(desired_heading, current_heading);
heading_int_value += error * h_i_gain;
if(heading_int_value > HEAD_INT_LIMI_US_F)
heading_int_value = HEAD_INT_LIMI_US_F;
else if(heading_int_value < - HEAD_INT_LIMI_US_F)
heading_int_value = - HEAD_INT_LIMI_US_F;
heading_trim = (error * h_p_gain) + heading_int_value - (relative_error(current_heading, last_heading) * h_d_gain);
last_heading = current_heading;
// to be continue...
// 限制最大控制量
for(n = 0; n < 2; n++)
{
if(attitude_trim > MAX_ATTI_TRIM_US_F)
attitude_trim = MAX_ATTI_TRIM_US_F;
else if(attitude_trim < - MAX_ATTI_TRIM_US_F)
attitude_trim = - MAX_ATTI_TRIM_US_F;
}
if(heading_trim > MAX_HEAD_TRIM_US_F)
heading_trim = MAX_HEAD_TRIM_US_F;
else if(heading_trim < - MAX_HEAD_TRIM_US_F)
heading_trim = - MAX_HEAD_TRIM_US_F;
// 计算最终油门量 (期望油门、姿态控制、航向控制的总和)
thro = desired_thro + attitude_trim + heading_trim;
thro = desired_thro - attitude_trim + heading_trim;
thro = desired_thro + attitude_trim - heading_trim;
thro = desired_thro - attitude_trim - heading_trim;
// 限制油门范围
for(n = 0; n < 4; n++)
{
if(thro > MAX_THRO_US)
thro = MAX_THRO_US;
else if(thro < 1000)
thro = 1000;
}
// 更新油门量
TIM_SetCompare1(TIM3, thro);
TIM_SetCompare2(TIM3, thro);
TIM_SetCompare3(TIM3, thro);
TIM_SetCompare4(TIM3, thro);
}
else
{
TIM_SetCompare1(TIM3, 1000);
TIM_SetCompare2(TIM3, 1000);
TIM_SetCompare3(TIM3, 1000);
TIM_SetCompare4(TIM3, 1000);
}
////////////////////
// 处理上位机指令
if(!(UPPER_USART->CR3 & USART_DMAReq_Rx))
{
// 继续接收
usart_receive(UPPER_USART, 2, G_upper_cmd);
// 分析指令
if(G_upper_cmd == 0x11)
h_p_gain = (float)G_upper_cmd / 50.f;
else if(G_upper_cmd == 0x22)
h_i_gain = (float)G_upper_cmd / 5000.f;
else if(G_upper_cmd == 0x33)
h_d_gain = (float)G_upper_cmd;
}
}
}
这里说一下心得，
MPU9150是由AK8975和MPU6050两个芯片组成的，只是封装在一起，操作AK8975时请打开MPU6050的旁通模式（bypass）
MPU6050和AK8975的摆放位置是不一样的，轴向不一致
不加yaw的PID控制，是很难试飞的，会自转。其实yaw的PID很好调，我是把四轴放在锅盖上。。
http://v.youku.com/v_show/id_XNjcxODIyMTg4.html
废话完了，请看试飞视频吧~
http://v.youku.com/v_show/id_XNjcxODIwMzU2.html 支持一下！ 赞一个 哈哈 帮顶~~~！！！强烈支持~！！！ 不错，期待楼主更精彩的发帖 学习pid 呵呵，谢谢大家支持，给大家拜个晚年，祝各位DIY马上成功！
那个PID其实很简单的，要是写报告，写论文，就要多研究一下了。
我发的这个代码可能写的比较乱，再发一个PID的小论文。
还有，调姿态的平衡一般是不需要积分项的，用绳子拴着调，要是有小于10度的静差可能是绳子绑的不好造成的，我认为~。。 楼主有没有用到MPU9150的DMP？ 飞控没用DMP，之前试过DMP解姿态，MPU6050的DMP只能做6轴姿态融合，不能把AK8975的数据融合进去。
这有个帖子，我就是看这个移植成功的 http://www.amobbs.com/forum.php?mod=viewthread&tid=5538389&highlight=%E6%94%BE%E9%80%81%2BMPU6050 ，DMP算出来的四元数要除以2的31次方
官方文档上没有半点介绍DMP怎么使用的信息，好多与DMP有关的寄存器手册上都没提，只能移植它的430单片机例程，而且DMP有个问题，就是不能改陀螺仪与加速度计的满量程。感觉用DMP用的不明不白的，还不如把数据读出来自己算姿态，好多飞控也没用MPU6050 很强大！！！ 本帖最后由 zksniper 于 2014-2-12 11:56 编辑
想问下楼主，将接收到的遥控数据转化成desired的思路是什么？
遥控数据应该是通过pwm_input捕捉到的一个高电平，pid中误差error=遥控的角度-传感器测量的角度，如何将这个高电平转化为控的角度？
我看到你代码中有这个：
desired_attitude = - (float)(G_sbus_channel - 1024) / 1024.f * MAX_ATTITUDE_DEG_F;
desired_attitude = (float)(G_sbus_channel - 1024) / 1024.f * MAX_ATTITUDE_DEG_F;
但是不知道为什么要这样做？求解惑。
我看别人的代码中也有直接用：error=捕捉到的遥控信号（从pwm上升沿到下降沿的那一段）-传感器测量的角度
zhanwang_sky 发表于 2014-2-12 10:51
飞控没用DMP，之前试过DMP解姿态，MPU6050的DMP只能做6轴姿态融合，不能把AK8975的数据融合进去。
这有个帖 ...
感谢楼主！这个资料很重要，阿莫上找了很多也找不到ARM核的程序，哈哈，好人一生平安~ 冀宸 你用的是模拟iic还是？ {:lol:} 下雪了，赞一个。
看着好像挺笨重的。噪音也挺大。
本帖最后由 zhanwang_sky 于 2014-2-13 00:33 编辑
Moran_ 发表于 2014-2-12 20:06
冀宸 你用的是模拟iic还是？
硬件I2C，用DMA传的
enum i2c_Status_e i2c_sequential_read(I2C_TypeDef * I2Cx, uint8_t device_addr, uint8_t internal_addr, uint16_t len, __IO void * buffer) {
////////////////////////////////////////
DMA_Channel_TypeDef * DMA_RX;
////////////////////////////////////////
// 判断总线是否空闲
if(I2C_GetFlagStatus(I2Cx, I2C_FLAG_BUSY) == SET)
return i2c_Busy;
////////////////////////////////////////
// 配置DMA
if(I2Cx == I2C1)
DMA_RX = DMA1_Channel7;
else
DMA_RX = DMA1_Channel5;
DMA_Cmd(DMA_RX, DISABLE);
DMA_RX->CMAR = (uint32_t)buffer;
DMA_SetCurrDataCounter(DMA_RX, len);
DMA_Cmd(DMA_RX, ENABLE);
// 使能主机应答
I2C_AcknowledgeConfig(I2Cx, ENABLE);
// 最后一次传输发送NACK
I2C_DMALastTransferCmd(I2Cx, ENABLE);
////////////////////
// 产生开始条件
I2C_GenerateSTART(I2Cx, ENABLE);
while(I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT) != SUCCESS);
// 写模式器件寻址
I2C_Send7bitAddress(I2Cx, device_addr, I2C_Direction_Transmitter);
while(I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED) != SUCCESS);
// 发送内部地址
I2C_SendData(I2Cx, internal_addr);
while(I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_BYTE_TRANSMITTED) != SUCCESS);
// 再次产生开始条件
I2C_GenerateSTART(I2Cx, ENABLE);
while(I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT) != SUCCESS);
// 读模式器件寻址
I2C_Send7bitAddress(I2Cx, device_addr, I2C_Direction_Receiver);
while(I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED) != SUCCESS);
// 使能DMA请求
I2C_DMACmd(I2Cx, ENABLE);
////////////////////
// 以下工作交给DMA, DMA_TC中断中产生停止条件
return i2c_Receiving;
} 顶一个
这个片子 陀螺仪加速度磁角全部集中了？ 帮忙顶一下，{:smile:} {:lol:}{:lol:}{:lol:}{:lol:}{:lol:}{:lol:} 用6208sb的高帅富也搞代码？18mz？ 赞一个。。。。。 支持楼主，支持四轴飞行器的开放{:victory:} 楼主，请问关于S-BUS能再麻烦讲一下吗，那个连接里面下面的评论说有问题，一个是波特率的问题，还有说到一个数据块不是25Byte而是49Byte， 楼主，跪求S-BUS的读取程序，研究两天了，还是不行，现在的情况是波特率不知道设置为多少，他说是100K，但是后面又有人说是99K，数据还有大小端模式更昏了，求楼主指点， 木君之上 发表于 2014-2-21 01:39
楼主，跪求S-BUS的读取程序，研究两天了，还是不行，现在的情况是波特率不知道设置为多少，他说是100K，但 ...
就是100K，其实99K也能读到了，但是注意信号是要经过反相的，得接个反相器或者三极管也行。
初始化串口的程序如下
USART_InitStructure.USART_BaudRate = 100000;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Tx;
USART_InitStructure.USART_Parity = USART_Parity_Even;
USART_InitStructure.USART_StopBits = USART_StopBits_2;
USART_InitStructure.USART_WordLength = USART_WordLength_9b;
USART_Init(UART4, &USART_InitStructure);
// Enable USART
SART_Cmd(UART4, ENABLE);
解析各通道数据的代码如下
G_sbus_channel = (S_sbus_buffer & 0x7) << 8 | S_sbus_buffer;
G_sbus_channel = (S_sbus_buffer & 0x7) << 8 | S_sbus_buffer;
G_sbus_channel = (S_sbus_buffer & 0x3F) << 5 | S_sbus_buffer >> 3;
G_sbus_channel = (S_sbus_buffer & 0x3F) << 5 | S_sbus_buffer >> 3;
G_sbus_channel = (S_sbus_buffer & 0x1) << 10 | S_sbus_buffer << 2 | S_sbus_buffer >> 6;
G_sbus_channel = (S_sbus_buffer & 0x1) << 10 | S_sbus_buffer << 2 | S_sbus_buffer >> 6;
G_sbus_channel = (S_sbus_buffer & 0xF) << 7 | S_sbus_buffer >> 1;
G_sbus_channel = (S_sbus_buffer & 0xF) << 7 | S_sbus_buffer >> 1;
G_sbus_channel = (S_sbus_buffer & 0x7F) << 4 | S_sbus_buffer >> 4;
G_sbus_channel = (S_sbus_buffer & 0x7F) << 4 | S_sbus_buffer >> 4;
G_sbus_channel = (S_sbus_buffer & 0x3) << 9 | S_sbus_buffer << 1 | S_sbus_buffer >> 7;
G_sbus_channel = (S_sbus_buffer & 0x3) << 9 | S_sbus_buffer << 1 | S_sbus_buffer >> 7;
G_sbus_channel = (S_sbus_buffer & 0x1F) << 6 | S_sbus_buffer >> 2;
G_sbus_channel = (S_sbus_buffer & 0x1F) << 6 | S_sbus_buffer >> 2;
G_sbus_channel = S_sbus_buffer << 3 | S_sbus_buffer >> 5;
G_sbus_channel = S_sbus_buffer << 3 | S_sbus_buffer >> 5;
G_sbus_flag = S_sbus_buffer;
我定义了一个50字节的缓冲区，h是帧头地址，cc()是个函数
static uint16_t cc(uint16_t i)
{
if(i > 49)
return i - 50;
else
return i;
}
帧格式是这样的：第一字节为帧头标志0x0F。第三字节的bit2, bit1, bit0 代表第一通道的bit10, bit9, bit8 ，与第二字节的8位组成11位无符号整形数代表通道一的数据, 分辨率为2048，
第四字节的低6位与第三字节剩下5位组成第二通道的数据，以此类推。
怎么样，Futaba定义的够别扭吧~ 帧头是0x0F，帧尾是0 。这个我试过的，那个国外论坛上说帧头是0xF0，好像是他说错了.
一帧就是25字节没错，不是49字节，否则我飞机早掉下来了 zhanwang_sky 发表于 2014-2-21 22:05
帧头是0x0F，帧尾是0 。这个我试过的，那个国外论坛上说帧头是0xF0，好像是他说错了.
一帧就是25字节没错， ...
非常感谢，真的，我马上就去试试，我已经调了两天了，昨晚又弄到3点才睡，不知道为什么一直都不对，能留个qq方便以后交流吗？
我的584025452，验证问题就说阿莫论坛吧（怕乱加，所以有验证） zhanwang_sky 发表于 2014-2-21 22:01
就是100K，其实99K也能读到了，但是注意信号是要经过反相的，得接个反相器或者三极管也行。
初始化串口的 ...
简直太别扭了，哎，搞死我了，
PS我又发现一个地方我不对了，他说的是8E2，但是你的程序里面数据位是USART_WordLength_9b，用的是9位数据位，，继续看，
继续学习， 木君之上 发表于 2014-2-21 22:26
简直太别扭了，哎，搞死我了，
PS我又发现一个地方我不对了，他说的是8E2，但是你的程序里面数据位是USAR ...
数据位的最高位当做校验位，STM32就是这样的。这个参考文档上说了 zhanwang_sky 发表于 2014-2-21 22:48
数据位的最高位当做校验位，STM32就是这样的。这个参考文档上说了
恩恩，知道了，没有细看，
PS，加个好友吧，584025452 十分感谢楼主，我已经搞定了，
我的非门用的是一个三极管，共射放大电路，b级和C级的电阻用到的都是1K，E直接接地，好像听说电阻的大小影响功耗和转换速度
你好，整套字节可以发送给我吗？ 我QQ：465316129 谢谢 楼主是帅哥啊 初次入手MPU9150，还没有入门，不过还是感谢楼主 非常厉害！！ 值得大家学习！！ 哈哈收下了谢谢高手 很漂亮 支持！ 谢谢楼主了。
支持楼主 楼主好样的{:lol:}{:lol:}{:lol:}{:lol:}{:lol:} 高手！顶起
楼主很强大 又看到这个帖子了，现在我已经研究卡尔曼好久了，理论知识还是很深奥的，但是很基础，很有力 你这个太牛掰！！学习！！ Yaw还是需要掰过来吗？感觉后来有那么微微地抖动。。 实在搞不清楚互补滤波的公式和参数怎么算出来的 LZ可以给个资料么 楼主，你飞起来的日子正是我开始DIY的日子，可是到了愚人节自己的四轴是没有飞起来，自己有些迷茫了。希望你能帮我分析一下原因：X型四轴。硬件都做好了，大同小异。用的是MPU6050——DMP，进度自己认为到了PID调试阶段。现在遇到主要是两个问题。1：只加入P时，四轴处与平衡位置附近+-10度范围，P作用都很弱，偏大了就感觉回复力突增。我迅速摆向一侧，那侧电机的加速总会慢半拍。请问这是 P的值大小问题，还是角度输出滞后？？若是P大小问题，是大了还是小了？若是角度滞后PID能弥补吗？2、我用的是陀螺仪的输出作为D控制。但是陀螺仪的读数时不时会有很大的噪点，使得加入D时电机偶尔突然间爆发。冷汗都出来了。请问你有遇到过这种情况吗？是IIC读的问题（我用的是模拟IIC）还是MPU6050本来就会这样？怎样解决呢？给点建议，不胜感激！ 楼主大哥，我也用的9150，移植AHRS那个算法融合陀螺仪，加计，磁力计。
现在Yaw不太正常，确定是磁力计出了问题，我是可以读出ID的，但是为什么磁力计数据的高八位要么是0要么是255？（低八位很正常）导致磁力计的数据不超过+ -256，
磁力计的量程不是-4096---4096吗，我读出来的数据也太小了，请问你有遇到这个问题吗？恳请指教！！！ 谦谦 发表于 2014-4-1 18:23
楼主，你飞起来的日子正是我开始DIY的日子，可是到了愚人节自己的四轴是没有飞起来，自己有些迷茫了。希望 ...
不知你在调PID的时候电机的油门设的多大，我是在差不多可以起飞的油门左右调参数的，PWM信号和转速只在小范围内可以认为是线性的~
我调P参数时，在差不多可以起飞的油门下，倾斜40度左右，一放手自己能摆回来就好，不能太猛。我是没感觉到电机马力突然增大，这个你应该好好检查一下代码，
看看是时序有问题，还是计算时用到的哪个变量值溢出了什么的。
你用陀螺仪输出来调整D参数，这样效果应该更好才对。至于电机转速突然变化，我觉得不是传感器噪声的问题。
DMP有个挺恶心的毛病，如果使能校准功能，一上电的时候必须保证传感器水平且静止，他会把这时输出作为零点漂移，如果传感器没放平的话，程序就不能计算出正确姿态~
加油，不要迷茫~飞控做到刚好能飞起来这个程度不是很难，但是也用到单片机不少资源了，像定时器, I2C, 串口，还要熟悉传感器，综合性还是比较强的。
我做这个之前是对STM32的外设很了解，也用过MPU6050的DMP。寒假用了三周左右让他飞起来的。感觉做这个，大部分时间都用在写“驱动程序”上了，值得研究的算法都没弄明白，
一个月没弄这个了...
怎么说有兴趣就好，要做的话还是得一步步耐心调试，先把一种单片机玩熟了再做这个就容易了，希望你做的比我更好！
silence2455 发表于 2014-4-2 09:26
楼主大哥，我也用的9150，移植AHRS那个算法融合陀螺仪，加计，磁力计。
现在Yaw不太正常，确定是磁力计出了 ...
这个我没遇到过~要是能读出ID那就不是I2C总线的问题。
你确定你的操作步骤对吗，先向CNTL寄存器写01进入单次测量模式，然后等上至少8ms，再读 zhanwang_sky 发表于 2014-4-4 23:16
不知你在调PID的时候电机的油门设的多大，我是在差不多可以起飞的油门左右调参数的，PWM信号和转速只在小 ...
楼主，非常感谢你细心的回答和鼓励。给你发帖后，我也在反思和测试。偶然在*度百科里头看到四轴真正的难度在于对姿态解算。然后不断对DMP通过FIFO输出的角度和陀螺仪波形，与非DMP下的角度和陀螺仪波形。我发现：1、DMP模式下也可以通过读寄存器获得陀螺仪波形，然后与从FIFO中读出的波形对比，后者明显滞后，而且存在噪点。原因还不清楚。（使用同一IIC驱动读的，时序没有问题。）2.四轴作用迟缓与DMP的输出的角度也有关系，可能是FIFO的原因，DMP的角度变化与寄存器中读出的陀螺仪数据变化时刻总会慢100ms左右。
现在，我放弃DMP了，直接取出加速度和陀螺仪数据，通过四元数算法融合后转成欧拉角。效果很好没有突变的值。从解算出姿态到能勉强飞起来只用了一天。哎，姿态解算很关键啊。
还有，你的PID调试倒提醒了我。我调试时是让四轴在平衡木上，给较小的油门，这样做真的跟试飞时不一样。
再次感谢你的帮助。四轴还在努力中。。。 谦谦 发表于 2014-4-5 16:26
楼主，非常感谢你细心的回答和鼓励。给你发帖后，我也在反思和测试。偶然在*度百科里头看到四轴真正的难 ...
呵呵，恭喜~~
我觉得DMP这功能设计的很不地道，Invensense在宣传时强调他的DMP多好多好，可是寄存器文档上连个字都没提，只能移植它的430例程，
叫程序员自己猜去，这样的功能咱不用也罢！再说好多飞控也都是自己算的姿态，APM还支持输出模拟信号的陀螺仪呢，网上有好多开源的姿态解算程序，
算法大家可以一起讨论，这才是DIY的乐趣所在嘛~ 支持一下！ 感谢楼主分享。同时有几个问题想请教下楼主。
1.楼主你用的遥控器接收机是Futaba 6208，什么s。bus之类的我不太了解。我用的遥控器是天地飞07，我读取三个通道的PWM高电平时间【1000--2000us】作为三个姿态角的输入，以1500为中立点，与中立的的差值除以20作为目标姿态角，范围为【-25°，+25°】，这样是否可行？
2.在姿态解算中，磁力计测量的是什么参数？磁力计的作用是什么？你给的 “互补滤波请参考 这里有关于四元数跟互补滤波的详细资料和现成代码~~”中，里面有两个姿态解算的函数，一个用的是加速度计，磁力计，陀螺仪，还有一个没用磁力计，我分别试验了一下，发现两者基本没有区别。那请问加上磁力计有什么优势吗？
3.姿态解算中的滤波算法不是很了解。你给的代码中的滤波代码如下：
halfvx = q1 * q3 - q0 * q2;
halfvy = q0 * q1 + q2 * q3;
//1==q0^2+q1^2+q3^2+q4^2; 做一个代数运算 目的是减少计算量
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);
//使用PI来纠正误差 叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。
//特殊的互补滤波
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f)
{
integralFBx += twoKi * halfex * dt;// integral error scaled by Ki
integralFBy += twoKi * halfey * dt;
integralFBz += twoKi * halfez * dt;
gx += integralFBx;// apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else
{
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
它是用机体坐标和加速度计数据的叉乘，然后PI处理用以纠正陀螺仪的偏移误差。我之前也看过互补滤波器的论文，有段介绍如下：
根据加速度计测定的姿态角其测量误差不随时间累加，但在短期内精度较差; 而根据陀螺仪测定的姿态角，由于陀螺漂移的影响，一段时间后精度降低，但短期内可提供高动态的姿态数据。利用两者在频域上的互补特性，采用互补滤波器对二者进行数据融合可提高姿态测量的精度和动态响应。其中， K/（s + K）具有低通特性， s/(s + K)具有高通特性。由于互补滤波器对陀螺仪的高通作用，陀螺仪低频漂移的影响可以得到很好的抑制，而低通作用则可以很好抑制加速度计的噪声。适当地选择K 值可以使系统具有合理的截止频
率。式( 16) 的时域形式为^θ = K( θref － θ^) + ωm
从上述代码的实现我感觉和论文的介绍没什么联系，所以不太明白。
希望能得到楼主的解答，谢谢。
1.我复制的代码，程序编译没有问题，但是出现的角度结果是nan，我查看了一下，nan表示的好像是什么逻辑错误，但我是直接复制的代码啊？
2.姿态解算中陀螺仪的输入是rad/s，加速度计的输入是ADC直接转化的结果吧？ 楼主，我想问下，你是X模式，还是十字模式，十字模式还好弄，X模式，调试的时候怎么搞？想不出单轴向调试的样子。。。 赞一个，感谢分享哦 谢谢楼主分享，mark标记一下 你给的那个互补滤波的网址我去了 可是英文 始终是我的痛啊 还有 下载代码那个 貌似要什么证书？ 搞不懂楼主好人 给我发一份吧 嘻嘻 我只用了一个MPU6050 不知能不能飞啊。。。 690800659@qq.com MPU9150飞控，有时间也玩一下。 楼主，请问你的四轴是X模式 飞行的还是 +模式 飞行的？希望帮忙解答一下，谢谢！！ 谢谢楼主分享！ 终于找到这个帖子了，mark下。 mark。。。。。{:lol:} 木君之上 发表于 2014-3-29 22:37
又看到这个帖子了，现在我已经研究卡尔曼好久了，理论知识还是很深奥的，但是很基础，很有力 ...
概率论与数理统计线性代数随机信号分析 最后就是现代数字信号处理这几门都研究一下 mark。。。。 向楼主学习！ 请问楼主，机架是那种，，，可否叉宝
LYY 发表于 2014-4-25 10:29
1.我复制的代码，程序编译没有问题，但是出现的角度结果是nan，我查看了一下，nan表示的好像是什么逻辑错误 ...
你解决了吗？我输出的也是nan
看起来很重的样子，大概多少克 cdysjw 发表于 2014-6-25 13:08
你解决了吗？我输出的也是nan
这段时间被老师叫去做别的事情，还没有解决呢 楼主是好人{:lol:} 刚开始做，标记下，硬件搭好再来看 标记一下， 标记一下。 学习一下了， mark! 谢谢楼主的参考资料 赞一个，飞的能再快点不啊？ 楼主很厉害。、 Mark,正打算玩玩 我有一遥控飞机的遥控器，液晶的，不知该如何读取控制编码？ 学习 不错，我自己打样硬件用APM了。没敢自己写。 以后有时间也学着做做四轴、。。。。。 大牛不少。。 mark!!!!!!!!!!!!!!!!!!!!!!!!1 楼主厉害！ 很厉害，标记一下，正在搞这个东西，用的是MPU6050 膜拜！！学习一下！ 感谢分享，标记一下。 标记一下，学习 螺旋桨上是用的两个M5防滑螺母吗？我那个原装的螺母很容易射桨...，差点把小伙伴伤到。 赞一个，谢谢分享！ 牛人！谢谢分享！
啥时候再做一个地面站？ 必须赞一个，留着将来学习 顶，感谢分享。。。 下载了，谢谢！ 楼主不错，顶一下 mark
页:
[1]
2