CINXE.COM

<!DOCTYPE html> <html> <head> <meta charset="UTF-8" /> <title>四轴飞行器姿态控制算法注释 -测控技术在线自动化技术 CK365测控网</title> <meta http-equiv="x-ua-compatible" content="IE=9,IE=10,IE=edge,chrome=1"/> <meta name="generator" content="DONNY B2B - "/> <meta name="keyword" content="四轴飞行器姿态控制算法注释,四轴飞行器,导航/遥测遥控" /> <meta name="description" content="四轴飞行器姿态控制算法注释" /> <meta http-equiv="mobile-agent" content="format=xhtml; url=http://m.ck365.cn/anli/show.php?itemid=56418"> <meta http-equiv="mobile-agent" content="format=html5; url=http://m.ck365.cn/anli/show.php?itemid=56418"> <meta name="robots" content="all" /> <meta name="googlebot" content="all" /> <meta name="baiduspider" content="all" /> <link rel="archives" title="CK365测控网" href="http://www.ck365.cn/archiver/"/> <link rel="stylesheet" type="text/css" href="http://www.ck365.cn/static/skin/default/style.css?time=20210501" /> <link rel="stylesheet" type="text/css" href="http://www.ck365.cn/static/skin/default/article.css?time=20210501"/> <script type="text/javascript" src="http://www.ck365.cn/static/script/config.js"></script>  <script type="text/javascript" src="http://www.ck365.cn/static/script/jquery-1.5.2.min.js"></script>   <script type="text/javascript" src="http://www.ck365.cn/static/script/jquery-2.1.1.min.js"></script>  <script type="text/javascript" src="http://www.ck365.cn/static/script/common.js"></script> <script type="text/javascript">var L = new Array(); L['system_tips'] = '系统提示'; L['ok'] = '确定'; L['cancel'] = '取消'; L['upload'] = '上传'; L['uploading'] = '上传中..'; L['upload_file'] = '上传文件'; L['del_file'] = '删除文件'; L['delete_file'] = '删除源文件'; L['upload_img'] = '上传图片'; L['allow'] = '限制为:'; L['preview_img'] = '图片预览'; L['choose_img'] = '选择图片'; L['image_cut'] = '图片裁剪'; L['width'] = '宽度'; L['height'] = '高度'; L['up_local'] = '本地图片'; L['up_remote'] = '网络图片'; L['type_imgurl'] = '请填写图片网址'; L['pleass_choose'] = '请选择'; L['pleass_fill_in'] = '请填写'; L['choose_file'] = '请选择文件'; L['choose_files'] = '选择文件'; L['choose_item'] = '选择信息'; L['choose_vote'] = '选择投票'; L['dialog_user'] = '会员{V0}资料'; L['dialog_admin'] = '管理员{V0}资料'; L['empty_img'] = '不可预览，图片地址为空'; L['empty_img_del'] = '图片地址为空'; L['confirm_del'] = '确定要删除吗？此操作将不可撤销'; L['file_img_del'] = '是否同时删除服务器中的文件'; L['type_category'] = '请输入分类名称或简称，例如：计算机'; L['related_found'] = '为您找到以下相关分类，请选择：'; L['related_not_found'] = '未找到相关分类，请调整名称'; L['type_valid_filepath'] = '请输入正确的文件路径'; L['wysiwyg_mode'] = '请切换到设计模式'; L['str_captcha'] = '点击显示'; L['str_delete'] = '删除'; L['more_open'] = '展开'; L['more_close'] = '收起'; L['browser_unfavorite'] = '浏览器不支持，请您手动添加！Ctrl+D'; L['dialog_close'] = '关闭[快捷键Esc]'; L['tpl_add'] = '新建模板'; L['tpl_edit'] = '修改模板'; L['loading'] = '请稍候...'; L['album_upload_limit'] = '您所在的会员组每条信息最多上传 {V0} 张相册图片'; L['resize_system'] = '系统裁剪'; L['resize_manual'] = '手动裁剪'; L['image_frist'] = '第一页'; L['image_last'] = '最后一页'; L['file_choose'] = '浏览文件'; L['file_not_select'] = '未选择文件'; L['system_reload'] = '加载中，请稍候...'; L['op_reason'] = '操作原因'; L['op_reason_null'] = '请填写操作原因'; L['choose_date'] = '选择日期'; L['date'] = '{V0}月{V1}日'; L['Week'] = '星期'; L['Sun'] = '日'; L['Mon'] = '一'; L['Tue'] = '二'; L['Wed'] = '三'; L['Thu'] = '四'; L['Fri'] = '五'; L['Sat'] = '六'; L['Months'] = ['一','二','三','四','五','六','七','八','九','十','十一','十二']; L['prev_year'] = '前一年'; L['next_year'] = '后一年'; L['prev_month'] = '上一月'; L['next_month'] = '下一月'; L['ca_year_msg'] = '可以直接填写年份，双击鼠标左键清空'; L['ca_tips_msg'] = '点击鼠标改变值(左键↑，右键↓)'; L['ca_hour_msg'] = '可以直接填写小时，双击鼠标左键清空', L['ca_minute_msg'] = '可以直接填写分钟，双击鼠标左键清空', L['ca_second_msg'] = '可以直接填写秒数，双击鼠标左键清空', L['time_not_within'] = '所选时间不在范围内'; L['Day'] = '天'; L['Hour'] = '时'; L['Minute'] = '分'; L['Second'] = '秒'; L['Uploaded'] = '已上传'; L['Speed'] = '速度'; L['Residue'] = '剩余时间'; L['empty_user'] = '用户名不能为空'; L['empty_template'] = '模板内容为空'; L['empty_pass'] = '用户密码不能为空'; L['empty_dpass'] = '确认密码不能为空'; L['dpass_error'] = '两次输入的密码不一致'; L['empty_yzm'] = '验证码不能为空'; L['err_yzm'] = '验证码填写不正确'; L['member_truename_null'] = '真实姓名不能为空'; L['member_email_null'] = 'Email不能为空'; L['member_mobile_null'] = '移动电话不能为空'; L['member_telephone_null'] = '固定电话不能问空'; L['member_company_null'] = '单位名称不能为空'; L['member_area_null'] = '请选择所在地区'; L['username_null'] = '请填写用户名'; L['address_null'] = '请填写详细地址'; L['postcode_null'] = '请填写邮政编码'; L['truename_null'] = '请填写真实姓名'; L['mobile_null'] = '请填写手机号码'; L['telephone_null'] = '请填写电话号码'; L['max_mode'] = '最多可选{V0}种经营模式'; L['max_cate'] = '最多可添加{V0}个分类'; L['choose_category'] = '请选择分类'; L['category_chosen'] = '已添加过此分类'; L['baike_ckzl_name'] = '请填写参考资料的名称，可以是书籍、文献，或网站的名称。（必填）'; L['baike_ckzl_url'] = '请填写详细网址，以http://或https://开头'; L['baike_ckzl_name_null'] = '参考资料名称不能为空'; L['edit'] = '编辑'; L['delete'] = '删除'; L['fck_zoom'] = '点击增加高度右击缩小高度'; L['zoom_out'] = '缩小高度'; L['zoom_in'] = '增加高度'; L['data_recovery'] = '数据恢复'; L['save_draft'] = '暂存草稿'; L['if_cover_data'] = '发现 {V0} 保存的数据，是否覆盖当前数据？'; L['no_data'] = '抱歉未找到保存的数据'; L['at_least_10_letters'] = '请至少输入10个字，当前已输入{V0}字'; L['stop_auto_save'] = '系统会自动保存草稿，此操作将中止系统自动保存功能，确定要继续吗？'; L['draft_auto_saved'] = '{V0}点{V1}分{V2}秒系统自动保存了草稿'; L['draft_saved'] = '草稿已保存'; L['stop_save'] = '关闭保存'; L['draft_save_stopped'] = '草稿保存已停止'; L['start_save'] = '开启保存'; L['upload_attach'] = '插入附件'; L['sell_compare_num'] = '对比的产品不能超过8款'; L['compare_sell_min'] = '您至少要选择2款产品'; L['sell_tip_delete'] = '移除该产品'; L['sell_compare'] = '参数对比'; L['compare_clear'] = '清空对比栏'; L['compare_close'] = '关闭对比栏'; L['tip_image'] = '提示：您的浏览器支持直接拖拽本地图片至编辑器或者直接Ctrl+C粘贴QQ截图里的图像，赶快试试吧！'; L['tip_know'] = '我知道了'; L['tip_zh_cn'] = '简'; L['tip_zh'] = '繁'; L['strips'] = '格式化'; L['tip_ifblank'] = '段前空格'; L['click_open'] = '点击打开原图'; L['keyword_value'] = '输入关键词'; L['keyword_message'] = '请输入关键词'; L['Sunday'] = '星期日'; L['Monday'] = '星期一'; L['Tuesday'] = '星期二'; L['Wednesday'] = '星期三'; L['Thursday'] = '星期四'; L['Friday'] = '星期五'; L['Saturday'] = '星期六'; L['show_date'] = '今天是 {V0}月{V1}日 {V2}'; L['close_letter'] = '收起'; L['last_page'] = '已至最后一页'; L['quote_value'] = '输入产品名'; L['iso_tips_video'] = '暂不支持此视频，请在电脑上观看'; L['popular_search_terms'] = '热门搜索：'; L['search_tips_close'] = '关闭'; L['max_mode'] = '最多可选{V0}种经营模式'; L['max_cate'] = '最多可添加{V0}个分类'; L['choose_category'] = '请选择分类'; L['category_chosen'] = '已添加过此分类'; L['click_side_close'] = '点击关闭侧栏'; L['click_side_open'] = '点击打开侧栏'; L['pless_input_username'] = '请输入用户登陆名'; L['pless_input_captcha'] = '请输入验证码'; L['pless_input_password'] = '请输入登陆密码'; L['status_lock_screen'] = '锁屏状态，请输入密码解锁'; L['password'] = '密码'; L['new_password_pass'] = '新登录密码最少6位，当前已输入{V0}位'; L['new_repeat_password_pass'] = '重复新密码与新登录密码不一致'; L['old_password_pass'] = '现有密码最少6位，当前已输入{V0}位'; L['mail_test_address'] = '请先输入一个接收测试邮件的邮件地址'; L['mail_password_decode'] = '密码已经加密，请重新输入密码测试'; L['template_name_sucess'] = '模板名修改成功'; L['admin_catdir_pass_'] = '请填写分类目录'; L['admin_catname_pass_'] = '请填写分类名称'; L['admim_catdir_pass'] = '请填写分类目录'; L['admin_cate_copy_formid_pass'] = '请选择来源模块'; L['admin_action_check'] = '此操作不可撤销，确定要执行吗？'; L['admin_fields_name_pass'] = '请填写字段'; L['admin_fields_title_pass'] = '请填写字段名称'; L['input_keyword'] = '请输入搜索关键词'; L['sync_sina'] = '同步到新浪微博'; L['sync_qq'] = '同步到腾讯微博'; L['sync_qzone'] = '同步到QQ空间'; L['sync_login_sina'] = '您还没有登录新浪微博或者登录已经失效，是否现在登录?'; L['sync_login_qq'] = '您还没有QQ登录或者QQ登录已经失效，是否现在登录?'; L['chat_msg_0'] = '对方已经中断了对话'; L['chat_msg_1'] = '对方拒绝与您对话'; L['chat_msg_2'] = '正在等待对方接受对话'; L['chat_msg_4'] = '对话超时已自动断开'; L['chat_msg_5'] = '对话不存在或中断'; L['chat_word_pass'] = '发送内容不能为空，请输入'; L['chat_word_length'] = '最多输入{V0}字，当前已输入{V1}字'; L['chat_time_query'] = '您的发言过快，请稍后再发'; L['chat_word_max'] = '发送内容过长'; L['chat_send_error'] = '发送失败，请重试'; L['chat_touser_move'] = '对方离开了对话'; L['chat_new_title'] = '【新消息({V0})】'; L['chat_end_tips'] = '确定要中断聊天吗？'; L['qrcode'] = '扫一扫，直接在手机上打开'; L['qrcode_tips'] = '推荐微信、QQ扫一扫等扫码工具'; L['big_file_upload_msg_0'] = '正在发送至服务器，请稍后...'; L['big_file_upload_msg_1'] = '服务器存在相同文件，快速上传成功。'; L['big_file_upload_msg_2'] = '本地文件已经改动，请重新上传'; L['big_file_upload_msg_3'] = '错误的来源'; L['big_file_upload_msg_4'] = '加载列表失败'; L['big_file_upload_msg_5'] = '重新选择本地文件继续上传'; L['continue_upload'] = '续传'; L['big_file_upload_msg_6'] = '请选中要删除的文件'; L['big_file_upload_msg_7'] = '删除记录同时删除文件，此操作不可撤销！'; L['big_file_upload_limit'] = '文件 {V0} 大小超出限制！'; L['big_file_upload_filetype'] = '文件 {V0} 类型不允许！'; L['unknow_error'] = '未知错误'; L['str_continue'] = '继续'; L['str_suspend'] = '暂停'; L['str_used'] = '使用'; L['upload_complete'] = '上传完成'; L['upload_error'] = '上传失败'; L['swfu_upload_limit'] = '图片选择过多,最多可选择 {V0} 张'; L['swfu_upload_progress'] = '正在处理...'; L['swfu_uploading'] = '正在上传...'; L['swfu_upload_success'] = '处理完毕'; L['swfu_upload_finish'] = '上传完毕'; L['swfu_upload_remove'] = '确定要移除此图片吗？'; L['swfu_upload_action'] = '点左键插入，点右键移除'; L['report_type'] = '不良信息'; L['count_down'] = '倒计时：'; L['please_set'] = '请配置'; L['activity_end'] = '活动已结束！'; L['activity_start'] = '进行中！'; L['required'] = '必填'; L['radio_name'] = '单选项'; L['check_name'] = '多选项'; L['file_name'] = '图片附件'; L['activity_fields_note'] = '请填写{V0}名称'; L['please_wait'] = '请稍候...'; L['jqeditor_clear'] = '确定要退出编辑页面吗？'; L['jqeditor_submit'] = '发布'; L['jqeditor_giveup'] = '放弃'; L['jqeditor_status'] = '状态：正在编辑中...'; L['activity_limit'] = '不限制人数'; L['activity_orders'] = '已报名';</script> <script type="text/javascript" src="http://www.ck365.cn/static/script/page.js?time=20210501"></script> <script type="text/javascript"> GoMobile('http://m.ck365.cn/anli/show.php?itemid=56418'); </script> </head> <body><style type="text/css"> .head_l li{margin:0 0 0 10px;} .head_l li a{margin-right:10px;} .h_mobile,.h_qrcode{padding: 0 6px 0 15px} .h_mobile{background: url('http://www.ck365.cn/static/skin/default/image/icon-mobile-gray.png') no-repeat 0 center;} .h_qrcode{background: url('http://www.ck365.cn/static/skin/default/image/icon-weixin-gray.png') no-repeat 0 center;text-align:right;padding-right:0;margin-right:0;} .head_l li.h_qrcode a{margin-right:0;} .head_l li.h_qrcode{margin-left:5px;} .head{background-color:#eeeeee;} .head_m{color:#595959;} .head_m a:link, .head_m a:visited, .head_m a:active, .head_m a:hover{color:#595959} .item-top-right{margin-left:15px;} </style> <div class="head" id="head"> <div class="head_m flex"> <div class="flex-full" id="xzgcms_member"></div> <div class="head_l"> <ul> <li><a href="http://www.ck365.cn/">首页</a>|</li> <li><a href="http://www.ck365.cn/news/">资讯</a>|</li> <li><a href="http://www.ck365.cn/product/">新品</a>|</li> <li><a href="http://www.ck365.cn/company/">品牌</a>|</li> <li class="menuon"><a href="http://www.ck365.cn/anli/">技术</a>|</li> <li><a href="http://www.ck365.cn/activity/">活动</a>|</li> <li><a href="http://www.ck365.cn/baike/">百科</a></li> <li class="h_mobile"><a href="javascript:MyQrcode();void(0);" rel="nofollow">手机版</a></li><li class="h_qrcode"><a href="javascript:Weixin();void(0);">微信</a></li> </ul> </div> </div> </div> <div class="m head_s" id="xzgcms_space"></div> <div id="xzgcms_qrcode" style="display:none;"></div><div class="m"><div id="search_tips" style="display:none;"></div></div><div class="b15"> </div> <div class="head-top-sub flex m flex-align-center"> <div class="logo"><a href="http://www.ck365.cn/"><img src="http://www.ck365.cn/static/skin/default/image/logo-exhibit.png" /></a></div> <a href="http://www.ck365.cn/anli/"><div class="header-title">技术中心</div></a> <div class="flex-full"></div> <div class="search-box-sub"> <form action="http://www.ck365.cn/anli/search.php" id="fsearch"> <div class="search-items flex"> <input type="text" size="60" name="kw" value="" placeholder="请输入关键字"/> <input type="submit" value=""/> </div> </form> </div> </div> <div class="b15"> </div> <div class="m"> <div class="menus-sub m"> <ul> <li><a href="http://www.ck365.cn/anli/">技术首页</a></li> <li><a href="http://www.ck365.cn/anli/leibie/1/">解决方案</a></li> <li><a href="http://www.ck365.cn/anli/leibie/2/">典型应用</a></li> <li class="on"><a href="http://www.ck365.cn/anli/leibie/3/">相关知识</a></li> <li><a href="http://www.ck365.cn/anli/leibie/4/">电路图</a></li> <li><a href="http://www.ck365.cn/download/">资料下载</a></li> </ul> </div> <div class="b15"> </div></div> <script type="text/javascript">var module_id= 25,item_id=56418,content_id='content',img_max_width=550;</script> <div class="m m3"> <div class="m3l"> <div class="nav"><a href="http://www.ck365.cn/">首页</a> > <a href="http://www.ck365.cn/anli/">技术</a> > <a href="http://www.ck365.cn/anli/leibie/3/">相关知识</a></div> <div class="b20 bd-t"></div> <h1 class="title" id="title">四轴飞行器姿态控制算法注释</h1><div class="b20"></div> <div class="info"><img src="http://www.ck365.cn/static/skin/default/image/ico-zoomin.png" width="16" height="16" title="放大字体" class="c_p" onclick="fontZoom('+', 'article');"/>  <img src="http://www.ck365.cn/static/skin/default/image/ico-zoomout.png" width="16" height="16" title="缩小字体" class="c_p" onclick="fontZoom('-', 'article');"/> 日期：2015-05-04     </div> <div id="content"><div class="content" id="article">从开始做四轴到现在，已经累计使用了三个月的时间，从开始的尝试用四元数法进行姿态检测，到接着使用的卡尔曼滤波算法，我们走过了很多弯路，我在从上周开始了对德国人四轴代码的研究和移植，发现德国人的代码的确有他的独到之处，改变了很多我对模型的想法，因为本人是第一次尝试着制作模型，因此感觉很多想法还是比较简单。经过了一周的时间，我将德国人的代码翻译并移植到了我目前的四轴上，并进行了调试，今天，专门请到了一个飞直升机的教练，对我们的四轴进行试飞，并与一个华科尔的四轴进行了现场比较，现在我们四轴的稳定性已经达到了商品四轴的程度。下面是我这一周时间内对德国人代码的一些理解： 德国人代码中的姿态检测算法： 首先，将陀螺仪和加速度及的测量值减常值误差，得到角速度和加速度，并对角速度进行积分，然后对陀螺仪积分和加速度计的数值进行融合。融合分为两部分，实时融合和长期融合,实时融合每一次算法周期都要执行，而长期融合没256个检测周期执行一次，(注意检测周期小于控制周期的2ms) 实时融合： 1.将陀螺仪积分和加表滤波后的值做差; 2.按照情况对差值进行衰减，并作限幅处理; 3.将衰减值加入到角度中。 长期融合： 长期融合主要包括两个部分，一是对角速度的漂移进行估计(估计值是要在每一个控制周期都耦合到角度中的)，二是对陀螺仪的常值误差(也就是陀螺仪中立点)进行实时的修正。 1.将陀螺仪积分的积分和加速度积分做差(PS：为什么这里要使用加表积分和陀螺仪积分的积分，因为在256个检测周期内，有一些加速度计的值含有有害的加速度分量，如果只使用一个时刻的加表值对陀螺仪漂移进行估计，显然估计值不会准确，使用多个周期的值进行叠加后做座平均处理，可以减小随机的有害加速度对估计陀螺仪漂移过程中所锁产生的影响) 2.将上面两个值进行衰减，得到估计的陀螺仪漂移 3.对使考虑了陀螺仪漂移和不考虑陀螺仪漂移得到的角度做差，如果这两个值较大，说明陀螺仪在前段时间内测到的角速率不够准确，需要对差值误差(也就是陀螺仪中立点)进行修正，修正幅度和差值有关 长期融合十分关键，如果不能对陀螺仪漂移做修正，则系统运行一段时间后，速率环的稳定性会降低。 下面比较一下德国四轴中姿态检测部分和卡尔曼滤波之间的关系： 卡尔曼滤波是一种线性系统的最优估计滤波方法。对于本系统而言，使用卡尔曼滤波的作用是通过对系统状态量的估计，和通过加速度计测量值对系统状态进行验证，从而得到该系统的最优状态量，并实时更新系统的各参数(矩阵)，而最重要的一点，改滤波器能够对陀螺仪的常值漂移进行估计，从而保证速率环的正常运行，并在加速度计敏感到各种有害加速度的时候，使姿态检测更加准确。但是卡尔曼滤波器能否工作在最优状态很大程度上取决于系统模型的准确性，模型参数的标定和系统参数的选取。然而，仅仅通过上位机观测而得到最优工作参数是不显示的，因为参数的修改会导致整个系统中很多地方发生改变，很难保证几个值都恰好为最优解，这需要扎实的理论知识，大量的测量数据和系统的仿真，通过我对卡尔曼滤波器的使用，发现要想兼顾锁有的问题，还是有一定难度的。 而德国人的姿态检测部分是在尝试使用一种简单方法去解决复杂问题，他既没有使用传统的四元数法进行姿态检测，也么有使用卡尔曼滤波。他的计算量不比最简单的卡尔曼滤程序波程序的计算量小，但与卡尔曼滤波相比，更加直观，易于理解，参数调节也更加方便。我个人理解，这个方法是在尝试着对卡尔曼滤波这一复杂相互耦合的多状态变量的线性系统状态估计过程进行了简单的解耦，从而将姿态的最优估计和陀螺仪漂移的最优估计分隔开，这样，就可以通过比较直观的观测手段对两个部分的参数进行调整，但是，这种方法的理论性肯定不如使用四元数法和卡尔曼滤波，在一些特殊的情况下还可能出现问题，但是，由于卡尔曼滤波器设计的难度，使用这种方法还是比较现实的。 控制算法： 德国人的控制算法的核心是对角速度做PI计算，P的作用是使四轴能够产生对于外界干扰的抵抗力矩，I的作用是让四轴产生一个与角度成正比的抵抗力。 如果只有P的作用，将四轴拿在手上就会发现，四轴能够抵抗外界的干扰力矩的作用，而且这个抵抗力非常快速，只要手妄图改变四轴的转速，四轴就会产生一个抵抗力矩，但是，如果用手将四轴扳过一个角度，则四轴无法自己回到水平的角度位置，这就需要I的调节作用。 对角速度做I(积分)预算实际得到的就是角度，德国人四轴里面用的也是角度值，如果四轴有一个倾斜角度，那么四轴就会自己进行调整，直到四轴的倾角为零，它所产生的抵抗力是与角度成正比的，但是，如果只有I的作用，会使四轴迅速产生振荡，因此，必须将P和I结合起来一起使用，这时候基本上就会得到德国四轴的效果了。 在对角速度进行了PI调节之后，德国人将操纵杆的值融合到结果中去，并对得到的新的值有进行了一次PI计算，这个积分参数很小，使用这个积分的作用因为，四轴在有一个非常小的倾角的情况下产生的抵抗力矩很小，无法使四轴回到水平位置，这就会导致无论怎么手动调节微调，四轴都很难做到悬停，会不停得做水平漂移运动，这就必须不停的进行调整。 下面是我给德国四轴中飞控程序的一些注释： void Piep(unsigned char Anzahl) { while(Anzahl--) { if(MotorenEin) return; //auf keinen Fall im Flug! beeptime = 100; Delay_ms(250); } } //函数:SetNeutral设定传感器发出参数的中立数值，因为有漂移所以要使其每次工作都要测量出来。 //############################################################################ // Nullwerte ermitteln /*设置中立点*/ void SetNeutral(void) //############################################################################ { /*加速度计中立点*/ NeutralAccX = 0; NeutralAccY = 0; NeutralAccZ = 0; /*陀螺仪中立点*/ AdNeutralNick = 0; AdNeutralRoll = 0; AdNeutralGier = 0; Parameter_AchsKopplung1 = 0; Parameter_AchsGegenKopplung1 = 0;。。。 /*这个地方我还没有弄得太明白，检测中立点的函数被调用了两次，但是第一次的数据好像没有保存，只用到了 第二次的数据*/ /*记录中立点*/ CalibrierMittelwert(); Delay_ms_Mess(100); /*记录中立点*/ CalibrierMittelwert(); /*既然只使用了后一次的数据，为什么要进行两次记录中立点的函数*/ if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) { if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();//如果气压表输出在 950外750内，则设定气压初始的偏差。 } /*将量测值作为陀螺仪的中立点*/ AdNeutralNick= AdWertNick; AdNeutralRoll= AdWertRoll; AdNeutralGier= AdWertGier; /*这两个参数在飞控程序中没有用到*/ StartNeutralRoll = AdNeutralRoll; StartNeutralNick = AdNeutralNick; if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) // { /*由于在函数CalibrierMittelwert()中加速度计的输出乘以了ACC_AMPLIFY，所以这里必须处以ACC_AMPLIFY， 在这段程序中，所有的对加速度计和陀螺仪的数值的衰减或者放大都是为了让 陀螺仪积分和加速度计数值在同样的角度偏差的情况下能基本匹配，如果不匹配，那么就谈不上用加速度计来补 偿陀螺仪积分了*/ NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; NeutralAccZ = Aktuell_az; } else { /*如果发现变化不大，则仍然储存上一次的*/ NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]); NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]); NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]); } /*将所有数据清零，这里带2的变量都是加入了陀螺仪漂移补偿值之后得到的角度*/ Mess_IntegralNick = 0; Mess_IntegralNick2 = 0; Mess_IntegralRoll = 0; Mess_IntegralRoll2 = 0; Mess_Integral_Gier = 0; MesswertNick = 0; MesswertRoll = 0; MesswertGier = 0; StartLuftdruck = Luftdruck; HoeheD = 0; Mess_Integral_Hoch = 0; KompassStartwert = KompassValue; GPS_Neutral(); beeptime = 50; // /*从EEPROM中读取陀螺仪积分到达90°时候的值，并储存，当得到的姿态角度大于这个范围时，说明超过了90° ，就要进行相应的处理*/ Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; ExternHoehenValue = 0; } /////////////////////////////// //函数描述：求参数的平均数值 ////////////////////////////// //############################################################################ // Bearbeitet die Messwerte void Mittelwert(void) // 根据测量值计算陀螺仪和加速度计数据 //############################################################################ { static signed long tmpl,tmpl2; /*将陀螺仪数据减去常值误差，得到实际的叫速率的倍数*/ MesswertGier = (signed int) AdNeutralGier - AdWertGier; MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; MesswertNick = (signed int) AdWertNick - AdNeutralNick; //DebugOut.Analog[26] = MesswertNick; DebugOut.Analog[28] = MesswertRoll; //加速度传感器输出 /*加速度计数据滤波，ACC_AMPLIFY=12 得到的Mittelwert_AccNick是加速度计数值的12倍*/ /*AdWertAccNick为测量值*/ // Beschleuni×gssensor ++++++++++++++++++++++++++++++++++++++++++++++++ Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;//具有滤波功能的方法，用当前加速度和上次的加速度平均 Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L; Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L; /*计算加速度计的积分，加速度计对运动十分敏感，采用加速度计积分，可以减少瞬间的运动加速度的影响*/ IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; IntegralAccZ += Aktuell_az - NeutralAccZ; // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ /*偏航方向无法进行滤波，因此直接进行积分得到偏航角度*/ Mess_Integral_Gier += MesswertGier; Mess_Integral_Gier2 += MesswertGier; /*耦合项应该是另外两个陀螺仪对这个轴上陀螺仪的影响，当四轴在稳定姿态不为水平的时候，进行偏航运动时 候所进行的补偿*/ /*假设目前的俯仰角是30°，而横滚角是0°，这时候如保持俯仰和横滚轴没有任何运动，而将偏航轴转动90° ，那么实际的俯仰角就会变为0°，横滚角就会变为30° 但是，按照目前的算法，由于俯仰和横滚方向没有运动，因此就不会有陀螺仪的积分，俯仰和横滚角是不变的 ，这就是采用陀螺仪直接积分测角度的不完善性，这时候 使用加速度计对姿态进行修正能够起到作用，但是需要一段时间，使用下面的这段话，就是将偏航轴的运动耦 合在另外两个轴上，使姿态角度能够迅速收敛到真实值上*/ /*注：使用四元数法进行姿态结算可以避免出现这种问题，但这种方法需要有准确的陀螺仪和加表的数学模型，四元数法还需要进行大量的矩阵计算， 而且对四元数姿态进行加速度计的融合不太方便*/ if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))//不在俯仰滚转控制循环中 { tmpl = Mess_IntegralNick / 4096L; tmpl *= MesswertGier; tmpl *= Parameter_AchsKopplung1; //125 tmpl /= 2048L; tmpl2 = Mess_IntegralRoll / 4096L; tmpl2 *= MesswertGier; tmpl2 *= Parameter_AchsKopplung1; tmpl2 /= 2048L; } else tmpl = tmpl2 = 0; // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ MesswertRoll += tmpl; MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; Mess_IntegralRoll2 += MesswertRoll; Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll; /*积分超过半圈的情况*/ if(Mess_IntegralRoll > Umschlag180Roll) { Mess_IntegralRoll = -(Umschlag180Roll - 10000L); Mess_IntegralRoll2 = Mess_IntegralRoll; } if(Mess_IntegralRoll <-Umschlag180Roll)//一样 { Mess_IntegralRoll = (Umschlag180Roll - 10000L); Mess_IntegralRoll2 = Mess_IntegralRoll; } if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000; if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; } else { if(AdWertRoll > 2020) MesswertRoll = +1000; if(AdWertRoll > 2034) MesswertRoll = +2000; } // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ MesswertNick -= tmpl2; MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L; Mess_IntegralNick2 += MesswertNick; /*LageKorrekturNick是通过加速度计积分和角速率积分的积分进行做差比较得到 的,*/ Mess_IntegralNick += MesswertNick - LageKorrekturNick; if(Mess_IntegralNick > Umschlag180Nick) { Mess_IntegralNick = -(Umschlag180Nick - 10000L); Mess_IntegralNick2 = Mess_IntegralNick; } if(Mess_IntegralNick <-Umschlag180Nick) { Mess_IntegralNick = (Umschlag180Nick - 10000L); Mess_IntegralNick2 = Mess_IntegralNick; } if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000; if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; } else { if(AdWertNick > 2020) MesswertNick = +1000; if(AdWertNick > 2034) MesswertNick = +2000; } //++++++++++++++++++++++++++++++++++++++++++++++++ // ADC einschalten ANALOG_ON; //重新开始模拟量的采集 //++++++++++++++++++++++++++++++++++++++++++++++++ /*上一步计算完了积分之后，现在将积分赋值，因此后面使用的就将是IntegralNick，IntegralNick2等数据了 */ Integral_Gier = Mess_Integral_Gier; IntegralNick = Mess_IntegralNick; IntegralRoll = Mess_IntegralRoll; IntegralNick2 = Mess_IntegralNick2; IntegralRoll2 = Mess_IntegralRoll2; /*这部分代码不执行，因为在EEPROM中CFG_DREHRATEN_BEGRENZER这一位为0*/ if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) { if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); } if(Poti1 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110 && Poti1) Poti1--; if(Poti2 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in [EE_Parameter.Kanalbele×g[K_POTI2]] + 110 && Poti2) Poti2--; if(Poti3 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in [EE_Parameter.Kanalbele×g[K_POTI3]] + 110 && Poti3) Poti3--; if(Poti4 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in [EE_Parameter.Kanalbele×g[K_POTI4]] + 110 && Poti4) Poti4--; if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; } //函数：校正平均值 //############################################################################ // Messwerte beim Ermitteln der Nullage /*确定零位*/ /*记录中立点*/ void CalibrierMittelwert(void) //############################################################################ { // ADC auschalten, damit die Werte sich nicht w鋒rend der Berechnung 鋘dern ANALOG_OFF; MesswertNick = AdWertNick; MesswertRoll = AdWertRoll; MesswertGier = AdWertGier; /*要乘以 ACC_AMPLIFY 是为了进行数值的匹配*/ Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; Mittelwert_AccHoch = (long)AdWertAccHoch; // ADC einschalten ANALOG_ON; //开模拟量 if(Poti1 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI1]] + 110 && Poti1) Poti1--; if(Poti2 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI2]] + 110 && Poti2) Poti2--; if(Poti3 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI3]] + 110 && Poti3) Poti3--; if(Poti4 < PPM_in[EE_Parameter.Kanalbele×g[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbele×g[K_POTI4]] + 110 && Poti4) Poti4--; if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; /*这两个数据是在对陀螺仪积分区域进行的限制，如果超过这个范围，说明就超出了+-90°的范围，则需要相应 的改变*/ Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L; } //发送电机数据 //############################################################################ // Senden der Motorwerte per I2C-Bus void SendMotorData(void) //############################################################################ { if(MOTOR_OFF || !MotorenEin)//关机或未工作 { Motor_Hinten = 0; Motor_Vorne = 0; Motor_Rechts = 0; Motor_Links = 0;//都置零 if(MotorTest[0]) Motor_Vorne = MotorTest[0]; if(MotorTest[1]) Motor_Hinten = MotorTest[1]; if(MotorTest[2]) Motor_Links = MotorTest[2]; if(MotorTest[3]) Motor_Rechts = MotorTest[3];//如果是试验就干。 } DebugOut.Analog[12] = Motor_Vorne; DebugOut.Analog[13] = Motor_Hinten; DebugOut.Analog[14] = Motor_Links; DebugOut.Analog[15] = Motor_Rechts; //Start I2C Interrupt Mode twi_state = 0; motor = 0; i2c_start(); } //函数：参数分配 //############################################################################ // Tr鋑t ggf. das Poti als Parameter ein void ParameterZuordnung(void) //############################################################################ { // /*这个宏定义的作用是：将a中的值赋给b，并将b限制在max和min之间*/ #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;} CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255); CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100); CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255); CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255); CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255); CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255); CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255); CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255); CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255); CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255); CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255); Ki = (float) Parameter_I_Faktor * 0.0001; MAX_GAS = EE_Parameter.Gas_Max; MIN_GAS = EE_Parameter.Gas_Min; } /*飞控核心*/ //############################################################################ // void MotorRegler(void) //############################################################################ { int motorwert,pd_ergebnis,h,tmp_int;//电机数值，PI算法的计算数值 int GierMischanteil,GasMischanteil;//偏航混合数值，油门混和数值 static long SummeNick=0,SummeRoll=0;//俯仰积分总和，滚转积分总和 static long sollGier = 0,tmp_long,tmp_long2;//标准偏航值， static long IntegralFehlerNick = 0;//俯仰误差积分 static long IntegralFehlerRoll = 0;//滚转误差积分 static unsigned int RcLostTimer; static unsigned char delay_neutral = 0; static unsigned char delay_einschalten = 0,delay_ausschalten = 0;//延迟接通，延迟关闭 static unsigned int modell_fliegt = 0;//飞机飞行时间 static int hoehenregler = 0;//高度调节 static char TimerWerteausgabe = 0;//时间数值 static char NeueKompassRichtungMerken = 0;//罗盘方向调整中立值 static long ausgleichNick, ausgleichRoll;//俯仰均衡，滚转均衡 /*根据测量值计算陀螺仪和加速度计数据*/ Mittelwert(); GRN_ON;//打开端口 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gaswert ermitteln//判断油门数值 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ GasMischanteil = StickGas; if(GasMischanteil < 0) GasMischanteil = 0; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Emfang schlecht//无线电故障，不好 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(SenderOkay < 100) { if(!PcZugriff) { if(BeepMuster == 0xffff) { beeptime = 15000; BeepMuster = 0x0c00; } } if(RcLostTimer) RcLostTimer--; else { MotorenEin = 0; Notlandung = 0; } ROT_ON; if(modell_fliegt > 2000) { GasMischanteil = EE_Parameter.NotGas; Notlandung = 1; PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] = 0; PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] = 0; PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] = 0; } else MotorenEin = 0; } // end of if(SenderOkay < 100) // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Emfang gut// // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ else if(SenderOkay > 140) { Notlandung = 0; RcLostTimer = EE_Parameter.NotGasZeit * 50; if(GasMischanteil > 40) { if(modell_fliegt < 0xffff) modell_fliegt++; } if((modell_fliegt < 200) || (GasMischanteil < 40)) { SummeNick = 0; SummeRoll = 0; Mess_Integral_Gier = 0; Mess_Integral_Gier2 = 0; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // auf Nullwerte kalibrieren // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if((PPM_in[EE_Parameter.Kanalbele×g[K_GAS]] > 80) && MotorenEin == 0) { if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] > 75) { if(++delay_neutral > 200) { GRN_OFF; MotorenEin = 0; delay_neutral = 0; modell_fliegt = 0; if(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] > 70 || abs(PPM_in [EE_Parameter.Kanalbele×g[K_ROLL]]) > 70) { unsigned char setting=1; if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > 70 && PPM_in [EE_Parameter.Kanalbele×g[K_NICK]] < 70) setting = 1; if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > 70 && PPM_in [EE_Parameter.Kanalbele×g[K_NICK]] > 70) setting = 2; if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] < 70 && PPM_in [EE_Parameter.Kanalbele×g[K_NICK]] > 70) setting = 3; if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] <-70 && PPM_in [EE_Parameter.Kanalbele×g[K_NICK]] > 70) setting = 4; if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] <-70 && PPM_in [EE_Parameter.Kanalbele×g[K_NICK]] < 70) setting = 5; eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting); } if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // H鰄enregelung aktiviert? { if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); } ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbele×g[0], STRUCT_PARAM_LAENGE); SetNeutral(); Piep(GetActiveParamSetNumber()); } } else if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] < -75) { if(++delay_neutral > 200) // nicht sofort { GRN_OFF; eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte l鰏chen MotorenEin = 0; delay_neutral = 0; modell_fliegt = 0; SetNeutral();//设立中性点。 eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256); eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256); eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256); eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256); Piep(GetActiveParamSetNumber()); } } else delay_neutral = 0; } // end if of if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] > 75) // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gas ist unten // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(PPM_in[EE_Parameter.Kanalbele×g[K_GAS]] < 35-120) { // Starten if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] < -75) { // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Einschalten // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(++delay_einschalten > 200) { delay_einschalten = 200; modell_fliegt = 1; MotorenEin = 1; sollGier = 0; Mess_Integral_Gier = 0; Mess_Integral_Gier2 = 0; Mess_IntegralNick = 0; Mess_IntegralRoll = 0; Mess_IntegralNick2 = IntegralNick; Mess_IntegralRoll2 = IntegralRoll; SummeNick = 0; SummeRoll = 0; } } else delay_einschalten = 0;//没事，就让其延迟关闭为0 //Auf Neutralwerte setzen // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Auschalten /*切换*/ // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]] > 75) { if(++delay_ausschalten > 200) // nicht sofort { MotorenEin = 0; delay_ausschalten = 200; modell_fliegt = 0; } } else delay_ausschalten = 0; } } // end if of else if(SenderOkay > 140) // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // neue Werte von der Funke // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(!NewPpmData-- || Notlandung) { int tmp_int; static int stick_nick,stick_roll;//俯仰杆，倾斜杆 ParameterZuordnung(); /*新老数据滤波混合，这里改变的应该是期望角位置，必须知道EE_Parameter.Stick_P的数值才可以得到滤波效 果*/ StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] * EE_Parameter.Stick_P) / 4; //新数据和老数据混合起滤波作用 /*将期望角位置的微分加入操纵杆变量上，这里必须知道EE_Parameter.Kanalbele×g[K_ROLL]的求法，和 EE_Parameter.Stick_D得数值*/ StickNick += PPM_diff[EE_Parameter.Kanalbele×g[K_NICK]] * EE_Parameter.Stick_D;//增加上微分 量，用于提高反应的快速性。 StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] * EE_Parameter.Stick_P) / 4; StickRoll += PPM_diff[EE_Parameter.Kanalbele×g[K_ROLL]] * EE_Parameter.Stick_D; StickGier = -PPM_in[EE_Parameter.Kanalbele×g[K_GIER]]; StickGas = PPM_in[EE_Parameter.Kanalbele×g[K_GAS]] + 120; /*用此记录历史上的最大给杆量，如果给杆量很小，则Max数值会不断减小，用于在后面给陀螺仪积分做补偿时 ，对加速度计数据和陀螺仪积分的差值做衰减*/ if(abs(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]]) > MaxStickNick) MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]]); else MaxStickNick--; if(abs(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]]) > MaxStickRoll) MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]]); else MaxStickRoll--; /*如果在降落过程中，则数据为0，也就是说降落的过程中不需要衰减，降落时候的保持位置全部为0，所以不需 要衰减*/ if(Notlandung) { MaxStickNick = 0; MaxStickRoll = 0; } /*可以认为是控制参数，前一个是陀螺仪的比例项(速率环参数) 后一个是陀螺仪积分即姿态角的比例(位置 环参数)*/ GyroFaktor = ((float) Parameter_Gyro_P + 10.0) / 256.0; IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //+ Digitale Steuerung per DubWise //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #define KEY_VALUE (Parameter_UserParam1 * 4) //(Poti3 * 8)//为了增加杆的输入的丰富性，提供了扩展 的杆的描述，对最终杆的描述更加丰富。 if(DubWiseKeys[1]) beeptime = 10; if(DubWiseKeys[1] & DUB_KEY_UP) tmp_int = KEY_VALUE; else if(DubWiseKeys[1] & DUB_KEY_DOWN) tmp_int = -KEY_VALUE; else tmp_int = 0; ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8; if(DubWiseKeys[1] & DUB_KEY_LEFT) tmp_int = KEY_VALUE; else if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0; ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8; if(DubWiseKeys[0] & 8) ExternStickGier = 50; else if(DubWiseKeys[0] & 4) ExternStickGier =-50; else ExternStickGier = 0; if(DubWiseKeys[0] & 2) ExternHoehenValue++; if(DubWiseKeys[0] & 16) ExternHoehenValue--; StickNick += ExternStickNick / 8; StickRoll += ExternStickRoll / 8; StickGier += ExternStickGier; //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //+ Analoge Steuerung per Seriell //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)//同上，具有扩展功能的控制输入 { StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; StickGier += ExternControl.Gier; ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; } /*陀螺仪积分比例为零，应该是Looping的情况?*/ if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; if(GyroFaktor < 0) GyroFaktor = 0; if(IntegralFaktor < 0) IntegralFaktor = 0; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Looping?//这里是在空中转圈的情况 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if((PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; else { { if((PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0; } } if((PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1; else { if(Looping_Rechts) // Hysterese { if(PPM_in[EE_Parameter.Kanalbele×g[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0; } } if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1; else { if(Looping_Oben) // Hysterese { if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0; } } if((PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1; else { if(Looping_Unten) // Hysterese { if(PPM_in[EE_Parameter.Kanalbele×g[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0; } } /*不应该出现轴都是Looping的情况*/ if(Looping_Links || Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0; if(Looping_Oben || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0; } // end if of if(!NewPpmData-- || Notlandung) if(Looping_Roll) beeptime = 100; if(Looping_Roll || Looping_Nick) { if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Bei Empfangsausfall im Flug // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(Notlandung) { /*如果出现紧急降落，则将三个期望位置全部置零，即让飞行器向最稳定的方向调整，同时改变控制参 数，并且不让飞行器处在空中打转的状态*/ StickGier = 0; StickNick = 0; StickRoll = 0; GyroFaktor = 0.1; IntegralFaktor = 0.005; Looping_Roll = 0; Looping_Nick = 0; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Integrale auf ACC-Signal abgleichen//加速度信号的积分校准 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #define ABGLEICH_ANZAHL 256L /*计算陀螺仪积分的积分，为了和加速度计的积分做比较，进行角速率的补偿和陀螺仪中立点的修正*/ MittelIntegralNick += IntegralNick; MittelIntegralRoll += IntegralRoll; MittelIntegralNick2 += IntegralNick2; MittelIntegralRoll2 += IntegralRoll2; /*在空中打转过程中，让所有的积分项都为零，因为机动过程会产生很大的误差，因此需要尽快结束其控制，然 后自动调平。 */ if(Looping_Nick || Looping_Roll) { IntegralAccNick = 0; IntegralAccRoll = 0; MittelIntegralNick = 0; MittelIntegralRoll = 0; MittelIntegralNick2 = 0; MittelIntegralRoll2 = 0; Mess_IntegralNick2 = Mess_IntegralNick; Mess_IntegralRoll2 = Mess_IntegralRoll; ZaehlMessungen = 0; LageKorrekturNick = 0; LageKorrekturRoll = 0; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(!Looping_Nick && !Looping_Roll) { long tmp_long, tmp_long2; /*使用加速度计的值去补偿陀螺仪的积分，这里必须知道EE_Parameter.GyroAccFaktor参数，才能够知道补偿了 多少*/ /*其中IntegralNick应该是陀螺仪积分 Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; 是滤波后的加速度，用当前加速度和上次的加速度平均 */ tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccNick);// tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccRoll); // tmp_long /= 16; tmp_long2 /= 16; /*如果历史最大摇杆的量比较大，则说明在前段时间内飞行器的姿态可能不为0，这就导致加速度计的输出受到 有害加速度的影响，因此必须加速度计和陀螺仪积分差值的基础上做一次衰减*/ if((MaxStickNick > 15) || (MaxStickRoll > 15)) { tmp_long /= 3; tmp_long2 /= 3; } /*当偏航轴的操纵杆输入较大时候，则说明这时候偏航轴有一个角速度，为了消除有害加速度的影响，必须对这 两个数值再做一次衰减*/ if(abs(PPM_in[EE_Parameter.Kanalbele×g[K_GIER]]) > 25) { tmp_long /= 3; tmp_long2 /= 3; } /*做一个限制，补偿值必须在一定的范围内。将补偿的范围限制在+-32*/ #define AUSGLEICH 32 if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; /*将补偿值考虑进去，这时候Mess_IntegralNick补偿了，Mess_IntegralNick2没有补偿，因为在后面还要用到 */ Mess_IntegralNick -= tmp_long; Mess_IntegralRoll -= tmp_long2; } // end if of if(!Looping_Nick && !Looping_Roll) // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ /*当 >ABGLEICH_ANZAHL(256)时候说明测量了256次航向*/ /*变量ZaehlMessungen是在AD检测的函数中改变的，也就是说，下面这个if语句是每256个检测周期计算一次， 而不是控制周期，检测周期要高于控制周期*/ if(ZaehlMessungen >= ABGLEICH_ANZAHL)//关于时间积累的处理过程 { static int cnt = 0; static char last_n_p,last_n_n,last_r_p,last_r_n; static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt; if(!Looping_Nick && !Looping_Roll) { MittelIntegralNick /= ABGLEICH_ANZAHL; MittelIntegralRoll /= ABGLEICH_ANZAHL; /*计算加速度计积分的作用，在不运动时候，xy加速度计的积分应该是0，所以xy积 分而z不积分*/ IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL; #define MAX_I 0//(Poti2/10) // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ /*不考虑补偿的陀螺仪积分的积分-加速度计积分/平衡项*/ IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick); ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll); ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in [EE_Parameter.Kanalbele×g[K_GIER]]) > 25)) { /*这个参数在后面的程序中还要进行修正，修正后的值加入到陀螺仪的积分 中，可以认为这个参数是系统对于陀螺仪漂移的估计*/ LageKorrekturNick /= 2; LageKorrekturRoll /= 2; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gyro-Drift ermitteln//陀螺漂移的确定 /*对陀螺仪漂移的估计过程*/ // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ /*前面MittelIntegralNick已经用过了，因此这里使用MittelIntegralNick2*/ MittelIntegralNick2 /= ABGLEICH_ANZAHL; MittelIntegralRoll2 /= ABGLEICH_ANZAHL; /*有校正和没有校正的陀螺仪积分做差，即陀螺仪的漂移*/ /*IntegralNick2是没有校正的陀螺仪积分 IntegralNick是有校正的陀螺仪积分这里的校正指的是使用加速度 计积分进行的校正*/ tmp_long = IntegralNick2 - IntegralNick; tmp_long2 = IntegralRoll2 - IntegralRoll; //DebugOut.Analog[25] = MittelIntegralRoll2 / 26; /*将差值加入到Mess_IntegralNick2和Mess_IntegralRoll2中这时Mess_IntegralNick2和Mess_IntegralRoll2 被使用*/ IntegralFehlerNick = tmp_long; IntegralFehlerRoll = tmp_long2; /*下面两个公式的作用就是让Mess_IntegralNick2=Mess_IntegralNick， Mess_IntegralRoll2=Mess_IntegralRoll为下一个计算周期做准备*/ Mess_IntegralNick2 -= IntegralFehlerNick; Mess_IntegralRoll2 -= IntegralFehlerRoll; // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; DebugOut.Analog[17] = IntegralAccNick / 26; DebugOut.Analog[18] = IntegralAccRoll / 26; DebugOut.Analog[19] = IntegralFehlerNick;// / 26; DebugOut.Analog[20] = IntegralFehlerRoll;// / 26; DebugOut.Analog[21] = MittelIntegralNick / 26; DebugOut.Analog[22] = MittelIntegralRoll / 26; //DebugOut.Analog[28] = ausgleichNick; DebugOut.Analog[29] = ausgleichRoll; DebugOut.Analog[30] = LageKorrekturRoll * 10; #define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4) #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) #define BEWE×GS_LIMIT 20000 // Nick +++++++++++++++++++++++++++++++++++++++++++++++++ /*以下部分就是对LageKorrekturNick的修正和对陀螺仪常值误差的修正*/ cnt = 1;// + labs(IntegralFehlerNick) / 4096; if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWE×GS_LIMIT) { if(IntegralFehlerNick > FEHLER_LIMIT2) { /*必须连续两次的误差都很大，才能进入下面的if语句*/ if(last_n_p) { /*连续两次误差较大时，对陀螺仪漂移进行补偿*/ /*最后改变了LageKorrekturNick的值*/ cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; ausgleichNick = IntegralFehlerNick / 8; if(ausgleichNick > 5000) ausgleichNick = 5000; LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; } else last_n_p = 1; } else last_n_p = 0; if(IntegralFehlerNick < -FEHLER_LIMIT2) { if(last_n_n) { cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; ausgleichNick = IntegralFehlerNick / 8; if(ausgleichNick < -5000) ausgleichNick = -5000; LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL; } else last_n_n = 1; } else last_n_n = 0; } else cnt = 0; if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; /*在飞行器飞行的过程中，如果发现陀螺仪的中立点发生变化，则仍然进行修正*/ /*误差过大时候，改变陀螺仪的常值误差，每次最多改变EE_Parameter.Driftkomp*/ if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; DebugOut.Analog[27] = ausgleichRoll; DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll); } // 整个的融合过程结束 else { LageKorrekturRoll = 0; LageKorrekturNick = 0; } /*如果IntegralFaktor为零，也就是没有使用陀螺仪积分对电机输出进行修正，则不使用LageKorrekturRoll， 也就是不进行陀螺仪漂移的补偿*/ /*在Heading_hold标志位被置位的情况下*/ if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0; } // z.B. bei HH // +++++++++++++++++++++++++++++++++++++++++++++++++++++ /*将上一次的值储存下来*/ MittelIntegralNick_Alt = MittelIntegralNick; MittelIntegralRoll_Alt = MittelIntegralRoll; // +++++++++++++++++++++++++++++++++++++++++++++++++++++ /*数据清零加速度计积分每一次都进行清零*/ IntegralAccNick = 0; IntegralAccRoll = 0; IntegralAccZ = 0; MittelIntegralNick = 0; MittelIntegralRoll = 0; MittelIntegralNick2 = 0; MittelIntegralRoll2 = 0; ZaehlMessungen = 0; } //end if of if(ZaehlMessungen >= ABGLEICH_ANZAHL) //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gieren//偏航 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(abs(StickGier) > 20) // war 35 { if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; } tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx? tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; sollGier = tmp_int; /*如果没有这句话那么偏航轴的期望角度将一直等于0度那么如果需要调整偏航轴的角度就必须一直 不断的进行偏航的修正加上这句话后就不用一直修正了*/ Mess_Integral_Gier -= tmp_int; if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen约束和限制 if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Kompass // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) { int w,v; static int SignalSchlecht = 0; w = abs(IntegralNick /512); v = abs(IntegralRoll /512); if(v > w) w = v; if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht) { KompassStartwert = KompassValue; NeueKompassRichtungMerken = 0; } w = (w * Parameter_KompassWirkung) / 64; w = Parameter_KompassWirkung - w; if(w > 0) { if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; if(SignalSchlecht) SignalSchlecht--; } else SignalSchlecht = 500; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Debugwerte zuordnen // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ if(!TimerWerteausgabe--) { TimerWerteausgabe = 24; DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; DebugOut.Analog[2] = Mittelwert_AccNick; DebugOut.Analog[3] = Mittelwert_AccRoll; DebugOut.Analog[4] = MesswertGier; DebugOut.Analog[5] = HoehenWert; DebugOut.Analog[6] =(Mess_Integral_Hoch / 512); DebugOut.Analog[8] = KompassValue; DebugOut.Analog[9] = UBat; DebugOut.Analog[10] = SenderOkay; DebugOut.Analog[16] = Mittelwert_AccHoch; /* DebugOut.Analog[16] = motor_rx[0]; DebugOut.Analog[17] = motor_rx[1]; DebugOut.Analog[18] = motor_rx[2]; DebugOut.Analog[19] = motor_rx[3]; DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3]; DebugOut.Analog[20] /= 14; DebugOut.Analog[21] = motor_rx[4]; DebugOut.Analog[22] = motor_rx[5]; DebugOut.Analog[23] = motor_rx[6]; DebugOut.Analog[24] = motor_rx[7]; DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7]; */ // DebugOut.Analog[9] = MesswertNick; // DebugOut.Analog[9] = SollHoehe; // DebugOut.Analog[10] = Mess_Integral_Gier / 128; // DebugOut.Analog[11] = KompassStartwert; // DebugOut.Analog[10] = Parameter_Gyro_I; // DebugOut.Analog[10] = EE_Parameter.Gyro_I; // DebugOut.Analog[9] = KompassRichtung; // DebugOut.Analog[10] = GasMischanteil; // DebugOut.Analog[3] = HoeheD * 32; // DebugOut.Analog[4] = hoehenregler; } // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen//角速度和角度变化的归纳部分 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //DebugOut.Analog[26] = MesswertNick; //DebugOut.Analog[28] = MesswertRoll; /*对角度做PD，也就是对角速率做了PI*/ if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor; else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor; if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2; DebugOut.Analog[25] = IntegralRoll * IntegralFaktor; DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor); DebugOut.Analog[28] = MesswertRoll; /*对控制器输出进行幅度限制*/ #define MAX_SENSOR 2048 if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR; if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR; if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR; if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR; if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR; if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // H鰄enregelung // Die H鰄enregelung schw鋍ht lediglich das Gas ab, erh鰄t es allerdings nicht // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //OCR0B = 180 - (Poti1 + 120) / 4; //DruckOffsetSetting = OCR0B; if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) { int tmp_int; if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) { if(Parameter_MaxHoehe < 50) { SollHoehe = HoehenWert - 20; HoehenReglerAktiv = 0; } else HoehenReglerAktiv = 1; } else { SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int) EE_Parameter.Hoehe_Verstaerkung - 20; HoehenReglerAktiv = 1; } if(Notlandung) SollHoehe = 0; h = HoehenWert; if((h > SollHoehe) && HoehenReglerAktiv) { h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; h = GasMischanteil - h; h -= (HoeheD * Parameter_Luftdruck_D)/8; tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32; if(tmp_int > 50) tmp_int = 50; else if(tmp_int < -50) tmp_int = -50; h -= tmp_int; hoehenregler = (hoehenregler*15 + h) / 16; if(hoehenregler < EE_Parameter.Hoehe_MinGas) { if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas; if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil; } if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; GasMischanteil = hoehenregler; } } // 高度调节器工作完成 if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // + Mischer und PI-Regler 在PI控制器下的混合数值 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ DebugOut.Analog[7] = GasMischanteil; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Gier-Anteil//偏航部分 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ #define MUL_G 1.0 GierMischanteil = MesswertGier - sollGier; // GierMischanteil = 0; /*对偏航数值进行限制，尽量避免最后计算出的四个电机的转速小于0*/ if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2; if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2); if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil)); if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS - GasMischanteil)); /*油门本身如果太小了，就限制偏航为0*/ if(GasMischanteil < 20) GierMischanteil = 0;// // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Nick-Achse俯仰轴 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ /*PI调节器*/ /*这个控制算法实际上是位置环为PI控制，速率环为P控制*/ DiffNick = MesswertNick - (StickNick - GPS_Nick); if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); else SummeNick += DiffNick; if(SummeNick > 16000) SummeNick = 16000; if(SummeNick < -16000) SummeNick = -16000; pd_ergebnis = DiffNick + Ki * SummeNick; //PD控制结果为比例+积分控制 // Motor Vorn tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs (GierMischanteil)/2)) / 64; if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; //如果控制器输出太大，则要限制幅度 if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; /*前后两个电机的实际输出*/ motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; /*对电机数值进行限幅*/ if ((motorwert < 0)) motorwert = 0; else if(motorwert > MAX_GAS) motorwert = MAX_GAS; if (motorwert < MIN_GAS) motorwert = MIN_GAS; Motor_Vorne = motorwert; // Motor Heck motorwert = GasMischanteil - pd_ergebnis + GierMischanteil; if ((motorwert < 0)) motorwert = 0; else if(motorwert > MAX_GAS) motorwert = MAX_GAS; if (motorwert < MIN_GAS) motorwert = MIN_GAS; Motor_Hinten = motorwert; // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Roll-Achse横滚轴 // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I- Anteil bei Winkelregelung else SummeRoll += DiffRoll; // I-Anteil bei HH if(SummeRoll > 16000) SummeRoll = 16000; if(SummeRoll < -16000) SummeRoll = -16000; pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler f黵 Roll tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs (GierMischanteil)/2)) / 64; if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; // Motor Links motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; #define GRENZE Poti1 if ((motorwert < 0)) motorwert = 0; else if(motorwert > MAX_GAS) motorwert = MAX_GAS; if (motorwert < MIN_GAS) motorwert = MIN_GAS; Motor_Links = motorwert; // Motor Rechts motorwert = GasMischanteil - pd_ergebnis - GierMischanteil; if ((motorwert < 0)) motorwert = 0; else if(motorwert > MAX_GAS) motorwert = MAX_GAS; if (motorwert < MIN_GAS) motorwert = MIN_GAS; Motor_Rechts = motorwert; // +++++++++++++++++++++++++++++++++++++++++++++++ }</div> </div> <div class="b20 c_b"> </div> <div class="keytags"> 标签： <a href="http://www.ck365.cn/anli/search-xzg-kw-%E5%9B%9B%E8%BD%B4%E9%A3%9E%E8%A1%8C%E5%99%A8.html" target="_blank" class="b">四轴飞行器</a> </div> <div class="b20">  </div> <div class="tool"> 11 </div> <div class="b20">  </div><div class="b20">  </div> <div style="float:right;width:150px;text-align:right;"> <div class="bdsharebuttonbox"><a href="#" class="bds_more" data-cmd="more"></a><a title="分享到微信" href="#" class="bds_weixin" data-cmd="weixin"></a><a title="分享到QQ空间" href="#" class="bds_qzone" data-cmd="qzone"></a><a title="分享到新浪微博" href="#" class="bds_tsina" data-cmd="tsina"></a><a title="分享到人人网" href="#" class="bds_renren" data-cmd="renren" style="margin-right:0;"></a></div> <script>window._bd_share_config={"common":{"bdSnsKey":{},"bdText":"","bdMini":"1","bdMiniList":false,"bdPic":"","bdStyle":"0","bdSize":"24"},"share":{},"image":{"viewList":["qzone","tsina","tqq","renren","weixin"],"viewText":"分享到：","viewSize":"24"},"selectShare":{"bdContainerClass":null,"bdSelectMiniList":["qzone","tsina","tqq","renren","weixin"]}};with(document)0[(getElementsByTagName('head')[0]||body).appendChild(createElement('script')).src='http://bdimg.share.baidu.com/static/api/js/share.js?v=89860593.js?cdnversion='+~(-new Date()/36e5)];</script> </div> <div class="b20 c_b">  </div> <div class="b20"> </div> <div class="head-txt"><a href="http://www.ck365.cn/anli/search-xzg-kw-%E5%9B%9B%E8%BD%B4%E9%A3%9E%E8%A1%8C%E5%99%A8.html">更多></a>同类技术</div> <div class="related"><table width="100%"> <tr><td width="50%"><a href="http://www.ck365.cn/anli/12/58706.html" target="_blank" title="大联大品佳集团推出基于新唐技术和产品的四轴飞行器整体解决方案">• 大联大品佳集团推出基于新唐技术和产品的四轴飞行器整体解决</a></td> <td width="50%"><a href="http://www.ck365.cn/anli/10/46068.html" target="_blank" title="圆点博士微型四轴飞行器之：如何解读陀螺仪MPU6050数据">• 圆点博士微型四轴飞行器之：如何解读陀螺仪MPU6050数据</a></td> </tr><tr><td width="50%"><a href="http://www.ck365.cn/anli/201304/22/46042.html" target="_blank" title="圆点博士微型四轴飞行器之：如何解读陀螺仪MPU6050数据">• 圆点博士微型四轴飞行器之：如何解读陀螺仪MPU6050数据</a></td> </table> </div> <div class="b20"> </div> <div class="send-paper flex">全年征稿 / 资讯合作 <div class="items"> <div class="qq"><a href="tencent://message/?uin=2398080099&Site=CK365测控网&Menu=yes"> 2398080099</a></div> <div class="email">联系邮箱：<img src="http://www.ck365.cn/static/tool.php?a=image&auth=AGdXNwEwUmVSNVdjVzFSNgdhA2RRZFBhXWgDb1IzADlUMwJiBj5SbFVCViNWcVYsXGkDOwZs&red=0&green=0&blue=0&fred=0&fgreen=0&fblue=0" align="absmddle" /></div> </div> </div> <div class="b15"> </div> <div class="copyright-box"> <div class="head-txt">版权与免责声明</div> <ul class="copyright-body"> <li>凡本网注明“来源：CK365测控网”的所有作品，均为北京新科时代传媒信息技术有限公司-CK365测控网合法拥有版权或有权使用的作品，未经本网授权不得转载、摘编或利用其它方式使用上述作品。已经本网授权使用作品的，应在授权范围内使用，并注明“来源：CK365测控网”。违反上述声明者，本网将追究其相关法律责任。</li> <li>本网转载并注明自其它来源的作品，目的在于传递更多信息，并不代表本网赞同其观点或证实其内容的真实性，不承担此类作品侵权行为的直接责任及连带责任。其他媒体、网站或个人从本网转载时，必须保留本网注明的作品来源，并自负版权等法律责任。</li> <li>如涉及作品内容、版权等问题，请在作品发表之日起一周内与本网联系，否则视为放弃相关权利。</li> </ul> </div></div> <div class="m3r"> <div class="head-sub">推荐图文</div> <div class="list-thumb"><table width="100%"> <tr><td width="50%" valign="top"><a href="http://www.ck365.cn/anli/13/61706.html" target="_blank"><img src="http://new2.ck365.cn/static/file/upload/202104/10/180422461.jpg" width="124" height="93" alt="揭秘卫星互联网！6G关键技术，国内行业雏形初现：智东西内参"/></a> <ul><li><a href="http://www.ck365.cn/anli/13/61706.html" title="揭秘卫星互联网！6G关键技术，国内行业雏形初现：智东西内参" target="_blank">揭秘卫星互联网！6G关键技术，国内行业雏形</a></li></ul></td> <td width="50%" valign="top"><a href="http://www.ck365.cn/anli/13/60236.html" target="_blank"><img src="http://new2.ck365.cn/static/file/upload/202104/10/180246471.jpg" width="124" height="93" alt="3D打印技术发展及在航空航天领域应用"/></a> <ul><li><a href="http://www.ck365.cn/anli/13/60236.html" title="3D打印技术发展及在航空航天领域应用" target="_blank">3D打印技术发展及在航空航天领域应用</a></li></ul></td> </tr><tr><td width="50%" valign="top"><a href="http://www.ck365.cn/anli/13/61473.html" target="_blank"><img src="http://www.ck365.cn/data/upload/201810/16/15-36-07-40-0.png" width="124" height="93" alt="陀螺仪未来有哪些新应用？"/></a> <ul><li><a href="http://www.ck365.cn/anli/13/61473.html" title="陀螺仪未来有哪些新应用？" target="_blank">陀螺仪未来有哪些新应用？</a></li></ul></td> <td width="50%" valign="top"><a href="http://www.ck365.cn/anli/10/49244.html" target="_blank"><img src="http://www.ck365.cn/data/upload/201310/30/105728-56-0.gif" width="124" height="93" alt="基于GPRS专网系统油井监测系统通信解决方案"/></a> <ul><li><a href="http://www.ck365.cn/anli/10/49244.html" title="基于GPRS专网系统油井监测系统通信解决方案" target="_blank">基于GPRS专网系统油井监测系统通信解决方案</a></li></ul></td> </tr></table> </div> <div class="head-sub">推荐技术</div> <div class="list-txt"><ul> <li><a href="http://www.ck365.cn/anli/13/61689.html" target="_blank" title="我国载人月球探测相关工作正稳步推进">我国载人月球探测相关工作正稳步推进</a></li> <li><a href="http://www.ck365.cn/anli/13/61684.html" target="_blank" title="NASA计划研制月球GPS帮宇航员导航">NASA计划研制月球GPS帮宇航员导航</a></li> <li><a href="http://www.ck365.cn/anli/13/61676.html" target="_blank" title="我国自主研发新能源四座电动飞机首飞">我国自主研发新能源四座电动飞机首飞</a></li> <li><a href="http://www.ck365.cn/anli/13/61665.html" target="_blank" title="国内首个航空遥感系统机库落户营口">国内首个航空遥感系统机库落户营口</a></li> <li><a href="http://www.ck365.cn/anli/13/61651.html" target="_blank" title="世界直升机市场综合研究与分析">世界直升机市场综合研究与分析</a></li> <li><a href="http://www.ck365.cn/anli/13/61631.html" target="_blank" title="气象探空仪中应用的温湿度传感器">气象探空仪中应用的温湿度传感器</a></li> <li><a href="http://www.ck365.cn/anli/13/61613.html" target="_blank" title="航天科技五院508所国内口径最大的透射式低温相机即将定焦">航天科技五院508所国内口径最大的透射式低温相机即将定焦</a></li> <li><a href="http://www.ck365.cn/anli/12/59646.html" target="_blank" title="国外反无人机技术发展分析">国外反无人机技术发展分析</a></li> <li><a href="http://www.ck365.cn/anli/13/61572.html" target="_blank" title="FMCW激光雷达核心技术及量产难点分析">FMCW激光雷达核心技术及量产难点分析</a></li> <li><a href="http://www.ck365.cn/anli/13/61506.html" target="_blank" title="移动机器人如何实现自主导航？">移动机器人如何实现自主导航？</a></li> </ul> </div> <div class="head-sub">可能喜欢</div> <div class="list-txt" mid="25" id="xzgcms_level"><ul> <li><a href="http://www.ck365.cn/anli/200511/23/15002.html" target="_blank" title="ABB控制系统与Siemens PLC间的数据通讯">ABB控制系统与Siemens PLC间的数据通讯</a></li> <li><a href="http://www.ck365.cn/anli/201212/25/43979.html" target="_blank" title="薄膜电容器的相关知识介绍">薄膜电容器的相关知识介绍</a></li> <li><a href="http://www.ck365.cn/anli/201301/22/44944.html" target="_blank" title="盐雾腐蚀试验箱的故障处理">盐雾腐蚀试验箱的故障处理</a></li> <li><a href="http://www.ck365.cn/anli/12/59456.html" target="_blank" title="AMD技术转型模式处理方案">AMD技术转型模式处理方案</a></li> <li><a href="http://www.ck365.cn/anli/201212/10/40196.html" target="_blank" title="基于电量均衡的无线传感器网络分簇算法">基于电量均衡的无线传感器网络分簇算法</a></li> <li><a href="http://www.ck365.cn/anli/201105/29/21424.html" target="_blank" title="基于ATmega16的气动标记控制系统设计">基于ATmega16的气动标记控制系统设计</a></li> <li><a href="http://www.ck365.cn/anli/11/53379.html" target="_blank" title="物联网无线ZigBee智能家居设计解决方案">物联网无线ZigBee智能家居设计解决方案</a></li> <li><a href="http://www.ck365.cn/anli/12/58612.html" target="_blank" title="银泉网络采用飞利浦驱动器开发网络LED路灯照明解决方案">银泉网络采用飞利浦驱动器开发网络LED路灯照明解决方案</a></li> <li><a href="http://www.ck365.cn/anli/201205/15/24576.html" target="_blank" title="4路SDI采集卡在大屏融合中的应用">4路SDI采集卡在大屏融合中的应用</a></li> </ul> </div> </div> <div class="c_b"></div> </div> <script type="text/javascript" src="http://www.ck365.cn/static/script/content.js"></script><div class="m b15"> </div> <div id="back2top" class="back2top"><a href="javascript:void(0);" title="返回顶部"> </a></div> <div class="footer"> <div class="menu-foot m" style="border-top:0;"> <a title="CK365测控网" href="http://www.ck365.cn/">网站首页</a>  | <a href="http://www.ck365.cn/about/privacy.htm" target="_blank">免责声明</a> | <a href="http://www.ck365.cn/extend/contact.html" target="_blank">联系方式</a> | <a href="http://www.ck365.cn/about/index.html" target="_blank">关于我们</a> | <a href="http://www.ck365.cn/sitemap/" target="_blank">网站地图</a>  | <a href="http://www.ck365.cn/guestbook/" target="_blank">网站留言</a> | <a href="http://www.ck365.cn/feed/" target="_blank">RSS订阅</a> | <a href="https://beian.miit.gov.cn/" target="_blank">京ICP备11033066号-1</a>京公网安备:11010802018309号 </div> <div class="copyright">Copyright © 2005-2020 <a href="http://www.ck365.cn">www.ck365.cn</a> All Rights Reserved 版权所有 <a href="http://www.ck365.cn">CK365测控网-北京新科时代传媒信息技术有限公司</a> 客服：<a target="_blank" href="http://wpa.qq.com/msgrd?v=3&uin=2398080099&site=qq&menu=yes" rel="external nofollow"><img border="0" src="http://wpa.qq.com/pa?p=2:1365099215:41" alt="测控网客服" title="测控网客服"></a></div> <div class="b10"></div> <div class="copyright-foot"> <div class="m"> <a href="http://www.bj.cyberpolice.cn/" target="_blank" rel="nofollow"><img src="http://www.ck365.cn/static/skin/default/image/beianbgs.png" ></a>   <a href="http://www.miitbeian.gov.cn/" target="_blank" rel="nofollow"><img src="http://www.ck365.cn/static/skin/default/image/blxx.png" ></a>   <a href="http://www.wenming.cn/" target="_blank" rel="nofollow"><img src="http://www.ck365.cn/static/skin/default/image/zgwm.png" ></a> </div> </div> </div> <script type="text/javascript"> task('m=25&html=show&itemid=56418&page=1'); $('#back2top').click(function() { $("html, body").animate({scrollTop:0}, 200); }); stats(25,'19083','56418'); if($('#xzgcms_level').length > 0){ $.get(AJPath, {m:25,a:'loadlevel',itemid:'56418'}, function(d){ if(d){ $('#xzgcms_level').html(d); } }); } </script> <script type="text/javascript">var textJ2F = true;</script> <script type="text/javascript" src="http://www.ck365.cn/static/script/zh2cn.js"></script> <script type="text/javascript" src="http://www.ck365.cn/static/script/gb2big.js"></script> </body> </html>

CINXE.COM

四轴飞行器姿态控制算法注释 -测控技术在线 自动化技术 CK365测控网

四轴飞行器姿态控制算法注释 -测控技术在线自动化技术 CK365测控网