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之姿态解算篇(3)_源码姿态解算算法分析

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

Pixhawk之启动代码和入口函数(.mk、rcS、__start、hrt)

一、开篇         首先告诉大家一个坏消息,DJI提供了SDK~~~~         然后再来一个好消息,本篇博文很多干货哦~~~~~         最近比较纠结的一个...

Pixhawk之UAV控制理论、ardupilot源码框架介绍

一、开篇         “您有无人机么?         没有。         那赶紧去某宝买一套。”         昨天开会开到接近...

pixhawk源码下载与编译

pixhawk源码下载 与 编译
  • Gen_Ye
  • Gen_Ye
  • 2016-08-11 17:10
  • 3655

Pixhawk原生固件以往代码版本的下载

我们按照官网的下载方法只能下载到最新版本的代码,而GitHub上http://write.blog.csdn.net/postedit?type=edit包含了以往的各个版本: 若需要下载其他版本...
  • niu_88
  • niu_88
  • 2016-09-12 17:31
  • 1038

Pixhawk官网飞行模式介绍

pixhawk飞行模式一览

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

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

Pixhawk——rcs源码分析

注:本文转载自——  博主:虾米一代      博客:《pixhawk原生码rcS分析》 代码执行流程 1.       编译时将cmake/configs/nuttx_px4fmu-v...
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:深度学习:神经网络中的前向传播和反向传播算法推导
举报原因:
原因补充:

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