PX4模块设计之三十一：ManualControl模块

Posted 2022-09-07 lida2003

PX4模块设计之三十一：ManualControl模块

• 1. ManualControl模块简介
• 2. 模块入口函数
• • 2.1 主入口manual_control_main
  • 2.2 自定义子命令custom_command
• 3. ManualControl模块重要函数
• • 3.1 task_spawn
  • 3.2 instantiate
  • 3.3 init
  • 3.4 Run
• 4. 总结
• 5. 参考资料

1. ManualControl模块简介

从帮助的角度，该模块主要处理manual_control_inputs消息，发布one manual_control_setpoint消息。

### Description
Module consuming manual_control_inputs publishing one manual_control_setpoint.

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

   stop

   status        print status info

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

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

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

2. 模块入口函数

2.1 主入口manual_control_main

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

manual_control_main
 └──> return ManualControl::main(argc, argv)

2.2 自定义子命令custom_command

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

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

3. ManualControl模块重要函数

3.1 task_spawn

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

ManualControl::task_spawn
 ├──> ManualControl *instance = new ManualControl()
 ├──> <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中使用，直接调用SubscriptionCallbackWorkItem::ScheduleNow，ManualControl::Run的初次调度任务。

ManualControl::init
 ├──> ScheduleNow()
 └──> return true

3.4 Run

根据RC遥控器控制输入，发布不同switch和setpoint消息。

ManualControl::Run
 ├──> [优雅退出处理]
 ├──> <_vehicle_status_sub.updated()><_vehicle_status_sub.copy(&vehicle_status)>  //飞行器状态：system_id/rotary_wing/vtol
 │   ├──> _system_id = vehicle_status.system_id;
 │   ├──> _rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
 │   └──> _vtol = vehicle_status.is_vtol;
 ├──> <_parameter_update_sub.updated()>
 │   ├──> _parameter_update_sub.copy(&param_update)
 │   ├──> updateParams()
 │   ├──> _stick_arm_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms)
 │   ├──> _stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms)
 │   ├──> _button_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms)
 │   ├──> _selector.setRcInMode(_param_com_rc_in_mode.get());
 │   ├──> _selector.setTimeout(_param_com_rc_loss_t.get() * 1_s);
 │   └──> [MAN_ARM_GESTURE处理场景：1)RC_MAP_ARM_SW & MAN_ARM_GESTURE: disable arm gesture if an arm switch is configured；2)MC_AIRMODE & MAN_ARM_GESTURE: check for unsafe Airmode settings: yaw airmode requires disabling the stick arm gesture]
 ├──> _selector.updateValidityOfChosenInput(now) //检查RC输入有效性
 ├──> <for (int i = 0; i < MAX_MANUAL_INPUT_COUNT; i++)><_manual_control_setpoint_subs[i].update(&manual_control_input)>
 │   └──>_selector.updateWithNewInputSample(now, manual_control_input, i);
 ├──> switches_updated = _manual_control_switches_sub.update(&switches);
 ├──> <_selector.setpoint().valid> //setpoint有效
 │   ├──> _published_invalid_once = false
 │   ├──> processStickArming(_selector.setpoint())
 │   ├──> [ 更新_selector.setpoint().sticks_moving ]
 │   ├──> <switches_updated><_previous_switches_initialized>
 │   │   ├──> <switches.mode_slot != _previous_switches.mode_slot>
 │   │   │   └──> evaluateModeSlot(switches.mode_slot)
 │   │   ├──> [message: Arming button/switch]  // _action_request_pub发布
 │   │   ├──> [message:Return switch] // _action_request_pub发布
 │   │   ├──> [message:Loiter switch] // _action_request_pub发布
 │   │   ├──> [message:Offboard switch] // _action_request_pub发布
 │   │   ├──> [message:Kill switch] // _action_request_pub发布
 │   │   ├──> [message:Gear switch]        // _landing_gear_pub发布
 │   │   ├──> [message:Transition switch] // _action_request_pub发布
 │   │   ├──> [message:Photo switch] // _action_request_pub发布
 │   │   ├──> [message:Video switch] // _action_request_pub发布
 │   │   ├──> _previous_switches_initialized = true;
 │   │   └──>  _previous_switches = switches;
 │   ├──> <else>
 │   │   └──> _previous_switches_initialized = false
 │   ├──> _selector.setpoint().timestamp = now;
 │   ├──> _manual_control_setpoint_pub.publish(_selector.setpoint());  //有效setpoint发布
 ├──> <！_selector.setpoint().valid> //setpoint无效
 │   ├──> <!_published_invalid_once>
 │   │   ├──> _published_invalid_once = true
 │   │   └──> _manual_control_setpoint_pub.publish(_selector.setpoint())  //无效setpoint发布
 │   ├──> _x_diff.reset();
 │   ├──> _y_diff.reset();
 │   ├──> _z_diff.reset();
 │   ├──> _r_diff.reset();
 │   ├──> _stick_arm_hysteresis.set_state_and_update(false, now);
 │   ├──> _stick_disarm_hysteresis.set_state_and_update(false, now);
 │   └──> _button_hysteresis.set_state_and_update(false, now);
 ├──> _last_time = now
 └──> ScheduleDelayed(200_ms)

注：ManualControl::Run的主要途径是Init/Run和下面两个WorkItem内部的ScheduleNow触发的。

uORB::SubscriptionCallbackWorkItem _manual_control_setpoint_subs[MAX_MANUAL_INPUT_COUNT] 
	this, ORB_ID(manual_control_input), 0,
	this, ORB_ID(manual_control_input), 1,
	this, ORB_ID(manual_control_input), 2,
;
uORB::SubscriptionCallbackWorkItem _manual_control_switches_subthis, ORB_ID(manual_control_switches);

4. 总结

从代码的角度

• 输入

  uORB::SubscriptionInterval _parameter_update_subORB_ID(parameter_update), 1_s;
  uORB::Subscription _vehicle_status_subORB_ID(vehicle_status);
  uORB::SubscriptionCallbackWorkItem _manual_control_setpoint_subs[MAX_MANUAL_INPUT_COUNT] this, ORB_ID(manual_control_input), 0,this, ORB_ID(manual_control_input), 1,this, ORB_ID(manual_control_input), 2,;
  uORB::SubscriptionCallbackWorkItem _manual_control_switches_subthis, ORB_ID(manual_control_switches);

• 输出

  uORB::Publication<action_request_s> _action_request_pubORB_ID(action_request);
  uORB::Publication<landing_gear_s> _landing_gear_pubORB_ID(landing_gear);
  uORB::Publication<manual_control_setpoint_s> _manual_control_setpoint_pubORB_ID(manual_control_setpoint);

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