px4最新版commander代码分析
太叔高义
commander位于Firmware/src/modules/commander文件夹中。该部分主要负责对地面站、遥控器以及其它部分发布的cmd命令，包括vehicle_command::VEHICLE_CMD_DO_SET_MODE、VEHICLE_CMD_DO_SET_HOME和VEHICLE_CMD_NAV_GUIDED_ENABLE、VEHICLE_CMD_NAV_TAKEOFF等命令（从名称差不多就能知道相应的命令对应的功能）。
该部分主要是完成无人机任务模式的切换，比如从loiter模式切换到position模式等。为了完成模式的切换，需要在切换模式前检查对应的模式的切换条件是否满足，也就包含了arm检查和失控检测。然后还完成了传感器校验的功能，就是在地面中进行陀螺仪、加速度计、磁力计的校验的功能。
在分析代码之前，先简单介绍一下任务切换的流程，先建立一个大概的思路，这样有利于整个代码的理解。commander的run函数中执行handled_command函数，对不同的命令进行解析。如果是vehicle_command::VEHICLE_CMD_DO_SET_MODE命令就调用main_state_transition()函数完成主状态的切换。在main_state_transition()函数中internal_state->main_state=commander_state_s::MAIN_STATE_AUTO_LOITER或者internal_state->main_state=commander_state_s::MAIN_STATE_AUTO_MISSION_TAKEOFF等。commander中将internal_state_main_state发布，在navigator中订阅该消息，并根据internal_state_main_state（主状态）设置status->nav_state(导航状态)实现任务模式的切换。
由头文件commander.hpp可知，class Commander : public ModuleBase<Commander>, public ModuleParams 所以类Commander是继承了ModuleBase类，所以Commander类中的run()函数是该类的主要的执行函数，所以接来下主要分析该函数。
Commander::run()
{
bool sensor_fail_tune_played = false;
const param_t param_airmode = param_find("MC_AIRMODE");//获取对应的参数
const param_t param_rc_map_arm_switch = param_find("RC_MAP_ARM_SW");
/* initialize */
led_init();//led灯初始化，在boards/px4/对应的飞控文件中实现，主要是完成对应的gpio口的初始化
buzzer_init();//蜂鸣器初始化，跟led一样
#if defined(BOARD_HAS_POWER_CONTROL)
{
// we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen
// in IRQ context.
power_button_state_s button_state{};
button_state.timestamp = hrt_absolute_time();
button_state.event = 0xff;
power_button_state_pub = orb_advertise(ORB_ID(power_button_state), &button_state);
_power_button_state_sub.copy(&button_state);
}
if (board_register_power_state_notification_cb(power_button_state_notification_cb) != 0) {
PX4_ERR("Failed to register power notification callback");
}
#endif // BOARD_HAS_POWER_CONTROL
get_circuit_breaker_params();//获取对应的参数
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
//初始化任务mission的状态，在mavlink启动失败是，允许navigator导航使用存储的mission数据（mission都是存在sd卡中，通过dm_read()函数来读取）
mission_init();
bool param_init_forced = true;
control_status_leds(&status, &armed, true, _battery_warning);//设置状态灯的状态
thread_running = true;
/* update vehicle status to find out vehicle type (required for preflight checks) */
status.system_type = _param_mav_type.get();
//判断无人机的类型
if (is_rotary_wing(&status) || is_vtol(&status)) {
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
} else if (is_fixed_wing(&status)) {
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
} else if (is_ground_rover(&status)) {
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
} else {
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
}
//根据status判断是否是垂直起降固定翼
status.is_vtol = is_vtol(&status);
status.is_vtol_tailsitter = is_vtol_tailsitter(&status);
_boot_timestamp = hrt_absolute_time();
// initially set to failed
_last_lpos_fail_time_us = _boot_timestamp;
_last_gpos_fail_time_us = _boot_timestamp;
_last_lvel_fail_time_us = _boot_timestamp;
// user adjustable duration required to assert arm/disarm via throttle/rudder stick
//用户可调整的持续时间，以通过油门/方向舵杆进行防护/解除防护,就是设置通过油门解锁/上锁需要的时间
uint32_t rc_arm_hyst = _param_rc_arm_hyst.get() * COMMANDER_MONITORING_LOOPSPERMSEC;
int32_t airmode = 0;
int32_t rc_map_arm_switch = 0;
//为创建低优先级的线程初始化好运行环境
/* initialize low priority thread */
pthread_attr_t commander_low_prio_attr;
pthread_attr_init(&commander_low_prio_attr);
pthread_attr_setstacksize(&commander_low_prio_attr, PX4_STACK_ADJUSTED(3304));
#ifndef __PX4_QURT
// This is not supported by QURT (yet).
struct sched_param param;
pthread_attr_getschedparam(&commander_low_prio_attr, ¶m);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
pthread_attr_setschedparam(&commander_low_prio_attr, ¶m);
#endif
pthread_t commander_low_prio_thread;
pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, nullptr);//创建新的线程，也就是会有两个线程在执行
pthread_attr_destroy(&commander_low_prio_attr);
status.system_id = _param_mav_sys_id.get();
arm_auth_init(&mavlink_log_pub, &status.system_id);
// run preflight immediately to find all relevant parameters, but don't report
//立即运行飞行前检查以查找所有相关参数，但不要报告
PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, _arm_requirements.global_position, false, true,
hrt_elapsed_time(&_boot_timestamp));
//各种初始化完成后进入循环，执行主要的程序
while (!should_exit()) {
transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
/* update parameters */
//更新需要的参数
bool params_updated = _parameter_update_sub.updated();
//如果参数更新了，会执行下面程序
if (params_updated || param_init_forced) {
// clear update
parameter_update_s update;
_parameter_update_sub.copy(&update);
// update parameters from storage
updateParams();
// NAV_DLL_ACT value 4 Data Link Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10
if (_param_nav_dll_act.get() == 4) {
mavlink_log_critical(&mavlink_log_pub, "CASA Outback Challenge rules (NAV_DLL_ACT = 4) retired");
_param_nav_dll_act.set(2); // value 2 Return mode
_param_nav_dll_act.commit_no_notification();
}
// NAV_RCL_ACT value 4 RC Auto Recovery (CASA Outback Challenge rules) deleted 2020-03-10
if (_param_nav_rcl_act.get() == 4) {
mavlink_log_critical(&mavlink_log_pub, "CASA Outback Challenge rules (NAV_RCL_ACT = 4) retired");
_param_nav_rcl_act.set(2); // value 2 Return mode
_param_nav_rcl_act.commit_no_notification();
}
/* update parameters */
if (!armed.armed) {//如果没有解锁，则执行下面程序
status.system_type = _param_mav_type.get();
const bool is_rotary = is_rotary_wing(&status) || (is_vtol(&status) && _vtol_status.vtol_in_rw_mode);
const bool is_fixed = is_fixed_wing(&status) || (is_vtol(&status) && !_vtol_status.vtol_in_rw_mode);
const bool is_ground = is_ground_rover(&status);
/* disable manual override for all systems that rely on electronic stabilization */
if (is_rotary) {
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
} else if (is_fixed) {
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
} else if (is_ground) {
status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
}
/* set vehicle_status.is_vtol flag */
status.is_vtol = is_vtol(&status);
status.is_vtol_tailsitter = is_vtol_tailsitter(&status);
/* check and update system / component ID */
status.system_id = _param_mav_sys_id.get();
status.component_id = _param_mav_comp_id.get();
get_circuit_breaker_params();
_status_changed = true;
}
status_flags.avoidance_system_required = _param_com_obs_avoid.get();
status.rc_input_mode = _param_rc_in_off.get();//通过参数获取遥控的输入模式
// percentage (* 0.01) needs to be doubled because RC total interval is 2, not 1
_min_stick_change = _param_min_stick_change.get() * 0.02f;
rc_arm_hyst = _param_rc_arm_hyst.get() * COMMANDER_MONITORING_LOOPSPERMSEC;
//通过参数配置解锁所需要的条件
_arm_requirements.arm_authorization = _param_arm_auth_required.get();
_arm_requirements.esc_check = _param_escs_checks_required.get();
_arm_requirements.global_position = !_param_arm_without_gps.get();
_arm_requirements.mission = _param_arm_mission_required.get();
/* flight mode slots */
//配置6种飞行模式
_flight_mode_slots[0] = _param_fltmode_1.get();
_flight_mode_slots[1] = _param_fltmode_2.get();
_flight_mode_slots[2] = _param_fltmode_3.get();
_flight_mode_slots[3] = _param_fltmode_4.get();
_flight_mode_slots[4] = _param_fltmode_5.get();
_flight_mode_slots[5] = _param_fltmode_6.get();
_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
/* check for unsafe Airmode settings: yaw airmode requires the use of an arming switch */
if (param_airmode != PARAM_INVALID && param_rc_map_arm_switch != PARAM_INVALID) {
param_get(param_airmode, &airmode);
param_get(param_rc_map_arm_switch, &rc_map_arm_switch);
if (airmode == 2 && rc_map_arm_switch == 0) {
airmode = 1; // change to roll/pitch airmode
param_set(param_airmode, &airmode);
mavlink_log_critical(&mavlink_log_pub, "Yaw Airmode requires the use of an Arm Switch")
}
}
_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get());
param_init_forced = false;
}
/* Update OA parameter */
status_flags.avoidance_system_required = _param_com_obs_avoid.get();
#if defined(BOARD_HAS_POWER_CONTROL)
/* handle power button state */
if (_power_button_state_sub.updated()) {
power_button_state_s button_state;
if (_power_button_state_sub.copy(&button_state)) {
if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
#if defined(CONFIG_BOARDCTL_POWEROFF)
if (shutdown_if_allowed() && (px4_shutdown_request() == 0)) {
while (1) { px4_usleep(1); }
}
#endif // CONFIG_BOARDCTL_POWEROFF
}
}
}
#endif // BOARD_HAS_POWER_CONTROL
//这个应该是offboard模式控制相关的设置，待分析
offboard_control_update();
//下面是对供电的检查，如果通过usb供电，就不会解锁
if (_system_power_sub.updated()) {
system_power_s system_power{};
_system_power_sub.copy(&system_power);
if (hrt_elapsed_time(&system_power.timestamp) < 1_s) {
if (system_power.servo_valid &&
!system_power.brick_valid &&
!system_power.usb_connected) {
/* flying only on servo rail, this is unsafe */
status_flags.condition_power_input_valid = false;
} else {
status_flags.condition_power_input_valid = true;
}
#if defined(CONFIG_BOARDCTL_RESET)
if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
/* if the USB hardware connection went away, reboot */
if (_system_power_usb_connected && !system_power.usb_connected) {
/*
* Apparently the USB cable went away but we are still powered,
* so we bring the system back to a nominal state for flight.
* This is important to unload the USB stack of the OS which is
* a relatively complex piece of software that is non-essential
* for flight and continuing to run it would add a software risk
* without a need. The clean approach to unload it is to reboot.
*/
if (shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) {
mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting for flight safety");
while (1) { px4_usleep(1); }
}
}
}
#endif // CONFIG_BOARDCTL_RESET
_system_power_usb_connected = system_power.usb_connected;
}
}
/* update safety topic */
//更新有关安全开关
if (_safety_sub.updated()) {
const bool previous_safety_off = _safety.safety_off;
if (_safety_sub.copy(&_safety)) {
// disarm if safety is now on and still armed
if (armed.armed && _safety.safety_switch_available && !_safety.safety_off) {
bool safety_disarm_allowed = (status.hil_state == vehicle_status_s::HIL_STATE_OFF);
// if land detector is available then prevent disarming via safety button if not landed
if (hrt_elapsed_time(&_land_detector.timestamp) < 1_s) {
bool maybe_landing = (_land_detector.landed || _land_detector.maybe_landed);
if (!maybe_landing) {
safety_disarm_allowed = false;
}
}
if (safety_disarm_allowed) {
if (TRANSITION_CHANGED == arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::SAFETY_BUTTON)) {
_status_changed = true;
}
}
}
// Notify the user if the status of the safety switch changes
if (_safety.safety_switch_available && previous_safety_off != _safety.safety_off) {
if (_safety.safety_off) {
set_tune(TONE_NOTIFY_POSITIVE_TUNE);
} else {
tune_neutral(true);
}
_status_changed = true;
}
}
}
/* update vtol vehicle status*/
//更新垂直起降固定翼的状态
if (_vtol_vehicle_status_sub.updated()) {
/* vtol status changed */
_vtol_vehicle_status_sub.copy(&_vtol_status);
status.vtol_fw_permanent_stab = _vtol_status.fw_permanent_stab;
/* Make sure that this is only adjusted if vehicle really is of type vtol */
if (is_vtol(&status)) {
// Check if there has been any change while updating the flags
const auto new_vehicle_type = _vtol_status.vtol_in_rw_mode ?
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
if (new_vehicle_type != status.vehicle_type) {
status.vehicle_type = _vtol_status.vtol_in_rw_mode ?
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING :
vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
_status_changed = true;
}
if (status.in_transition_mode != _vtol_status.vtol_in_trans_mode) {
status.in_transition_mode = _vtol_status.vtol_in_trans_mode;
_status_changed = true;
}
if (status.in_transition_to_fw != _vtol_status.in_transition_to_fw) {
status.in_transition_to_fw = _vtol_status.in_transition_to_fw;
_status_changed = true;
}
if (status_flags.vtol_transition_failure != _vtol_status.vtol_transition_failsafe) {
status_flags.vtol_transition_failure = _vtol_status.vtol_transition_failsafe;
_status_changed = true;
}
const bool should_soft_stop = (status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
if (armed.soft_stop != should_soft_stop) {
armed.soft_stop = should_soft_stop;
_status_changed = true;
}
}
}
//电调状态检查
if (_esc_status_sub.updated()) {
/* ESCs status changed */
esc_status_s esc_status{};
if (_esc_status_sub.copy(&esc_status)) {
esc_status_check(esc_status);
}
}
//检查位置估计和状态估计是否正常
estimator_check(status_flags);
/* Update land detector */
//落地检查
if (_land_detector_sub.updated()) {
_was_landed = _land_detector.landed;
_land_detector_sub.copy(&_land_detector);
// Only take actions if armed
if (armed.armed) {
if (_was_landed != _land_detector.landed) {
if (_land_detector.landed) {
mavlink_log_info(&mavlink_log_pub, "Landing detected");
} else {
mavlink_log_info(&mavlink_log_pub, "Takeoff detected");
_have_taken_off_since_arming = true;
// Set all position and velocity test probation durations to takeoff value
// This is a larger value to give the vehicle time to complete a failsafe landing
// if faulty sensors cause loss of navigation shortly after takeoff.
_gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
_lpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
_lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
}
}
}
}
// Auto disarm when landed or kill switch engaged
//当落地或者停止开关接通时自动上锁
if (armed.armed) {
// Check for auto-disarm on landing or pre-flight
if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) {
if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming) {
_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s);
_auto_disarm_landed.set_state_and_update(_land_detector.landed, hrt_absolute_time());
} else if (_param_com_disarm_preflight.get() > 0 && !_have_taken_off_since_arming) {
_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s);
_auto_disarm_landed.set_state_and_update(true, hrt_absolute_time());
}
if (_auto_disarm_landed.get_state()) {
arm_disarm(false, true, &mavlink_log_pub,
(_have_taken_off_since_arming ? arm_disarm_reason_t::AUTO_DISARM_LAND : arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT));
}
}
// Auto disarm after 5 seconds if kill switch is engaged
_auto_disarm_killed.set_state_and_update(armed.manual_lockdown || armed.lockdown, hrt_absolute_time());
if (_auto_disarm_killed.get_state()) {
if (armed.manual_lockdown) {
arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::KILL_SWITCH);
} else {
arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::LOCKDOWN);
}
}
} else {
_auto_disarm_landed.set_state_and_update(false, hrt_absolute_time());
_auto_disarm_killed.set_state_and_update(false, hrt_absolute_time());
}
if (_geofence_warning_action_on
&& _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
&& _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER
&& _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
// reset flag again when we switched out of it
_geofence_warning_action_on = false;
}
_cpuload_sub.update(&_cpuload);
//电池状态检查，包括在低电量时自动切换飞行模式
battery_status_check();
/* update subsystem info which arrives from outside of commander*/
//更新来自commander外部的子系统信息
subsystem_info_s info;
while (_subsys_sub.update(&info)) {
set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
_status_changed = true;
}
/* If in INIT state, try to proceed to STANDBY state */
if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
_arm_requirements, hrt_elapsed_time(&_boot_timestamp),
arm_disarm_reason_t::TRANSITION_TO_STANDBY);
if (arming_ret == TRANSITION_DENIED) {
/* do not complain if not allowed into standby */
arming_ret = TRANSITION_NOT_CHANGED;
}
}
/* start mission result check */
const auto prev_mission_instance_count = _mission_result_sub.get().instance_count;
//检查任务结果
if (_mission_result_sub.update()) {
const mission_result_s &mission_result = _mission_result_sub.get();
// if mission_result is valid for the current mission
const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp)
&& (mission_result.instance_count > 0);
status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid;
if (mission_result_ok) {
if (status.mission_failure != mission_result.failure) {
status.mission_failure = mission_result.failure;
_status_changed = true;
if (status.mission_failure) {
mavlink_log_critical(&mavlink_log_pub, "Mission cannot be completed");
}
}
/* Only evaluate mission state if home is set */
if (status_flags.condition_home_position_valid &&
(prev_mission_instance_count != mission_result.instance_count)) {
if (!status_flags.condition_auto_mission_available) {
/* the mission is invalid */
tune_mission_fail(true);
} else if (mission_result.warning) {
/* the mission has a warning */
tune_mission_fail(true);
} else {
/* the mission is valid */
tune_mission_ok(true);
}
}
}
}
// update manual_control_setpoint before geofence (which might check sticks or switches)
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
/* start geofence result check */
//检查电子围栏的结果
_geofence_result_sub.update(&_geofence_result);
const bool in_low_battery_failsafe = _battery_warning > battery_status_s::BATTERY_WARNING_LOW;
// Geofence actions
const bool geofence_action_enabled = _geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE;
if (armed.armed
&& geofence_action_enabled
&& !in_low_battery_failsafe) {
// check for geofence violation transition
if (_geofence_result.geofence_violated && !_geofence_violated_prev) {
switch (_geofence_result.geofence_action) {
case (geofence_result_s::GF_ACTION_NONE) : {
// do nothing
break;
}
case (geofence_result_s::GF_ACTION_WARN) : {
// do nothing, mavlink critical messages are sent by navigator
break;
}
case (geofence_result_s::GF_ACTION_LOITER) : {
if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags,
&_internal_state)) {
_geofence_loiter_on = true;
}
break;
}
case (geofence_result_s::GF_ACTION_RTL) : {
if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
&_internal_state)) {
_geofence_rtl_on = true;
}
break;
}
case (geofence_result_s::GF_ACTION_LAND) : {
if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
&_internal_state)) {
_geofence_land_on = true;
}
break;
}
case (geofence_result_s::GF_ACTION_TERMINATE) : {
PX4_WARN("Flight termination because of geofence");
mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated");
armed.force_failsafe = true;
_status_changed = true;
break;
}
}
}
_geofence_violated_prev = _geofence_result.geofence_violated;
// reset if no longer in LOITER or if manually switched to LOITER
const bool in_loiter_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER;
const bool manual_loiter_switch_on = _manual_control_setpoint.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON;
if (!in_loiter_mode || manual_loiter_switch_on) {
_geofence_loiter_on = false;
}
// reset if no longer in RTL or if manually switched to RTL
const bool in_rtl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL;
const bool manual_return_switch_on = _manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON;
if (!in_rtl_mode || manual_return_switch_on) {
_geofence_rtl_on = false;
}
// reset if no longer in LAND or if manually switched to LAND
const bool in_land_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND;
if (!in_land_mode) {
_geofence_land_on = false;
}
_geofence_warning_action_on = _geofence_warning_action_on || (_geofence_loiter_on || _geofence_rtl_on
|| _geofence_land_on);
} else {
// No geofence checks, reset flags
_geofence_loiter_on = false;
_geofence_rtl_on = false;
_geofence_land_on = false;
_geofence_warning_action_on = false;
_geofence_violated_prev = false;
}
// abort auto mode or geofence reaction if sticks are moved significantly
// but only if not in a low battery handling action
const bool is_rotary_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
const bool override_auto_mode =
(_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT) &&
(_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND);
const bool override_offboard_mode =
(_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT) &&
_internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD;
if ((override_auto_mode || override_offboard_mode) && is_rotary_wing
&& !in_low_battery_failsafe && !_geofence_warning_action_on) {
// transition to previous state if sticks are touched
if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < 1_s && // don't use uninitialized or old messages
((fabsf(_manual_control_setpoint.x) > _min_stick_change) ||
(fabsf(_manual_control_setpoint.y) > _min_stick_change))) {
// revert to position control in any case
main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state);
mavlink_log_info(&mavlink_log_pub, "Pilot took over control using sticks");
}
}
/* Check for mission flight termination */
if (armed.armed && _mission_result_sub.get().flight_termination &&
!status_flags.circuit_breaker_flight_termination_disabled) {
armed.force_failsafe = true;
_status_changed = true;
if (!_flight_termination_printed) {
mavlink_log_critical(&mavlink_log_pub, "Geofence violation! Flight terminated");
_flight_termination_printed = true;
}
if (_counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(&mavlink_log_pub, "Flight termination active");
}
}
/* RC input check */
//遥控输入检查
if (!status_flags.rc_input_blocked && _manual_control_setpoint.timestamp != 0 &&
(hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) {
/* handle the case where RC signal was regained */
if (!status_flags.rc_signal_found_once) {
status_flags.rc_signal_found_once = true;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, status_flags.rc_calibration_valid, status);
_status_changed = true;
} else {
if (status.rc_signal_lost) {
if (_rc_signal_lost_timestamp > 0) {
mavlink_log_info(&mavlink_log_pub, "Manual control regained after %.1fs",
hrt_elapsed_time(&_rc_signal_lost_timestamp) * 1e-6);
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, status_flags.rc_calibration_valid, status);
_status_changed = true;
}
}
status.rc_signal_lost = false;
const bool in_armed_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
const bool arm_switch_or_button_mapped =
_manual_control_setpoint.arm_switch != manual_control_setpoint_s::SWITCH_POS_NONE;
const bool arm_button_pressed = _param_arm_switch_is_button.get()
&& (_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON);
/* DISARM
* check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm
* and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed)
* do it only for rotary wings in manual mode or fixed wing if landed.
* Disable stick-disarming if arming switch or button is mapped */
//通过遥控器的油门进行解锁操作
const bool stick_in_lower_left = _manual_control_setpoint.r < -STICK_ON_OFF_LIMIT
&& (_manual_control_setpoint.z < 0.1f)
&& !arm_switch_or_button_mapped;
const bool arm_switch_to_disarm_transition = !_param_arm_switch_is_button.get() &&
(_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
(_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
if (in_armed_state &&
(status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
(status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _land_detector.landed) &&
(stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) {
const bool manual_thrust_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL
|| _internal_state.main_state == commander_state_s::MAIN_STATE_ACRO
|| _internal_state.main_state == commander_state_s::MAIN_STATE_STAB
|| _internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE;
const bool rc_wants_disarm = (_stick_off_counter == rc_arm_hyst && _stick_on_counter < rc_arm_hyst)
|| arm_switch_to_disarm_transition;
if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) {
arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
(arm_switch_to_disarm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK));
}
_stick_off_counter++;
} else if (!(_param_arm_switch_is_button.get()
&& _manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
/* do not reset the counter when holding the arm button longer than needed */
_stick_off_counter = 0;
}
/* ARM
* check if left stick is in lower right position or arm button is pushed or arm switch has transition from disarm to arm
* and we're in MANUAL mode.
* Disable stick-arming if arming switch or button is mapped */
//通过遥控器的油门进行上锁操作
const bool stick_in_lower_right = _manual_control_setpoint.r > STICK_ON_OFF_LIMIT && _manual_control_setpoint.z < 0.1f
&& !arm_switch_or_button_mapped;
/* allow a grace period for re-arming: preflight checks don't need to pass during that time,
* for example for accidential in-air disarming */
const bool in_arming_grace_period = (_last_disarmed_timestamp != 0)
&& (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s);
const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get() &&
(_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) &&
(_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
(_manual_control_setpoint.z < 0.1f || in_arming_grace_period);
if (!in_armed_state &&
(status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
(stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) {
if ((_stick_on_counter == rc_arm_hyst && _stick_off_counter < rc_arm_hyst) || arm_switch_to_arm_transition) {
/* we check outside of the transition function here because the requirement
* for being in manual mode only applies to manual arming actions.
* the system can be armed in auto if armed via the GCS.
*/
if ((_internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL)
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_ACRO)
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_STAB)
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL)
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL)
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE)
) {
print_reject_arm("Not arming: Switch to a manual mode first");
} else if (!status_flags.condition_home_position_valid &&
(_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)) {
print_reject_arm("Not arming: Geofence RTL requires valid home");
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_ARMED, &armed,
!in_arming_grace_period /* fRunPreArmChecks */,
&mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
(arm_switch_to_arm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK));
if (arming_ret != TRANSITION_CHANGED) {
px4_usleep(100000);
print_reject_arm("Not arming: Preflight checks failed");
}
}
}
_stick_on_counter++;
} else if (!(_param_arm_switch_is_button.get()
&& _manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
/* do not reset the counter when holding the arm button longer than needed */
_stick_on_counter = 0;
}
_last_manual_control_setpoint_arm_switch = _manual_control_setpoint.arm_switch;
if (arming_ret == TRANSITION_DENIED) {
/*
* the arming transition can be denied to a number of reasons:
* - pre-flight check failed (sensors not ok or not calibrated)
* - safety not disabled
* - system not in manual mode
*/
tune_negative(true);
}
/* evaluate the main state machine according to mode switches */
//根据模式的改变估计主状态
bool first_rc_eval = (_last_manual_control_setpoint.timestamp == 0) && (_manual_control_setpoint.timestamp > 0);
transition_result_t main_res = set_main_state(status, &_status_changed);
/* store last position lock state */
_last_condition_local_altitude_valid = status_flags.condition_local_altitude_valid;
_last_condition_local_position_valid = status_flags.condition_local_position_valid;
_last_condition_global_position_valid = status_flags.condition_global_position_valid;
/* play tune on mode change only if armed, blink LED always */
if (main_res == TRANSITION_CHANGED || first_rc_eval) {
tune_positive(armed.armed);
_status_changed = true;
} else if (main_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(&mavlink_log_pub, "Switching to this mode is currently not possible");
}
/* check throttle kill switch */
if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* set lockdown flag */
if (!armed.manual_lockdown) {
const char kill_switch_string[] = "Kill-switch engaged";
if (_land_detector.landed) {
mavlink_log_info(&mavlink_log_pub, kill_switch_string);
} else {
mavlink_log_critical(&mavlink_log_pub, kill_switch_string);
}
_status_changed = true;
armed.manual_lockdown = true;
}
} else if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
if (armed.manual_lockdown) {
mavlink_log_info(&mavlink_log_pub, "Kill-switch disengaged");
_status_changed = true;
armed.manual_lockdown = false;
}
}
/* no else case: do not change lockdown flag in unconfigured case */
} else {
// set RC lost
if (status_flags.rc_signal_found_once && !status.rc_signal_lost) {
// ignore RC lost during calibration
if (!status_flags.condition_calibration_enabled && !status_flags.rc_input_blocked) {
mavlink_log_critical(&mavlink_log_pub, "Manual control lost");
status.rc_signal_lost = true;
_rc_signal_lost_timestamp = _manual_control_setpoint.timestamp;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, status);
_status_changed = true;
}
}
}
// data link checks which update the status
//数传检查
data_link_check();
//避障检查
avoidance_check();
// engine failure detection
// TODO: move out of commander
//引擎错误检查
if (_actuator_controls_sub.updated()) {
/* Check engine failure
* only for fixed wing for now
*/
if (!status_flags.circuit_breaker_engaged_enginefailure_check &&
status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !status.is_vtol && armed.armed) {
actuator_controls_s actuator_controls{};
_actuator_controls_sub.copy(&actuator_controls);
const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
const float current2throttle = _battery_current / throttle;
if (((throttle > _param_ef_throttle_thres.get()) && (current2throttle < _param_ef_current2throttle_thres.get()))
|| status.engine_failure) {
const float elapsed = hrt_elapsed_time(&_timestamp_engine_healthy) / 1e6f;
/* potential failure, measure time */
if ((_timestamp_engine_healthy > 0) && (elapsed > _param_ef_time_thres.get())
&& !status.engine_failure) {
status.engine_failure = true;
_status_changed = true;
PX4_ERR("Engine Failure");
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, status);
}
}
} else {
/* no failure reset flag */
_timestamp_engine_healthy = hrt_absolute_time();
if (status.engine_failure) {
status.engine_failure = false;
_status_changed = true;
}
}
}
/* Reset main state to loiter or auto-mission after takeoff is completed.
* Sometimes, the mission result topic is outdated and the mission is still signaled
* as finished even though we only just started with the takeoff. Therefore, we also
* check the timestamp of the mission_result topic. */
//在起飞任务完成后，转到悬停或者任务模式
if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
&& (_mission_result_sub.get().timestamp > _internal_state.timestamp)
&& _mission_result_sub.get().finished) {
const bool mission_available = (_mission_result_sub.get().timestamp > _boot_timestamp)
&& (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid;
if ((_param_takeoff_finished_action.get() == 1) && mission_available) {
main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &_internal_state);
} else {
main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state);
}
}
/* check if we are disarmed and there is a better mode to wait in */
//检查是否上锁和是否有一个更好的模式在等待
if (!armed.armed) {
/* if there is no radio control but GPS lock the user might want to fly using
* just a tablet. Since the RC will force its mode switch setting on connecting
* we can as well just wait in a hold mode which enables tablet control.
*/
if (status.rc_signal_lost && (_internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL)
&& status_flags.condition_home_position_valid) {
main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state);
}
}
/* handle commands last, as the system needs to be updated to handle them */
//解析command命令
if (_cmd_sub.updated()) {
/* got command */
const unsigned last_generation = _cmd_sub.get_last_generation();
vehicle_command_s cmd;
if (_cmd_sub.copy(&cmd)) {
if (_cmd_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("vehicle_command lost, generation %d -> %d", last_generation, _cmd_sub.get_last_generation());
}
//根据当前状态和解锁情况对command进行解析
if (handle_command(&status, cmd, &armed, _command_ack_pub)) {
_status_changed = true;
}
}
}
/* Check for failure detector status */
//无人机失控状态检测
const bool failure_detector_updated = _failure_detector.update(status);
if (failure_detector_updated) {
const uint8_t failure_status = _failure_detector.getStatus();
if (failure_status != status.failure_detector_status) {
status.failure_detector_status = failure_status;
_status_changed = true;
}
}
if (armed.armed &&
failure_detector_updated) {
if (_failure_detector.isFailure()) {
const hrt_abstime time_at_arm = armed.armed_time_ms * 1000;
if (hrt_elapsed_time(&time_at_arm) < 500_ms) {
// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::FAILURE_DETECTOR);
_status_changed = true;
mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request");
}
}
if (hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get())) {
// This handles the case where something fails during the early takeoff phase
if (!_lockdown_triggered) {
armed.lockdown = true;
_lockdown_triggered = true;
_status_changed = true;
mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown");
}
} else if (!status_flags.circuit_breaker_flight_termination_disabled &&
!_flight_termination_triggered && !_lockdown_triggered) {
armed.force_failsafe = true;
_flight_termination_triggered = true;
_status_changed = true;
mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: terminate flight");
set_tune_override(TONE_PARACHUTE_RELEASE_TUNE);
}
}
}
/* Get current timestamp */
const hrt_abstime now = hrt_absolute_time();
// automatically set or update home position
//自动更新或者设置家的位置
if (!_home_pub.get().manual_home) {
const vehicle_local_position_s &local_position = _local_position_sub.get();
// set the home position when taking off, but only if we were previously disarmed
// and at least 500 ms from commander start spent to avoid setting home on in-air restart
if (_should_set_home_on_takeoff && _was_landed && !_land_detector.landed &&
(hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
_should_set_home_on_takeoff = false;
set_home_position();
}
if (!armed.armed) {
if (status_flags.condition_home_position_valid) {
if (_land_detector.landed && local_position.xy_valid && local_position.z_valid) {
/* distance from home */
float home_dist_xy = -1.0f;
float home_dist_z = -1.0f;
mavlink_wpm_distance_to_point_local(_home_pub.get().x, _home_pub.get().y, _home_pub.get().z,
local_position.x, local_position.y, local_position.z,
&home_dist_xy, &home_dist_z);
if ((home_dist_xy > local_position.eph * 2.0f) || (home_dist_z > local_position.epv * 2.0f)) {
/* update when disarmed, landed and moved away from current home position */
set_home_position();
}
}
} else {
/* First time home position update - but only if disarmed */
set_home_position();
/* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin.
* This allows home altitude to be used in the calculation of height above takeoff location when GPS
* use has commenced after takeoff. */
if (!status_flags.condition_home_position_valid) {
set_home_position_alt_only();
}
}
}
}
// check for arming state change
//检查解锁状态改变
if (_was_armed != armed.armed) {
_status_changed = true;
if (armed.armed) {
if (!_land_detector.landed) { // check if takeoff already detected upon arming
_have_taken_off_since_arming = true;
}
} else { // increase the flight uuid upon disarming
const int32_t flight_uuid = _param_flight_uuid.get() + 1;
_param_flight_uuid.set(flight_uuid);
_param_flight_uuid.commit_no_notification();
_last_disarmed_timestamp = hrt_absolute_time();
_should_set_home_on_takeoff = true;
}
}
if (!armed.armed) {
/* Reset the flag if disarmed. */
_have_taken_off_since_arming = false;
}
_was_armed = armed.armed;
/* now set navigation state according to failsafe and main state */
//根据失控和主状态设置导航状态
bool nav_state_changed = set_nav_state(&status,
&armed,
&_internal_state,
&mavlink_log_pub,
(link_loss_actions_t)_param_nav_dll_act.get(),
_mission_result_sub.get().finished,
_mission_result_sub.get().stay_in_failsafe,
status_flags,
_land_detector.landed,
(link_loss_actions_t)_param_nav_rcl_act.get(),
(offboard_loss_actions_t)_param_com_obl_act.get(),
(offboard_loss_rc_actions_t)_param_com_obl_rc_act.get(),
(position_nav_loss_actions_t)_param_com_posctl_navl.get());
if (nav_state_changed) {
status.nav_state_timestamp = hrt_absolute_time();
}
if (status.failsafe != _failsafe_old) {
_status_changed = true;
if (status.failsafe) {
mavlink_log_info(&mavlink_log_pub, "Failsafe mode activated");
} else {
mavlink_log_info(&mavlink_log_pub, "Failsafe mode deactivated");
}
_failsafe_old = status.failsafe;
}
//发布一些状态
/* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at 2 Hz or immediately when changed */
if (hrt_elapsed_time(&status.timestamp) >= 500_ms || _status_changed || nav_state_changed) {
update_control_mode();
status.timestamp = hrt_absolute_time();
_status_pub.publish(status);
switch ((PrearmedMode)_param_com_prearm_mode.get()) {
case PrearmedMode::DISABLED:
/* skip prearmed state */
armed.prearmed = false;
break;
case PrearmedMode::ALWAYS:
/* safety is not present, go into prearmed
* (all output drivers should be started / unlocked last in the boot process
* when the rest of the system is fully initialized)
*/
armed.prearmed = (hrt_elapsed_time(&_boot_timestamp) > 5_s);
break;
case PrearmedMode::SAFETY_BUTTON:
if (_safety.safety_switch_available) {
/* safety switch is present, go into prearmed if safety is off */
armed.prearmed = _safety.safety_off;
} else {
/* safety switch is not present, do not go into prearmed */
armed.prearmed = false;
}
break;
default:
armed.prearmed = false;
break;
}
armed.timestamp = hrt_absolute_time();
_armed_pub.publish(armed);
/* publish internal state for logging purposes */
_internal_state.timestamp = hrt_absolute_time();
_commander_state_pub.publish(_internal_state);
/* publish vehicle_status_flags */
status_flags.timestamp = hrt_absolute_time();
// Evaluate current prearm status
if (!armed.armed && !status_flags.condition_calibration_enabled) {
bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, status, status_flags, true, false, true, 30_s);
bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, status_flags, _safety, _arm_requirements, status, false);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, (preflight_check_res
&& prearm_check_res), status);
}
_vehicle_status_flags_pub.publish(status_flags);
}
/* play arming and battery warning tunes */
if (!_arm_tune_played && armed.armed &&
(_safety.safety_switch_available || (_safety.safety_switch_available && _safety.safety_off))) {
/* play tune when armed */
set_tune(TONE_ARMING_WARNING_TUNE);
_arm_tune_played = true;
} else if (!status_flags.usb_connected &&
(status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
(_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
/* play tune on battery critical */
set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
(_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) {
/* play tune on battery warning */
set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
} else if (status.failsafe) {
tune_failsafe(true);
} else {
set_tune(TONE_STOP_TUNE);
}
/* reset arm_tune_played when disarmed */
if (!armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) {
//Notify the user that it is safe to approach the vehicle
if (_arm_tune_played) {
tune_neutral(true);
}
_arm_tune_played = false;
}
/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
status_flags.condition_system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT);
if (!sensor_fail_tune_played && (!status_flags.condition_system_sensors_initialized
&& status_flags.condition_system_hotplug_timeout)) {
set_tune_override(TONE_GPS_WARNING_TUNE);
sensor_fail_tune_played = true;
_status_changed = true;
}
_counter++;
int blink_state = blink_msg_state();
if (blink_state > 0) {
/* blinking LED message, don't touch LEDs */
if (blink_state == 2) {
/* blinking LED message completed, restore normal state */
control_status_leds(&status, &armed, true, _battery_warning);
}
} else {
/* normal state */
control_status_leds(&status, &armed, _status_changed, _battery_warning);
}
_status_changed = false;
arm_auth_update(now, params_updated || param_init_forced);
px4_usleep(COMMANDER_MONITORING_INTERVAL);
}
thread_should_exit = true;
/* wait for threads to complete */
int ret = pthread_join(commander_low_prio_thread, nullptr);
if (ret) {
warn("join failed: %d", ret);
}
rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF);
/* close fds */
led_deinit();
buzzer_deinit();
thread_running = false;
}以上代码提供了command代码框架，如果需要二次开发，可以查找到对应的部分进行仔细分析，再进行开发。作者QQ：530655014 欢迎交流
类似资料：