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
扫一扫
3585
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
