PX4模块设计之三十四：ControlAllocator模块

1. ControlAllocator模块简介

### Description
This implements control allocation. It takes torque and thrust setpoints
as inputs and outputs actuator setpoint messages.

controller <command> [arguments...]
 Commands:
   start

   stop

   status        print status info

注：print_usage函数是具体对应实现。

class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem

注：ControlAllocator模块采用了ModuleBase和WorkQueue设计。

2. 模块入口函数

2.1 主入口control_allocator_main

同样继承了ModuleBase，由ModuleBase的main来完成模块入口。

control_allocator_main
 └──> return ControlAllocator::main(argc, argv)

2.2 自定义子命令custom_command

模块仅支持start/stop/status命令，不支持其他自定义命令。

ControlAllocator::custom_command
 └──> return print_usage("unknown command");

3. ControlAllocator模块重要函数

3.1 task_spawn

这里主要初始化了RCUpdate对象，后续通过WorkQueue来完成进行轮询。

ControlAllocator::task_spawn
 ├──> ControlAllocator *instance = new ControlAllocator();
 ├──> <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中使用，对_input_rc_sub成员变量进行事件回调注册(当有input_rc消息时，会调用SubscriptionCallbackWorkItem::ScheduleNow，再触发RCUpdate::Run过程)。

ControlAllocator::init
 ├──> <!_vehicle_torque_setpoint_sub.registerCallback()>
 │   ├──> PX4_ERR("callback registration failed");
 │   └──> return false;
 ├──> <!_vehicle_thrust_setpoint_sub.registerCallback()>
 │   ├──> PX4_ERR("callback registration failed");
 │   └──> return false;
 ├──> <ENABLE_LOCKSTEP_SCHEDULER> // Backup schedule would interfere with lockstep
 │   └──> ScheduleDelayed(50_ms);
 └──> return true;

3.4 Run

根据设定，计算扭矩和推力值。

ControlAllocator::Run
 ├──> [优雅退出处理]
 ├──> <ENABLE_LOCKSTEP_SCHEDULER> // Backup schedule would interfere with lockstep
 │   └──> ScheduleDelayed(50_ms)
 ├──> <_parameter_update_sub.updated() && !_armed>
 │   ├──> _parameter_update_sub.copy(&param_update)
 │   └──> <_handled_motor_failure_bitmask == 0> // We don't update the geometry after an actuator failure, as it could lead to unexpected results (e.g. a user could add/remove motors, such that the bitmask isn't correct anymore)
 │       ├──> updateParams()
 │       └──> parameters_updated()
 ├──> <_num_control_allocation == 0 || _actuator_effectiveness == nullptr>
 │   └──> return
 ├──> <_vehicle_status_sub.update(&vehicle_status)>
 │   ├──> _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED
 │   ├──> ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT}
 │   ├──> <vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING> // Check if the current flight phase is HOVER or FIXED_WING
 │   │   └──> flight_phase = ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT
 │   ├──> <else>
 │   │   └──> flight_phase = ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT
 │   ├──> <vehicle_status.is_vtol && vehicle_status.in_transition_mode> // Special cases for VTOL in transition
 │   │   ├──> <vehicle_status.in_transition_to_fw) {
 │   │   │   └──> flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_HF_TO_FF
 │   │   └──> <else>
 │   │       └──> flight_phase = ActuatorEffectiveness::FlightPhase::TRANSITION_FF_TO_HF
 │   └──> _actuator_effectiveness->setFlightPhase(flight_phase)// Forward to effectiveness source
 ├──> const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f)  // Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
 ├──> <_vehicle_torque_setpoint_sub.update(&vehicle_torque_setpoint)>  // Run allocator on torque changes
 │   ├──> _torque_sp = matrix::Vector3f(vehicle_torque_setpoint.xyz)
 │   ├──> do_update = true
 │   └──> _timestamp_sample = vehicle_torque_setpoint.timestamp_sample
 ├──> <_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)> // Run allocator on thrust setpoint changes if the torque setpoint
 │   ├──> _thrust_sp = matrix::Vector3f(vehicle_thrust_setpoint.xyz)
 │   └──> <dt > 5_ms>  // has not been updated for more than 5ms
 │       ├──> do_update = true
 │       └──> _timestamp_sample = vehicle_thrust_setpoint.timestamp_sample
 ├──> <do_update>
 │   ├──> _last_run = now
 │   ├──> check_for_motor_failures()
 │   ├──> update_effectiveness_matrix_if_needed(EffectivenessUpdateReason::NO_EXTERNAL_UPDATE)   //发布actuator_servos_trim消息
 │   ├──> [Set control setpoint vector(s) ]
 │   │		matrix::Vector<float, NUM_AXES> c[ActuatorEffectiveness::MAX_NUM_MATRICES]
 │   │		c[0](0) = _torque_sp(0)
 │   │		c[0](1) = _torque_sp(1)
 │   │		c[0](2) = _torque_sp(2)
 │   │		c[0](3) = _thrust_sp(0)
 │   │		c[0](4) = _thrust_sp(1)
 │   │		c[0](5) = _thrust_sp(2)
 │   ├──> <_num_control_allocation > 1>
 │   │   ├──> _vehicle_torque_setpoint1_sub.copy(&vehicle_torque_setpoint)
 │   │   ├──> _vehicle_thrust_setpoint1_sub.copy(&vehicle_thrust_setpoint)
 │   │   ├──> c[1](0) = vehicle_torque_setpoint.xyz[0]
 │   │   ├──> c[1](1) = vehicle_torque_setpoint.xyz[1]
 │   │   ├──> c[1](2) = vehicle_torque_setpoint.xyz[2]
 │   │   ├──> c[1](3) = vehicle_thrust_setpoint.xyz[0]
 │   │   ├──> c[1](4) = vehicle_thrust_setpoint.xyz[1]
 │   │   └──> c[1](5) = vehicle_thrust_setpoint.xyz[2]
 │   └──> <for (int i = 0 i < _num_control_allocation ++i)>
 │       ├──> _control_allocation[i]->setControlSetpoint(c[i])
 │       ├──> _control_allocation[i]->allocate()// Do allocation
 │       ├──> _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp)
 │       ├──> <_has_slew_rate>
 │       │   └──> _control_allocation[i]->applySlewRateLimit(dt)
 │       └──> _control_allocation[i]->clipActuatorSetpoint()
 ├──> publish_actuator_controls()  // Publish actuator setpoint and allocator status(actuator_motors/actuator_servos)
 └──> <now - _last_status_pub >= 5_ms>
     ├──> publish_control_allocator_status()     // 发布control_allocator_status消息
     └──> _last_status_pub = now

4. 总结

具体逻辑业务后续再做深入，从模块代码角度:

• 输入

uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)};  /**< vehicle torque setpoint subscription */
uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_sub{this, ORB_ID(vehicle_thrust_setpoint)};	 /**< vehicle thrust setpoint subscription */

uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1};  /**< vehicle torque setpoint subscription (2. instance) */
uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1};	 /**< vehicle thrust setpoint subscription (2. instance) */

uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};

• 输出

uORB::Publication<control_allocator_status_s>	_control_allocator_status_pub{ORB_ID(control_allocator_status)};	/**< actuator setpoint publication */

uORB::Publication<actuator_motors_s>	_actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<actuator_servos_s>	_actuator_servos_pub{ORB_ID(actuator_servos)};
uORB::Publication<actuator_servos_trim_s>	_actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)};

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