|
本帖最后由 zhanwang_sky 于 2014-2-12 11:09 编辑
先上个成品图
简述一下飞控的硬件组成:
1、主控芯片用STM32F107VCT6,用的最小系统板
2、姿态传感器用的MPU9150
3、遥控器接收机是Futaba 6208,直接读它的S.Bus信号,关于S.Bus的详细信息请看
http://mbed.org/users/Digixx/not ... 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[2], current_heading,
- desired_attitude[2] = {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[2] = {0.f}, last_heading = 0.f,
- attitude_int_value[2] = {0.f}, heading_int_value = 0.f,
- error;
- // PID调整量
- float attitude_trim[2], heading_trim;
- // 控制量
- uint16_t thro[4];
- // 状态变量
- 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] * 0.01745329f, G_Ten_Axis_Data_Structure.gyro[1] * 0.01745329f, G_Ten_Axis_Data_Structure.gyro[2] * 0.01745329f,
- G_Ten_Axis_Data_Structure.accel[0], G_Ten_Axis_Data_Structure.accel[1], G_Ten_Axis_Data_Structure.accel[2],
- G_Ten_Axis_Data_Structure.mag[0], G_Ten_Axis_Data_Structure.mag[1], G_Ten_Axis_Data_Structure.mag[2]);
- }
- else
- {
- MadgwickAHRSupdateIMU(G_Ten_Axis_Data_Structure.gyro[0] * 0.01745329f, G_Ten_Axis_Data_Structure.gyro[1] * 0.01745329f,
- G_Ten_Axis_Data_Structure.gyro[2] * 0.01745329f, G_Ten_Axis_Data_Structure.accel[0],
- G_Ten_Axis_Data_Structure.accel[1], G_Ten_Axis_Data_Structure.accel[2]);
- }
- current_attitude[0] = - atan2f(2.f*(q0*q1 + q2*q3), 1.f - 2.f*(q1*q1 + q2*q2)) * 57.29578f;
- current_attitude[1] = - 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[2]) / 1320.f * (float)(MAX_THRO_US - 1000) + 1000.f;
- desired_attitude[0] = - (float)(G_sbus_channel[0] - 1024) / 1024.f * MAX_ATTITUDE_DEG_F;
- desired_attitude[1] = (float)(G_sbus_channel[1] - 1024) / 1024.f * MAX_ATTITUDE_DEG_F;
- // desire_angle[2] = to be continue...
- }
- else if(sbus_rtn != 1)
- offline = 1;
- ////////////////////
- // PID控制
- if(!offline)
- {
- // 姿态控制
- for(n = 0; n < 2; n++)
- {
- error = desired_attitude[n] - current_attitude[n];
- attitude_int_value[n] += error * a_i_gain;
- if(attitude_int_value[n] > ATTI_INT_LIMIT_US_F)
- attitude_int_value[n] = ATTI_INT_LIMIT_US_F;
- else if(attitude_int_value[n] < - ATTI_INT_LIMIT_US_F)
- attitude_int_value[n] = - ATTI_INT_LIMIT_US_F;
- attitude_trim[n] = (error * a_p_gain) + attitude_int_value[n] - ((current_attitude[n] - last_attitude[n]) * a_d_gain);
- last_attitude[n] = current_attitude[n];
- }
- // 航向控制
- 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[n] > MAX_ATTI_TRIM_US_F)
- attitude_trim[n] = MAX_ATTI_TRIM_US_F;
- else if(attitude_trim[n] < - MAX_ATTI_TRIM_US_F)
- attitude_trim[n] = - 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[0] = desired_thro + attitude_trim[0] + heading_trim;
- thro[1] = desired_thro - attitude_trim[0] + heading_trim;
- thro[2] = desired_thro + attitude_trim[1] - heading_trim;
- thro[3] = desired_thro - attitude_trim[1] - heading_trim;
- // 限制油门范围
- for(n = 0; n < 4; n++)
- {
- if(thro[n] > MAX_THRO_US)
- thro[n] = MAX_THRO_US;
- else if(thro[n] < 1000)
- thro[n] = 1000;
- }
- // 更新油门量
- TIM_SetCompare1(TIM3, thro[0]);
- TIM_SetCompare2(TIM3, thro[1]);
- TIM_SetCompare3(TIM3, thro[2]);
- TIM_SetCompare4(TIM3, thro[3]);
- }
- 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[0] == 0x11)
- h_p_gain = (float)G_upper_cmd[1] / 50.f;
- else if(G_upper_cmd[0] == 0x22)
- h_i_gain = (float)G_upper_cmd[1] / 5000.f;
- else if(G_upper_cmd[0] == 0x33)
- h_d_gain = (float)G_upper_cmd[1];
- }
- }
- }
复制代码
这里说一下心得,
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 |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
阿莫论坛20周年了!感谢大家的支持与爱护!!
你熬了10碗粥,别人一桶水倒进去,淘走90碗,剩下10碗给你,你看似没亏,其实你那10碗已经没有之前的裹腹了,人家的一桶水换90碗,继续卖。说白了,通货膨胀就是,你的钱是挣来的,他的钱是印来的,掺和在一起,你的钱就贬值了。
|