commander.cpp
commander.cpp主要是通过指令改变当前的飞行模式
指令分为：
* 遥控器的指令
* 上位机控制台的指令
* mavlink指令
飞行模式分为
* manual
* altctl
* posctl
* mission
* rtl
* loiter
* acro
* offboard
* stabilized
* rattitude
* takeoff
* land
基本流程为订阅数据，处理数据，发布数据
遥控器指令
arm/disarm
上锁逻辑：
因为对于上锁解锁这种条件，不需要改动，也不能改动，安全最为重要，所以按照它的逻辑必定是可以上锁解锁。首先判断是不是要进入上锁模式，即在已经解锁状态，遥控器油门低且yaw轴摇杆小于0.9×1000
/* 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 */ const bool stick_in_lower_left = sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f; const bool arm_switch_to_disarm_transition = arm_switch_is_button == 0 && _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON && sp_man.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.is_rotary_wing || (!status.is_rotary_wing && land_detector.landed)) && (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition) ) {再判断是不是手动的模式（
MAIN_STATE_MANUAL，MAIN_STATE_ACRO，MAIN_STATE_STAB,MAIN_STATE_RATTITUDE）且不是正在降落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_RATTITUDE && !land_detector.landed) { print_reject_arm("NOT DISARMING: Not in manual mode or landed yet."); }不满足上面条件的话 ，就上锁
arming_ret = arming_state_transition(&status, &battery, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, avionics_power_rail_voltage, arm_requirements, hrt_elapsed_time(&commander_boot_timestamp));解锁
逻辑大致与上锁相同，只是摇杆yaw轴要大于0.9*1000
模式切换
只有在遥控器（rc）有输入的时候才能切换
if (!status_flags.rc_input_blocked && sp_man.timestamp != 0 && (hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {主要函数
main_res = set_main_state(&status, &global_position, &local_position, &status_changed)
主要输入飞行器状态，位置信息，internal_state
返回TRANSITION_CHANGED/TRANSITION_NOT_CHANGED与internal_state->main_state.控制台指令
分为两种情况：直接切换模式，发送cmd指令切换数据
直接切换数据，就只能切换模式。（所有的模式都能直接切换）
发送cmd切换数据，不仅能切换模式，还能在切换模式之后发送固定数据。（只有三种模式可以这么切换，takeoff，land，transition，且必须要有位置数据）直接切换模式
在控制台发送
commander mode auto：(misison)进入TRANSITION_DENIED == main_state_transition(&status, new_main_state, main_state_prev, &status_flags, &internal_state)发送cmd指令切换数据
在控制台输入
commander takeoffcommander land，它数据都已经写好了，所以要想改变数据的话，就要改动代码/* see if we got a home position */ if (status_flags.condition_local_position_valid) { if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) { struct vehicle_command_s cmd = { .timestamp = hrt_absolute_time(), .param5 = NAN, .param6 = NAN, /* minimum pitch */ .param1 = NAN, .param2 = NAN, .param3 = NAN, .param4 = NAN, .param7 = NAN, .command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF, .target_system = status.system_id, .target_component = status.component_id }; orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH); (void)orb_unadvertise(h); } else { warnx("arming failed"); }orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH)这句话将cmd topic发送出去在
while中处理cmd消息
cmdtopic更新的话会进入handle_command()函数
主要输入statuscmd判断cmd的模式然后进入
main_state_transition(&status, new_main_state, main_state_prev, &status_flags, &internal_state)
切换模式
改变飞行模式
主要函数
set_nav_state()输入:通过之前设置的
internal_state->main_state与相应的条件，来确定飞行器到底能不能改变模式
输出：status->nav_state
例如：position_ctrl只需要rc连接，解锁，有位置信息case commander_state_s::MAIN_STATE_POSCTL: { if (rc_lost && is_armed) { enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); set_rc_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act); /* As long as there is RC, we can fallback to ALTCTL, or STAB. */ /* A local position estimate is enough for POSCTL for multirotors, * this enables POSCTL using e.g. flow. * For fixedwing, a global position is needed. */ } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, !(posctl_nav_loss_act == 1), !status->is_rotary_wing)) { // nothing to do - everything done in check_invalid_pos_nav_state } else { status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; } } break;set_control_mode()根据之前的
status.nav_state来设置control_mode。switch (status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = stabilization_required(); control_mode.flag_control_attitude_enabled = stabilization_required(); control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; control_mode.flag_control_acceleration_enabled = false; control_mode.flag_control_termination_enabled = false; break; case vehicle_status_s::NAVIGATION_STATE_STAB: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_rattitude_enabled = false; control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; control_mode.flag_control_acceleration_enabled = false; control_mode.flag_control_termination_enabled = false; /* override is not ok in stabilized mode */ control_mode.flag_external_manual_override_ok = false; break; ...
公布数据
control_mode.timestamp = now; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); status.timestamp = now; orb_publish(ORB_ID(vehicle_status), status_pub, &status);
2982
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
