pixhawk(ArduCopter V3.3)源码分析—导航制导部分

原创 2016年05月31日 21:45:14

首先要感谢蜗牛大神的讲课,博客里面的流程图是蜗牛大神PPT里面的图,看了这个PPT我才把之前看过的一些代码理出来一点头绪~
以guided模式中guided_pos_control_run()模式为例

  1. 函数的调用
    fast_loop();—update_flight_mode();若检测到遥控器5通道相应信号,执行guided模式的guided_run();函数。
  2. guided_run();函数中以guided_pos_control_run();函数为例分析。

    这里写图片描述
    这里写图片描述
    导航制导部分的控制流程图如下:
    这里写图片描述
    分析如下(电脑画太麻烦了~手画的比较low,大家见谅~):
    这里写图片描述
    图中(1)期望加速度(2)期望速度_vel_desired (3)期望位置_pos_target (4)计算目标速度时的前馈项 (5)目标速度_vel_target (6)姿态融合算得的位置_curr_pos (7)姿态融合算得的速度_vel_curr (8)计算目标加速度时的前馈项_accel_feedforward (9)目标加速度_accel_target (10)目标角度 (11)姿态融合算得的加速度 (12)姿态融合算得的角度 (13)目标角加速度_rate_bf_target (14)姿态融合算得的角加速度

以XY平面的导航计算为例,从左向右,在guided_pos_control_run—wp_nav.update_wpnav();—_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f, false);函数中更新了XY平面的导航信息。
desired_vel_to_pos(dt);函数为由(2)期望速度_vel_desired 积分得到 (3)期望位置_pos_target。

pos_to_rate_xy(mode, dt, ekfNavVelGainScaler);,是图中的函数1,通过(3)期望位置_pos_target计算得到(5)目标速度_vel_target。一路为位置微分后的前馈项,另一路是目标位置和姿态融合算得的位置做差后,经过P计算得到。这里的P计算有线性区和非线性区。

rate_to_accel_xy(dt, ekfNavVelGainScaler);是图中的函数2,通过目标速度算得目标加速度。一路为速度微分后的前馈项,另一路是目标速度和姿态融合算得的速度做差后,经过PI计算得到。

accel_to_lean_angles(dt, ekfNavVelGainScaler, use_althold_lean_angle);是图中的函数3,通过目标加速度(重力加速度在机体各个轴上的分量)算得目标角度。

之后在attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);

attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(),wp_nav.get_pitch(),get_auto_heading(), true);函数中通过上一步计算的到的角度计算出应有的角加速度_rate_bf_target。

之后的PID计算在fast_loop()函数中的attitude_control.rate_controller_run();函数中实现。电机输出在fast_loop()—motors.output();—output_armed_stabilizing();函数中实现。output_armed_stabilizing();函数的定义在AP_MotorsMatrix.cpp中的AP_MotorsMatrix类里。

pixhawk的源码结构介绍(针对ArduCopter这套源码)(一)

pixhawk的源码结构介绍(针对ArduCopter这套源码)(一)版权声明 博  主:thunder_fan 声  明:喝水不忘挖井人,转载请注明出处。 联系方式:cllshxz@163.com ...

pixhawk软件环境的搭建

pixhawk软件开发环境的搭建(for windows)版权声明 博  主:thunder_fan 声  明:喝水不忘挖井人,转载请注明出处。 联系方式:cllshxz@163.com 具体流程 安...

pixhawk(ArduCopter V3.3)源码分析—导航制导部分

以guided模式中guided_pos_control_run()模式为例 函数的调用 fast_loop();—update_flight_mode();若检测到遥控器5通道相应...

pixhawk源码分析(1)

  • 2017年11月03日 15:34
  • 9.53MB
  • 下载

PIXHAWK源码分析之二—Hello Sky(PIXHAWK读取姿态数据并且显示)

阿木社区玩也要玩的专业,转载请注明出处!http://www.amovauto.com/?p=905 QQ群:526221258    我们先放开那些复杂的数学算法,向卡尔曼滤波,四元函数什么的...

nginx源码分析—hash结构ngx_hash_t(v1.0.4)

本博客(http://blog.csdn.net/livelylittlefish )贴出作者(阿波)相关研究、学习内容所做的笔记,欢迎广大朋友指正! Content 0.序 ...

Pixhawk飞控源码

  • 2017年11月23日 22:32
  • 7.77MB
  • 下载

Pixhawk源码总结

  • 2015年06月05日 19:27
  • 1.25MB
  • 下载

Pixhawk之姿态解算篇(3)_源码姿态解算算法分析

Pixhawk之姿态解算篇(3)_源码姿态解算算法分析                                          2016-04-05 09:53          ...

PIXHAWK源码分析之三---通过串口方式添加一个自定义传感器(1)

一直想尝试在PIXHAWK上添加超声波或者激光雷达,好在PIX提供了uORB这个数据接口(在前几篇文章里面有介绍),可以实现传感器数据的发布和订阅,我们利用uORB协议来实现把自定义的传感器数据从,串...
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:pixhawk(ArduCopter V3.3)源码分析—导航制导部分
举报原因:
原因补充:

(最多只允许输入30个字)