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(&param_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 q{v_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_sub{ORB_ID(parameter_update), 1_s};

uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};       /**< manual control setpoint subscription */
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};   /**< vehicle attitude setpoint subscription */
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};             /**< vehicle control mode subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};                         /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};           /**< vehicle land detected subscription */

uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};

• 输出

uORB::Publication<vehicle_rates_setpoint_s>     _vehicle_rates_setpoint_pub{ORB_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