px4中vtol姿态控制源码分析

/src/modules/vtol_att_control/文件夹中包含vtol_att_control_main、vtol_type、standard/tailsitter/tiltrotor等文件。下面是主要控制逻辑：

事实上，PX4飞控系统支持所有的垂直起降机型配置：

  • 尾座式tailsitter (X/+型布局的双/四旋翼)
  • 倾转式tiltrotor (Firefly Y6)
  • 复合式standard (飞机+四旋翼)

垂直起降飞行器与其它种类的飞行器共享代码库，所不同的仅仅是增加了额外的控制逻辑，特别是转换阶段。

下面看源码：

vtol_att_control_main

1、订阅并更新参数

与所有控制器一样，由start函数开启进程，执行任务，指向task_main()

orb订阅各种参数：

        /* do subscriptions */_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint));_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));_v_att_sub             = orb_subscribe(ORB_ID(vehicle_attitude));_v_control_mode_sub    = orb_subscribe(ORB_ID(vehicle_control_mode));_params_sub            = orb_subscribe(ORB_ID(parameter_update));_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));_local_pos_sub         = orb_subscribe(ORB_ID(vehicle_local_position));_local_pos_sp_sub         = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));_pos_sp_triplet_sub    = orb_subscribe(ORB_ID(position_setpoint_triplet));_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));_vehicle_cmd_sub       = orb_subscribe(ORB_ID(vehicle_command));_tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));parameters_update();  // initialize parameter cache
parameters_update()更新通用参数和特定机型的参数。
2、_vtol_type->set_idle_mc()确保飞机以mc模式贴地起飞。
3、进入while循环首先检查参数更新
vehicle_control_mode_poll() //检查飞机控制模式的轮询

手动模式下重置转换命令；

更新不同机型的飞行模式①：_vtol_type->update_vtol_state();

检查飞机处于哪种模式并调用特定函数②:

ROTARY_WING：主要函数为_vtol_type->update_mc_state(); 计算期望姿态

   mc_virtual_att_sp_poll();// vehicle is in rotary wing mode_vtol_vehicle_status.vtol_in_rw_mode = true;_vtol_vehicle_status.vtol_in_trans_mode = false;// got data from mc attitude controller_vtol_type->update_mc_state();fill_mc_att_rates_sp();

FIXED_WING：主要函数为_vtol_type->update_fw_state(); 计算期望姿态

   fw_virtual_att_sp_poll();// vehicle is in fw mode_vtol_vehicle_status.vtol_in_rw_mode = false;_vtol_vehicle_status.vtol_in_trans_mode = false;_vtol_type->update_fw_state();fill_fw_att_rates_sp();

TRANSITION：主要函数为_vtol_type->update_transition_state();计算转换时的姿态期望值

   mc_virtual_att_sp_poll();fw_virtual_att_sp_poll();// vehicle is doing a transition_vtol_vehicle_status.vtol_in_trans_mode = true;_vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition_vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW);_vtol_type->update_transition_state();fill_mc_att_rates_sp();
4、填充actuator输出并发布姿态期望值

发布的内容：

vehicle_attitude_setpoint // 飞机期望姿态

actuator_controls_0 // MC模式的作动器控制

actuator_controls_1 // FW模式的作动器控制

vtol_vehicle_status // 飞行状态

5、标志位_control_task = -1，退出任务。

总结流程图如下：

vtol_type.cpp

VTOL_mode:

ROTARY_WING

TRANSITION_TO_FW,

TRANSITION_TO_MC,

FIXED_WING

VtolType::check_quadchute_condition()

void VtolType::update_transition_state()
{check_quadchute_condition();
}

check_quadchute_condition(); // 检查四通道状况，固定翼模式使用tecs轨迹跟踪，转换过程中使用local_pos_sp

standard

复合式VTOL的参数更新parameters_update():

首先param_find得到系统参数，param_get拷贝到私有变量上，再有constrain（）对于一些参数的限制

复合式机型特定参数：

①Standard : : update_vtol_state（）:

该函数是更新vtol状态函数，并在每个状态模式下更新

_mc_roll_weight = 1.0f; // 初始化，mc权重为1，确定以旋翼模式起飞
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
_mc_throttle_weight = 1.0

固定翼模式请求为false（ is_fixed_wing_requested()==false）：

MC_MODE：保持原模式    mc_weight = 1.0f;
                                    _pusher_throttle = 0.0f;
                                    _reverse_output = 0.0f;

FW_MODE：

失效保护： 立即进入  MC_MODE-->  _vtol_schedule.flight_mode = MC_MODE;
                                                     _flag_enable_mc_motors = true;
                                                     _pusher_throttle = 0.0f;
                                                     _reverse_output = 0.0f;

正常转换：模式转化为TRANSITION_TO_MC--> _vtol_schedule.flight_mode = MC_MODE;
                                                                    _flag_enable_mc_motors = true;
                                                                    _pusher_throttle = 0.0f;
                                                                    _reverse_output = 0.0f;

TRANSITION_TO_FW：立即执行失效保护进入MC_MODE

TRANSITION_TO_MC:  转换超时或者前进速度低于水平巡航速度进入MC_MODE

固定翼模式请求为ture（ is_fixed_wing_requested()==ture）：

MC_MODE 或者TRANSITION_TO_MC：模式改变为TRANSITION_TO_FW，并开始计时。

FW_MODE：保持原模式。

TRANSITION_TO_FW：在合适空速范围和执行时间内，模式更改为FW_MODE，关闭旋翼电机并

结束计时。

注意：此处的飞行模式是_vtol_schedule.flight_mode， 需要对应映射到VTOL_mode上，

即

switch (_vtol_schedule.flight_mode) {case MC_MODE:_vtol_mode = mode::ROTARY_WING;break;case FW_MODE:_vtol_mode = mode::FIXED_WING;break;case TRANSITION_TO_FW:_vtol_mode = mode::TRANSITION_TO_FW;break;case TRANSITION_TO_MC:_vtol_mode = mode::TRANSITION_TO_MC;break;}

其他的机型和以上一样， _vtol_schedule.flight_mode  需要对应映射到VTOL_mode上。

        standard : : update_transition_state()：// 计算转换时的姿态期望值

VtolType::update_transition_state(); //检查四通道状况

TRANSITION_TO_FW：油门的计算，利用mc权重计算pitch得到期望姿态值

         //基于空速计算mc权重mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) /_airspeed_trans_blend_margin;
         //没有空速计时，基于转换时间计算mc权重mc_weight = 1.0f - ((float)(hrt_elapsed_time(&_vtol_schedule.transition_start) - ((_params_standard.front_trans_time_min / 2.0f) * 1000000.0f)) /((_params_standard.front_trans_time_min / 2.0f) * 1000000.0f));
        _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);//利用mc权重计算俯仰期望值，FW_PSP_OFF*（1-mc_weight)

超时检查：超时则中止前转换

TRANSITION_TO_MC：保持当前姿态，油门处理，逐渐增加mc权重，重启旋翼电机

       // maintain FW_PSP_OFF保持FW_PSP_OFF
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset;

standard : : update_mc_state()：//计算旋翼模式姿态期望值

        Standard::fill_actuator_outputs()  // 计算作动器输出

旋翼模式下输入量乘以各权重

tailsitter

①tailsitter : : update_vtol_state（）:
使用双向开关执行转换的简单逻辑。
        在翻转开关后，飞机将在MC控制模式下开始倾斜，提高前进速度。 在飞机拾起足够的俯仰角后，无人机将进入FW模式。
        对于反转换，再次在MC模式下控制俯仰角，并切换到全MC控制，以达到足够的俯仰角。

固定翼模式请求为false（ is_fixed_wing_requested()==false）：

MC_MODE：保持原模式

FW_MODE： 进入TRANSITION_BACK，转换计时开始

TRANSITION_FRONT_P1：立即执行失效保护进入MC_MODE

TRANSITION_BACK:  达到规定的俯仰角，即可切换到全MC控制

固定翼模式请求为ture（ is_fixed_wing_requested()==ture）：

MC_MODE ：模式改变为TRANSITION_FRONT_P1，并开始计时。

FW_MODE：保持原模式。

TRANSITION_FRONT_P1：在达到规定的转换空速和俯仰角时，模式更改为FW_MODE。

②tailsitter::update_transition_state()：

                  转换过程执行前，保持航向、俯仰角以及油门值，为转换过程做准备
                TRANSITION_FRONT_P1：俯仰角期望值基于时间变化；飞机达到速度时，禁用偏航控制 
_v_att_sp->pitch_body = _pitch_transition_start    - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) *(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));

if (_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {_mc_yaw_weight = 0.0f;}

                TRANSITION_FRONT_P2：飞机已经准备好进入FW模式，继续控制飞机平稳的减小俯仰角

_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 -(fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));

mc权重归0；在混合控制时按时间平稳的减少roll/pitch的mc权重：

/** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*///_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));//_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));

                TRANSITION_BACK：增大俯仰角；推力保持不变

_v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) *(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.back_trans_dur * 1000000.0f);

禁用偏航控制；逐渐增大mc权重；计算期望姿态四元数。

tilttotor

①Tiltrotor::update_vtol_state() 模式转换

* 使用双向开关执行转换的简单逻辑，在翻转开关后，飞机将开始倾斜旋翼，获得前进速度。

* 在飞机提升足够的速度后，旋翼完全向前倾斜。 对于反转换，电机只需旋转回来。

固定翼模式请求为false（ is_fixed_wing_requested()==false）：

            MC_MODE:保持原模式

            FW_MODE: 模式切换成TRANSITION_BACK，转换计时开始

            TRANSITION_FRONT_P1 || TRANSITION_FRONT_P2: 失效保护，进入MC_MODE

            TRANSITION_BACK：若_tilt_control <= _params_tiltrotor.tilt_mc（应该理解为倾转电机已经在旋翼模式下），则进入MC_MODE

固定翼模式请求为ture（ is_fixed_wing_requested()==ture）：

             MC_MODE：切换到TRANSITION_FRONT_P1，转换计时开始

            FW_MODE: 保持原模式

            TRANSITION_FRONT_P1 ：①有空速计，检查空速是否满足转换空速阈值

②没有空速计，则按时间转换

按位计算，切换到TRANSITION_FRONT_P2，并开始计时

            TRANSITION_FRONT_P2: 旋翼倾转完成，切换到FW_MODE

            TRANSITION_BACK：失效保护，进入FW_MODE

②Tiltrotor::update_transition_state(）计算转换过程中的姿态

保持转换模式下的期望航向和推力值

  TRANSITION_FRONT_P1：转换模式P1，后发动机不禁用；

向前倾转电机一定的角度，跟时间成正比；

// tilt rotors forward up to certain angle// 向前倾转电机一定的角度，跟时间成正比if (_tilt_control <= _params_tiltrotor.tilt_transition) {_tilt_control = _params_tiltrotor.tilt_mc +fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f);}

mc权重计算：①有空速计：

if (use_airspeed && _airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {_mc_yaw_weight = 0.0f;//一旦空速满足条件，即减少mc控制}
if (use_airspeed && _airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) {_mc_roll_weight = 1.0f - (_airspeed->indicated_airspeed_m_s - _params_tiltrotor.airspeed_blend_start) /(_params_tiltrotor.airspeed_trans - _params_tiltrotor.airspeed_blend_start);}

②无空速计：

// without airspeed do timed weight changes// 空速计禁用时，随时间改变权重if (!use_airspeed&& (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params->front_trans_time_min * 1e6f)) {_mc_roll_weight = 1.0f - ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) - _params->front_trans_time_min *1e6f) / (_params->front_trans_time_openloop * 1e6f - _params->front_trans_time_min * 1e6f);_mc_yaw_weight = _mc_roll_weight;}

            TRANSITION_FRONT_P2：P2 模式下飞机已经准备切换到Fw，旋翼电机已经完全向前倾转；mc权重置零。

_tilt_control = _params_tiltrotor.tilt_transition +fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f);

同时逐渐减小后部电机的PWM值直至关闭电机。

int rear_value = (1.0f - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /(_params_tiltrotor.front_trans_dur_p2 *1000000.0f)) * (float)(PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + (float)PWM_DEFAULT_MIN;set_rear_motor_state(VALUE, rear_value);

            TRANSITION_BACK：重新启动后部电机，使其待速；设置零油门；

向后倾转电机：

     if (_tilt_control > _params_tiltrotor.tilt_mc) {_tilt_control = _params_tiltrotor.tilt_fw -fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f);}

最后将虚拟姿态拷贝到只是姿态期望值

ps：陆续更新ing，已经丢了一版的数据，十分心痛

pps：若有遗漏或错误，欢迎指正

PX4中vtol_att_control 源码解析相关推荐

  1. 红黑树分析与JDK8中HashMap源码解析

    红黑树分析与JDK8中HashMap源码解析 BST O(1), O(n), O(logn), O(nlogn) 的区别 红黑树-RBTree 插入数据 HashMap中红黑树的插入操作 HashMa ...

  2. JDK8中ConcurrentHashMap源码解析

    在介绍ConcurrentHashMap源码之前,首先需要了解以下几个知识 1.JDK1.8中ConcurrentHashMap的基本结构 2.并发编程的三个概念:可见性,原子性,有序性 3.CAS( ...

  3. java中acquire()_Java高并发系列之AQS中acquire源码解析

    我们知道,AQS中最重要的两个方法就是acquire和release方法.我们本文来走读走读acquire的源码. 首先,tryAcquire是需要子类具体去实现,其作用就是设置state的值,如果设 ...

  4. java compareto源码_java中compareTo源码解析（String类型）

    官网API(JDK13): 先上结论: 1.计算compareTo两边字符串的总长度(length()) 2.求出总长度中的最小值 3.如果长度相等,比较ASCII值相同,返回0 4.如果长度不相同, ...

  5. 源码解析：Spring源码解析笔记（五）接口设计总览

    本文由colodoo(纸伞)整理 QQ 425343603 Java学习交流群(717726984) Spring解析笔记 启动过程部分已经完成,对启动过程源码有兴趣的朋友可以作为参考文章. 源码解析 ...

  6. python处理回显_Python中getpass模块无回显输入源码解析

    本文主要讨论了python中getpass模块的相关内容,具体如下. getpass模块 昨天跟学弟吹牛b安利Python标准库官方文档的时候偶然发现了这个模块.仔细一看内容挺少的,只有两个主要api ...

  7. JDK源码解析 迭代器模式在JAVA的很多集合类中被广泛应用，接下来看看JAVA源码中是如何使用迭代器模式的。

    JDK源码解析 迭代器模式在JAVA的很多集合类中被广泛应用,接下来看看JAVA源码中是如何使用迭代器模式的. 看完这段代码是不是很熟悉,与我们上面代码基本类似.单列集合都使用到了迭代器,我们以Arr ...

  8. JDK源码解析 Comparator 中的策略模式

    JDK源码解析 Comparator 中的策略模式.在Arrays类中有一个 sort() 方法,如下: public class Arrays{public static <T> voi ...

  9. JDK源码解析 —— IO流中的包装类使用到了装饰者模式

    JDK源码解析 IO流中的包装类使用到了装饰者模式. BufferedInputStream, BufferedOutputStream, BufferedReader, BufferedWriter ...

最新文章

  1. 大唐电信JAVA笔试题面试题
  2. MULE ESB简介
  3. django15:中间件
  4. android xml怎么建立,androidXmlSerializer创建XML文件
  5. mybatis-plus对datetime返回去掉.0_0欧姆电阻只能当导线用？12种用法来学习下
  6. 关于深圳城中村小产权房，你应该知道的
  7. truncate(can)
  8. *第九周*数据结构实践项目一【猴子选大王（数组）】
  9. 一个厂商网站的SQL安全检测 （啊D、明小子）
  10. 酷派W711刷机教程
  11. python3读取pdf文档；pdfminer3k
  12. CVE-2017-11176: A step-by-step Linux Kernel exploitation (part 3/4)
  13. mysql 切分_MySQL切分查询用法分析
  14. RF信号下采样/矩阵下采样（附python实现代码）
  15. 量化策略更新换代 五大私募机构演绎“快”字诀
  16. DJ logo图片 DJ logo设计
  17. 支持Apple pay支付的设备
  18. sublime使用简介
  19. 非常普通的鹿中文电脑版
  20. POI操作Word中的表格XWPFTable，在指定位置插入行

热门文章

  1. Windows 提高办公效率的应用
  2. 苹果老板乔布斯在斯坦福大学的演讲
  3. 用python做一个上位机串口通信_【教程】简易Python上位机之LED控制
  4. android 自定义canvas,android随笔之自定义View的Canvas用法
  5. vue项目中创建子路由组件
  6. android解锁界面分析,Android 7.0 锁屏解锁之向上滑动显示解锁界面分析
  7. VM (虚拟机）下载及安装详细步骤
  8. 智慧消防管理系统实现全民消防造福全社会
  9. 嵌入式系统基础A就9周作业
  10. 数据结构与算法分析C++语言描述(第四版)图论学习记录