PX4模块设计之三十四:ControlAllocator模块
Posted lida2003
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了PX4模块设计之三十四:ControlAllocator模块相关的知识,希望对你有一定的参考价值。
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(¶m_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_phaseActuatorEffectiveness::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_subthis, ORB_ID(vehicle_torque_setpoint); /**< vehicle torque setpoint subscription */
uORB::SubscriptionCallbackWorkItem _vehicle_thrust_setpoint_subthis, ORB_ID(vehicle_thrust_setpoint); /**< vehicle thrust setpoint subscription */
uORB::Subscription _vehicle_torque_setpoint1_subORB_ID(vehicle_torque_setpoint), 1; /**< vehicle torque setpoint subscription (2. instance) */
uORB::Subscription _vehicle_thrust_setpoint1_subORB_ID(vehicle_thrust_setpoint), 1; /**< vehicle thrust setpoint subscription (2. instance) */
uORB::SubscriptionInterval _parameter_update_subORB_ID(parameter_update), 1_s;
uORB::Subscription _vehicle_status_subORB_ID(vehicle_status);
uORB::Subscription _failure_detector_status_subORB_ID(failure_detector_status);
- 输出
uORB::Publication<control_allocator_status_s> _control_allocator_status_pubORB_ID(control_allocator_status); /**< actuator setpoint publication */
uORB::Publication<actuator_motors_s> _actuator_motors_pubORB_ID(actuator_motors);
uORB::Publication<actuator_servos_s> _actuator_servos_pubORB_ID(actuator_servos);
uORB::Publication<actuator_servos_trim_s> _actuator_servos_trim_pubORB_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
以上是关于PX4模块设计之三十四:ControlAllocator模块的主要内容,如果未能解决你的问题,请参考以下文章
PX4模块设计之三十六:MulticopterPositionControl模块
PX4模块设计之三十七:MulticopterRateControl模块