PX4模块设计之三十九：Commander模块

1. Commander模块简介

### Description
The commander module contains the state machine for mode switching and failsafe behavior.

commander <command> [arguments...]
 Commands:
   start
     [-h]        Enable HIL mode

   calibrate     Run sensor calibration
     mag|baro|accel|gyro|level|esc|airspeed Calibration type
     quick       Quick calibration (accel only, not recommended)

   check         Run preflight checks

   arm
     [-f]        Force arming (do not run preflight checks)

   disarm
     [-f]        Force disarming (disarm in air)

   takeoff

   land

   transition    VTOL transition

   mode          Change flight mode
     manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto
                 :rtl|auto:takeoff|auto:land|auto:precland Flight mode

   pair

   lockdown
     on|off      Turn lockdown on or off

   set_ekf_origin
     lat, lon, alt Origin Latitude, Longitude, Altitude

   lat|lon|alt   Origin latitude longitude altitude

   poweroff      Power off board (if supported)

   stop

   status        print status info

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

class Commander : public ModuleBase<Commander>, public ModuleParams

注：Commander模块采用了ModuleBase, 但没有使用WorkQueue设计，因此在实现上需要实现instantiate方法。

2. 模块入口函数

2.1 主入口commander_main

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

commander_main
 └──> return Commander::main(argc, argv)

2.2 自定义子命令custom_command

模块除支持start/stop/status命令，自定义命令支持以下子命令：

• calibrate: [gyro/mag/baro/accel/level/airspeed/esc]
• check:
• arm: [-f]
• disarm: [-f]
• takeoff:
• land:
• transition:
• mode: [manual/altctl/posctl/auto:mission/auto:loiter/auto:rtl/acro/offboard/stabilized/auto:takeoff/auto:land/auto:precland]
• lockdown: [on/off]
• pair:
• set_ekf_origin: [latitude] [longitude] [altitude]
• poweroff:

Commander::custom_command
 ├──> <!is_running()>
 │   ├──> print_usage("not running")
 │   └──> return 1
 ├──> <!strcmp(argv[0], "calibrate")>
 │   ├──> <!strcmp(argv[1], "gyro")>  // gyro calibration: param1 = 1
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 1.f, 0.f, 0.f, 0.f, 0.0, 0.0, 0.f)
 │   ├──> <!strcmp(argv[1], "mag")>
 │   │   ├──> <argc > 2 && (strcmp(argv[2], "quick") == 0)>  // magnetometer quick calibration: VEHICLE_CMD_FIXED_MAG_CAL_YAW
 │   │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW)
 │   │   └──> <else>  // magnetometer calibration: param2 = 1
 │   │       └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.0, 0.0, 0.f)
 │   ├──> <!strcmp(argv[1], "baro")>  // baro calibration: param3 = 1
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 1.f, 0.f, 0.0, 0.0, 0.f)
 │   ├──> <!strcmp(argv[1], "accel")>
 │   │   ├──> <argc > 2 && (strcmp(argv[2], "quick") == 0)>  // accelerometer quick calibration: param5 = 3
 │   │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 4.0, 0.0, 0.f)
 │   │   └──> <else>  // accelerometer calibration: param5 = 1
 │   │       └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.0, 0.0, 0.f)
 │   ├──> <!strcmp(argv[1], "level")> // board level calibration: param5 = 2
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.0, 0.0, 0.f)
 │   ├──> <!strcmp(argv[1], "airspeed")>  // airspeed calibration: param6 = 2
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 2.0, 0.f)
 │   ├──> <!strcmp(argv[1], "esc")>  // ESC calibration: param7 = 1
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 0.0, 1.f)
 │   └──> <else>
 │       └──> PX4_ERR("missing argument")
 ├──> <!strcmp(argv[0], "check")>
 │   ├──> uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)}
 │   ├──> vehicle_status_sub.copy(&vehicle_status)
 │   ├──> uORB::Subscription vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}
 │   ├──> vehicle_status_flags_sub.copy(&vehicle_status_flags)
 │   ├──> uORB::Subscription vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}
 │   ├──> vehicle_control_mode_sub.copy(&vehicle_control_mode)
 │   ├──> bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags,
 │   │					   vehicle_control_mode,
 │   │					   true, // report_failures
 │   │					   30_s,
 │   │					   false, // safety_buttton_available not known
 │   │					   false) // safety_off not known
 │   ├──> PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED")
 │   ├──> print_health_flags(vehicle_status)
 │   └──> return 0
 ├──> <!strcmp(argv[0], "arm")>
 │   ├──> float param2 = 0.f
 │   ├──> <argc > 1 && !strcmp(argv[1], "-f")>  // 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight)
 │   │   └──> param2 = 21196.f
 │   ├──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
 │   │				     static_cast<float>(vehicle_command_s::ARMING_ACTION_ARM),
 │   │				     param2)
 │   └──> return 0
 ├──> <!strcmp(argv[0], "disarm")>
 │   ├──> float param2 = 0.f
 │   ├──> <argc > 1 && !strcmp(argv[1], "-f")>  // 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight)
 │   │   └──> param2 = 21196.f
 │   ├──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
 │   │				     static_cast<float>(vehicle_command_s::ARMING_ACTION_DISARM),
 │   │				     param2)
 │   └──> return 0
 ├──> <!strcmp(argv[0], "takeoff")>  // switch to takeoff mode and arm
 │   ├──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF)
 │   ├──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
 │   │				     static_cast<float>(vehicle_command_s::ARMING_ACTION_ARM),
 │   │				     0.f)
 │   └──> return 0
 ├──> <!strcmp(argv[0], "land")>
 │   ├──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_LAND)
 │   └──> return 0
 ├──> <!strcmp(argv[0], "transition")>
 │   ├──> uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)}
 │   ├──> vehicle_status_sub.copy(&vehicle_status)
 │   ├──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
 │   │				     (float)(vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ?
 │   │					     vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW :
 │   │					     vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC), 0.0f)
 │   └──> return 0
 ├──> <!strcmp(argv[0], "mode")>
 │   ├──> <!strcmp(argv[1], "manual")) {
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_MANUAL)
 │   ├──> <!strcmp(argv[1], "altctl")) {
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ALTCTL)
 │   ├──> <!strcmp(argv[1], "posctl")) {
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_POSCTL)
 │   ├──> <!strcmp(argv[1], "auto:mission")) {
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,PX4_CUSTOM_SUB_MODE_AUTO_MISSION)
 │   ├──> <!strcmp(argv[1], "auto:loiter")) {
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,PX4_CUSTOM_SUB_MODE_AUTO_LOITER)
 │   ├──> <!strcmp(argv[1], "auto:rtl")) {
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,PX4_CUSTOM_SUB_MODE_AUTO_RTL)
 │   ├──> <!strcmp(argv[1], "acro")) {
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ACRO)
 │   ├──> <!strcmp(argv[1], "offboard")) {
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_OFFBOARD)
 │   ├──> <!strcmp(argv[1], "stabilized")) {
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_STABILIZED)
 │   ├──> <!strcmp(argv[1], "auto:takeoff")) {
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF)
 │   ├──> <!strcmp(argv[1], "auto:land")) {
 │   │   └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,PX4_CUSTOM_SUB_MODE_AUTO_LAND)
 │   └──> <!strcmp(argv[1], "auto:precland")) {
 │       └──> send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND)
 ├──> <!strcmp(argv[0], "lockdown")>
 │   ├──> bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION, strcmp(argv[1], "off") ? 2.0f : 0.0f /* lockdown */, 0.0f)
 │   └──> return (ret ? 0 : 1)
 ├──> <!strcmp(argv[0], "pair")> // GCS pairing request handled by a companion
 │   ├──> bool ret = broadcast_vehicle_command(vehicle_command_s::VEHICLE_CMD_START_RX_PAIR, 10.f)
 │   └──> return (ret ? 0 : 1)
 ├──> <!strcmp(argv[0], "set_ekf_origin")>  // Set the ekf NED origin global coordinates.
 │   ├──> double latitude  = atof(argv[1]) 
 │   ├──> double longitude = atof(argv[2]) 
 │   ├──> float  altitude  = atof(argv[3])
 │   ├──> bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN, 0.f, 0.f, 0.0, 0.0, latitude, longitude, altitude)
 │   └──> return (ret ? 0 : 1)
 ├──> <!strcmp(argv[0], "poweroff")>
 │   ├──> bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN,2.0f)
 │   └──> return (ret ? 0 : 1)
 └──> return print_usage("unknown command")

3. Commander模块重要函数

3.1 task_spawn

这里直接使用px4_task_spawn_cmd创建任务。

Commander::task_spawn
 ├──> _task_id = px4_task_spawn_cmd("commander",SCHED_DEFAULT,SCHED_PRIORITY_DEFAULT + 40,3250,(px4_main_t)&run_trampoline,(char *const *)argv);
 ├──> <_task_id < 0>
 │   ├──> _task_id = -1
 │   └──> return -errno
 ├──> <wait_until_running() < 0>  // wait until task is up & running
 │   ├──> _task_id = -1
 │   └──> return -1
 └──> return 0

注：鉴于ModuleBase::run_trampoline会直接调用instantiate初始化任务，Commander模块就必须对Commander::instantiate方法进行重载实现。

3.2 instantiate

初始化Commander类实例。

Commander::instantiate
 ├──> Commander *instance = new Commander()
 ├──> <instance><argc >= 2 && !strcmp(argv[1], "-h")>
 │   └──> instance->enable_hil()
 └──> return instance;

3.3 init

注：鉴于该模块采用任务，而之前大量的模块使用了WorkQueue的设计方法，在task_spawn里会调用init这个方法，为了对比，保留这个方法。

3.4 Run

根据输入参数及业务逻辑计算输出量，并发布消息。

Commander::Run
 ├──> [LED 初始化]
 ├──> [Buzzer 初始化]
 ├──> [PowerButton 初始化]
 ├──> arm_auth_init(&_mavlink_log_pub, &_vehicle_status.system_id);
 ├──> <while(!should_exit())>
 │   ├──> [update parameters ]
 │   ├──> [Update OA parameter ]
 │   ├──> [handle power button state ]
 │   ├──> [Update land detector ]
 │   ├──> [safety button ]
 │   ├──> [update vtol vehicle status]
 │   ├──> [Auto disarm when landed or kill switch engaged]
 │   ├──> [_geofence_warning_action_off when MAIN_STATE_AUTO_RTL/MAIN_STATE_AUTO_LOITER/MAIN_STATE_AUTO_LAND
 │   ├──> [battery_status_check]
 │   ├──> [If in INIT state, try to proceed to STANDBY state ]
 │   ├──> [start mission result check ]
 │   ├──> [start geofence result check & action ]
 │   ├──> [Check for mission flight termination ]
 │   ├──> [data link checks which update the status]
 │   ├──> [avoidance_check]
 │   ├──> [handle commands last, as the system needs to be updated to handle them ]
 │   ├──> [Check for failure detector status ]
 │   ├──> [Check wind speed actions if either high wind warning or high wind RTL is enabled]
 │   ├──> [Trigger RTL if flight time is larger than max flight time specified in COM_FLT_TIME_MAX.The user is not able to override once above threshold, except for triggering Land.]
 │   ├──> [automatically set or update home position]
 │   ├──> [check for arming state changes]
 │   ├──> [now set navigation state according to failsafe and main state ]
 │   ├──> [publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed
 │   ├──> [play arming and battery warning tunes ]
 │   ├──> [reset arm_tune_played when disarmed ]
 │   ├──> [check if the worker has finished]
 │   ├──> [store last position lock state ]
 │   └──> [sleep if there are no vehicle_commands or action_requests to process]
 ├──> led_deinit();
 └──> buzzer_deinit();

4. 总结

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

• 输入

uORB::Subscription					_action_request_sub {ORB_ID(action_request)};
uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
uORB::Subscription					_estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::Subscription					_estimator_status_sub{ORB_ID(estimator_status)};
uORB::Subscription					_geofence_result_sub{ORB_ID(geofence_result)};
uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
uORB::Subscription					_vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription					_rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)};
uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
uORB::Subscription					_vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription					_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription					_vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription					_vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::Subscription					_wind_sub{ORB_ID(wind)};

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

uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
uORB::SubscriptionMultiArray<distance_sensor_s>         _distance_sensor_subs{ORB_ID::distance_sensor};
uORB::SubscriptionMultiArray<telemetry_status_s>        _telemetry_status_subs{ORB_ID::telemetry_status};

#if defined(BOARD_HAS_POWER_CONTROL)
uORB::Subscription					_power_button_state_sub {ORB_ID(power_button_state)};
#endif // BOARD_HAS_POWER_CONTROL

uORB::SubscriptionData<estimator_status_flags_s>	_estimator_status_flags_sub{ORB_ID(estimator_status_flags)};
uORB::SubscriptionData<mission_result_s>		_mission_result_sub{ORB_ID(mission_result)};
uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};

• 输出

uORB::Publication<actuator_armed_s>			_actuator_armed_pub{ORB_ID(actuator_armed)};
uORB::Publication<commander_state_s>			_commander_state_pub{ORB_ID(commander_state)};
uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
uORB::Publication<test_motor_s>				_test_motor_pub{ORB_ID(test_motor)};
uORB::Publication<actuator_test_s>			_actuator_test_pub{ORB_ID(actuator_test)};
uORB::Publication<vehicle_control_mode_s>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
uORB::Publication<vehicle_status_flags_s>		_vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
uORB::Publication<vehicle_status_s>			_vehicle_status_pub{ORB_ID(vehicle_status)};

uORB::PublicationData<home_position_s>			_home_position_pub{ORB_ID(home_position)};

uORB::Publication<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};

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