PX4模块设计之三十五:MulticopterAttitudeControl模块
Posted lida2003
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了PX4模块设计之三十五:MulticopterAttitudeControl模块相关的知识,希望对你有一定的参考价值。
PX4模块设计之三十五:MulticopterAttitudeControl模块
1. MulticopterAttitudeControl模块简介
### Description
This implements the multicopter attitude controller. It takes attitude
setpoints (`vehicle_attitude_setpoint`) as inputs and outputs a rate setpoint.
The controller has a P loop for angular error
Publication documenting the implemented Quaternion Attitude Control:
Nonlinear Quadrocopter Attitude Control (2013)
by Dario Brescianini, Markus Hehn and Raffaello D'Andrea
Institute for Dynamic Systems and Control (IDSC), ETH Zurich
https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf
mc_att_control <command> [arguments...]
Commands:
start
[vtol] VTOL mode
stop
status print status info
注:print_usage函数是具体对应实现。
class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>, public ModuleParams, public px4::WorkItem
注:MulticopterAttitudeControl模块采用了ModuleBase和WorkQueue设计。
2. 模块入口函数
2.1 主入口mc_att_control_main
同样继承了ModuleBase,由ModuleBase的main来完成模块入口。
mc_att_control_main
└──> return MulticopterAttitudeControl::main(argc, argv)
2.2 自定义子命令custom_command
模块仅支持start/stop/status命令,不支持其他自定义命令。
MulticopterAttitudeControl::custom_command
└──> return print_usage("unknown command");
3. MulticopterAttitudeControl模块重要函数
3.1 task_spawn
这里主要初始化了MulticopterAttitudeControl对象,后续通过WorkQueue来完成进行轮询。
MulticopterAttitudeControl::task_spawn
├──> bool vtol = false;
├──> <argc > 1><strcmp(argv[1], "vtol") == 0>
│ └──> vtol = true;
├──> MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(vtol)
├──> <instance>
│ ├──> _object.store(instance)
│ ├──> _task_id = task_id_is_work_queue
│ └──> <instance->init()>
│ └──> return PX4_OK
├──> <else>
│ └──> PX4_ERR("alloc failed")
├──> delete instance
├──> _object.store(nullptr)
├──> _task_id = -1
└──> return PX4_ERROR
3.2 instantiate
注:鉴于该模块不采用任务(线程),所以ModuleBase::run_trampoline无需执行,所以可以不实现。
3.3 init
在task_spawn中使用,对_vehicle_attitude_sub成员变量进行事件回调注册(当有vehicle_attitude消息时,会调用SubscriptionCallbackWorkItem::ScheduleNow,再触发MulticopterAttitudeControl::Run过程)。
MulticopterAttitudeControl::init
├──> <!_vehicle_attitude_sub.registerCallback()>
│ ├──> PX4_ERR("callback registration failed")
│ └──> return false
└──> return true
3.4 Run
根据设定和传感数据,计算飞行姿态。
MulticopterAttitudeControl::Run
├──> [优雅退出处理]
├──> <_parameter_update_sub.updated()> // Check if parameters have changed
│ ├──> _parameter_update_sub.copy(¶m_update);
│ ├──> updateParams();
│ └──> parameters_updated();
├──> <!_vehicle_attitude_sub.update(&v_att)>
│ └──> return
├──> const float dt = math::constrain(((v_att.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f); // Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
├──> _last_run = v_att.timestamp_sample;
├──> const Quatf qv_att.q;
├──> <_vehicle_attitude_setpoint_sub.updated()> // Check for new attitude setpoint
│ └──> <_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint) && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)>
│ ├──> _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);
│ ├──> _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
│ └──> _last_attitude_setpoint = vehicle_attitude_setpoint.timestamp;
├──> <_quat_reset_counter != v_att.quat_reset_counter> // Check for a heading reset
│ ├──> const Quatf delta_q_reset(v_att.delta_q_reset);
│ ├──> _man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi()); // for stabilized attitude generation only extract the heading change from the delta quaternion
│ ├──> <v_att.timestamp > _last_attitude_setpoint>
│ │ └──> _attitude_control.adaptAttitudeSetpoint(delta_q_reset); // adapt existing attitude setpoint unless it was generated after the current attitude estimate
│ └──> _quat_reset_counter = v_att.quat_reset_counter;
├──> _manual_control_setpoint_sub.update(&_manual_control_setpoint);
├──> _vehicle_control_mode_sub.update(&_vehicle_control_mode);
├──> <_vehicle_land_detected_sub.updated()><_vehicle_land_detected_sub.copy(&vehicle_land_detected)>
│ └──> _landed = vehicle_land_detected.landed;
├──> _vehicle_status_sub.updated()><_vehicle_status_sub.copy(&vehicle_status)>
│ ├──> _vehicle_type_rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
│ ├──> _vtol = vehicle_status.is_vtol;
│ ├──> _vtol_in_transition_mode = vehicle_status.in_transition_mode;
│ └──> _vtol_tailsitter = vehicle_status.is_vtol_tailsitter;
├──> bool attitude_setpoint_generated = false;
├──> const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode);
├──> const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode); // vehicle is a tailsitter in transition mode
├──> bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
├──> <run_att_ctrl>
│ ├──> <_vehicle_control_mode.flag_control_manual_enabled &&
│ │ !_vehicle_control_mode.flag_control_altitude_enabled &&
│ │ !_vehicle_control_mode.flag_control_velocity_enabled &&
│ │ !_vehicle_control_mode.flag_control_position_enabled> // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
│ │ ├──> generate_attitude_setpoint(q, dt, _reset_yaw_sp); // 发布vehicle_attitude_setpoint消息
│ │ └──> attitude_setpoint_generated = true;
│ ├──> <else>
│ │ ├──> _man_x_input_filter.reset(0.f);
│ │ └──> _man_y_input_filter.reset(0.f);
│ ├──> Vector3f rates_sp = _attitude_control.update(q);
│ ├──> const hrt_abstime now = hrt_absolute_time();
│ ├──> autotune_attitude_control_status_s pid_autotune;
│ ├──> <_autotune_attitude_control_status_sub.copy(&pid_autotune)><(pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL
│ │ || pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH
│ │ || pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW
│ │ || pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST)
│ │ && ((now - pid_autotune.timestamp) < 1_s)>
│ │ └──> rates_sp += Vector3f(pid_autotune.rate_sp);
│ │ [publish rate setpoint]
│ ├──> vehicle_rates_setpoint_s rates_setpoint;
│ ├──> rates_setpoint.roll = rates_sp(0);
│ ├──> rates_setpoint.pitch = rates_sp(1);
│ ├──> rates_setpoint.yaw = rates_sp(2);
│ ├──> _thrust_setpoint_body.copyTo(rates_setpoint.thrust_body);
│ ├──> rates_setpoint.timestamp = hrt_absolute_time();
│ └──> _vehicle_rates_setpoint_pub.publish(rates_setpoint); //发布vehicle_rates_setpoint消息
└──> _reset_yaw_sp = !attitude_setpoint_generated || _landed || (_vtol && _vtol_in_transition_mode); // reset yaw setpoint during transitions, tailsitter.cpp generates, attitude setpoint for the transition
4. 总结
具体逻辑业务后续再做深入,从模块代码角度:
- 输入
uORB::SubscriptionInterval _parameter_update_subORB_ID(parameter_update), 1_s;
uORB::Subscription _autotune_attitude_control_status_subORB_ID(autotune_attitude_control_status);
uORB::Subscription _manual_control_setpoint_subORB_ID(manual_control_setpoint); /**< manual control setpoint subscription */
uORB::Subscription _vehicle_attitude_setpoint_subORB_ID(vehicle_attitude_setpoint); /**< vehicle attitude setpoint subscription */
uORB::Subscription _vehicle_control_mode_subORB_ID(vehicle_control_mode); /**< vehicle control mode subscription */
uORB::Subscription _vehicle_status_subORB_ID(vehicle_status); /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_subORB_ID(vehicle_land_detected); /**< vehicle land detected subscription */
uORB::SubscriptionCallbackWorkItem _vehicle_attitude_subthis, ORB_ID(vehicle_attitude);
- 输出
uORB::Publication<vehicle_rates_setpoint_s> _vehicle_rates_setpoint_pubORB_ID(vehicle_rates_setpoint); /**< rate setpoint publication */
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub;
5. 参考资料
【1】PX4开源软件框架简明简介
【2】PX4模块设计之十一:Built-In框架
【3】PX4模块设计之十二:High Resolution Timer设计
【4】PX4模块设计之十三:WorkQueue设计
【5】PX4模块设计之十七:ModuleBase模块
【6】PX4模块设计之三十:Hysteresis类
【7】PX4 modules_main
以上是关于PX4模块设计之三十五:MulticopterAttitudeControl模块的主要内容,如果未能解决你的问题,请参考以下文章
PX4模块设计之三十七:MulticopterRateControl模块
PX4模块设计之三十二:AttitudeEstimatorQ模块