|
首先，感谢“阿木”小伙伴们在PX4社区的各个方面做出贡献。在学习也是px4的过程中，我作为个人的小心得，以下纰漏蛮多，还望大家仁义包涵、期待批评。但我们的目的只有一个——在想改变一个规则之前，先学会它，打破再立（不破不立）
闲话少絮，归书正文。
Commander 这个是 px4 源代码中的控制命令模块（比如由 px4源代码中的控制命令模块，Alti_Mode，Pot_Mode，Auto_在同时切换的过程中成功切换模式）在切换模式中，所有模式都不能切换，都切换在执行相应的飞行检查。
自稳的切换模式，无需满足多种飞行条件。
须知点：
切换模式（站台切换，通常我们的亮灯只有3个档位，因此需要更多的模式切换，“修改”模式切换3个模式——则需要选择多个模式_SW档位切换）：这类模式，外挂样例，包含独立模式，切换模式，包含独立模式切换模式。
这样所有内容都在指挥官内部完成的，最终能不能成功的切换就看各种模式需要的传感器满足条件，以及切换间隙、故障问题的判断了。
一模式基本分类：阿木小伙伴先探过路自己补充一点点
我们需要有明确的想法：这样表达我们对指挥官的理解。为了说明白截图能够方便上图的地方我可以少说话（为了在窗户上登着QQ截图） ，随心所欲啊，涵了伙伴们），来点小伙伴们没有人提到过的东西。
1、对于指挥官文件夹内文件之间的关系；
这些部分暂时放出来以后我会伟伟道来这里（哈哈）
2、整个功能的基本层次结构。
首先，需要明确：commander.cpp、commander_params.c是commander模块线程（这样叫、道这个线程是暂挂的哦，当然需要基于任务）的任务调度入口文件：
int command_main ( int argc, char *argv[])
函数的“入口和跟进方”法大家关注了这就不累了，不过还是把入口文件在下面，便于阅读。
int command_main(int argc, char*argv[])
{不明确的
如果（argc <2）{未定义
用法（'缺少命令'）；
返回1；
}
if (!strcmp(argv[1], 'start' )) {undefined
如果（线程运行）{未定义
warnx( '已经在运行' );
/* this不是错误 */
返回0；
}
thread_should_exit = false ;
daemon_task = px4_task_spawn_cmd ( '指挥官' ,
SCHED_DEFAULT ,
SCHED_PRIORITY_DEFAULT 40，
3600,
command_thread_main,
( char * const *)&argv[0]);
无符号 constexpr max_wait_us = 1000000;
无符号 constexpr max_wait_steps = 2000;
无符号我；
for (i = 0; i< max_wait_steps; i ) {未定义
睡眠（ max_wait_us /max_wait_steps）；
如果（线程运行）{未定义
休息;
}
}
返回!(i <max_wait_steps);
}
if (!strcmp(argv[1], 'stop' )) {undefined
如果（！thread_running）{未定义
warnx( '指挥官已经停止' );
返回0；
}
thread_should_exit = true ;
而（线程运行）{未定义
睡眠（200000）；
警告（'。'）；
}
warnx( '终止。' );
返回0；
}
/* 下面需要应用程序运行的命令*/
如果（！thread_running）{未定义
warnx( '\ tcommander未启动' );
返回1；
}
if (!strcmp(argv[1], 'status' )) {undefined
打印状态（）；
返回0；
}
if (!strcmp(argv[1], 'calibrate' )) {undefined
if (argc >2) {未定义
int calib_ret=好的；
if (!strcmp(argv[2], ' mag ' )) {undefined
calib_ret =do_mag_calibration(&mavlink_log_pub);
} else if(!strcmp(argv[2], 'accel')) {undefined
calib_ret =do_accel_calibration(&mavlink_log_pub);
} else if(!strcmp(argv[2], 'gyro')) {undefined
calib_ret =do_gyro_calibration(&mavlink_log_pub);
} else if(!strcmp(argv[2], 'level')) {undefined
calib_ret =do_level_calibration(&mavlink_log_pub);
} else if(!strcmp(argv[2], 'esc')) {undefined
calib_ret =do_esc_calibration(&mavlink_log_pub, &armed);
} else if(!strcmp(argv[2], 'airspeed')) {undefined
calib_ret =do_airspeed_calibration(&mavlink_log_pub);
} else {undefined
warnx('argument%s unsupported.', argv[2]);
}
if(calib_ret) {undefined
warnx('calibrationfailed, exiting.');
return 1;
} else {undefined
return 0;
}
} else {undefined
warnx('missingargument');
}
}
if(!strcmp(argv[1], 'check')) {undefined
int checkres =0;
checkres = preflight_check(&status,&mavlink_log_pub, false, true,&status_flags, &battery, true, false,
hrt_elapsed_time(&commander_boot_timestamp));
warnx('Preflightcheck: %s', (checkres == 0) ? 'OK' : 'FAILED');
checkres = preflight_check(&status,&mavlink_log_pub, true, true,&status_flags, &battery, arm_without_gps,
arm_mission_required, hrt_elapsed_time(&commander_boot_timestamp));
warnx('Prearmcheck: %s', (checkres == 0) ? 'OK' : 'FAILED');
return 0;
}
if(!strcmp(argv[1], 'arm')) {undefined
if (TRANSITION_CHANGED !=arm_disarm(true, &mavlink_log_pub, 'commandline')) {undefined
warnx('armingfailed');
}
return 0;
}
if(!strcmp(argv[1], 'disarm')) {undefined
if (TRANSITION_DENIED ==arm_disarm(false, &mavlink_log_pub, 'commandline')) {undefined
warnx('rejecteddisarm');
}
return 0;
}
if(!strcmp(argv[1], 'takeoff')) {undefined
/* see if we got ahome position */
if(status_flags.condition_local_position_valid) {undefined
if (TRANSITION_DENIED !=arm_disarm(true, &mavlink_log_pub, 'commandline')) {undefined
vehicle_command_s cmd = {};
cmd.target_system= status.system_id;
cmd.target_component= status.component_id;
cmd.command =vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
cmd.param1 =NAN; /* minimum pitch */
/* param2-3 unused */
cmd.param2 =NAN;
cmd.param3 =NAN;
cmd.param4 =NAN;
cmd.param5 =NAN;
cmd.param6 =NAN;
cmd.param7 =NAN;
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command),&cmd,
vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h);
} else {undefined
warnx('armingfailed');
}
} else {undefined
warnx('rejectingtakeoff, no position lock yet. Please retry..');
}
return 0;
}
if(!strcmp(argv[1], 'land')) {undefined
vehicle_command_s cmd = {};
cmd.target_system = status.system_id;
cmd.target_component =status.component_id;
cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
/* param 2-3unused */
cmd.param2 = NAN;
cmd.param3 = NAN;
cmd.param4 = NAN;
cmd.param5 = NAN;
cmd.param6 = NAN;
cmd.param7 = NAN;
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command),&cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h);
return 0;
}
if(!strcmp(argv[1], 'transition')) {undefined
vehicle_command_s cmd = {};
cmd.target_system = status.system_id;
cmd.target_component =status.component_id;
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION;
/* transition to theother mode */
cmd.param1 = (status.is_rotary_wing)? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW :
vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
/*参数2-3 未使用 */
命令。参数2 = NAN；
命令。参数3 = NAN；
命令。参数4 = NAN；
命令。参数5 = NAN；
命令。参数6 = NAN；
命令。参数7 = NAN；
orb_advert_th = orb_advertise_queue (ORB_ID(vehicle_command),&cmd, vehicle_command_s ::ORB_QUEUE_LENGTH );
(无效)orb_unadvertise(h);
返回0；
}
if (!strcmp(argv[1], 'mode' )) {undefined
如果（argc > 2）{未定义
uint8_t new_main_state = command_state_s ::MAIN_STATE_MAX；
if (!strcmp(argv[2], 'manual' )) {undefined
new_main_state = command_state_s ::MAIN_STATE_MANUAL ;
} else if (!strcmp(argv[2], ' altctl ' )) {undefined
new_main_state = command_state_s ::MAIN_STATE_ALTCTL ;
} else if (!strcmp(argv[2], ' posctl ' )) {undefined
new_main_state = command_state_s ::MAIN_STATE_POSCTL ;
} else if (!strcmp(argv[2], 'auto:mission' )) {undefined
new_main_state = command_state_s ::MAIN_STATE_AUTO_MISSION ;
} else if (!strcmp(argv[2], 'auto:loiter' )) {undefined
new_main_state = command_state_s ::MAIN_STATE_AUTO_LOITER ;
} else if (!strcmp(argv[2], 'auto:rtl' )) {undefined
new_main_state = command_state_s ::MAIN_STATE_AUTO_RTL ;
} else if (!strcmp(argv[2], ' acro ' )) {undefined
new_main_state= commander_state_s ::MAIN_STATE_ACRO；
} else if (!strcmp(argv[2], ' offboard ' )) {undefined
new_main_state = command_state_s ::MAIN_STATE_OFFBOARD ;
} else if (!strcmp(argv[2], 'stabilized' )) {undefined
new_main_state = command_state_s ::MAIN_STATE_STAB ;
} else if (!strcmp(argv[2], ' rattitude ' )) {undefined
new_main_state = command_state_s ::MAIN_STATE_RATTITUDE；
} else if (!strcmp(argv[2], 'auto:takeoff' )) {undefined
new_main_state = command_state_s ::MAIN_STATE_AUTO_TAKEOFF ;
} else if (!strcmp(argv[2], 'auto:land' )) {undefined
new_main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
} else {undefined
warnx('argument%s unsupported.', argv[2]);
}
if (TRANSITION_DENIED ==main_state_transition(&status, new_main_state, main_state_prev, &status_flags,
&internal_state)) {undefined
warnx('modechange failed');
}
return 0;
} else {undefined
warnx('missingargument');
}
}
if(!strcmp(argv[1], 'lockdown')) {undefined
if (argc < 3) {undefined
usage('not enougharguments, missing [on, off]');
return 1;
}
vehicle_command_s cmd = {};
cmd.target_system = status.system_id;
cmd.target_component = status.component_id;
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;
/* if the comparison matches for off (== 0) set 0.0f, 2.0f (on)else */
cmd.param1 = strcmp(argv[2], 'off') ? 2.0f :0.0f; /* lockdown */
orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command),&cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(h);
return 0;
}
usage('unrecognized command');
return 1;
}
由以上代码可以知道，对于commander模块线程能够接受的参数还是蛮多的。
参数不能少于1个：如：
除了“start”“stop”命令参数外，剩下的参数命令可以跟多个参数命令。好了剩下的就是慢慢啃、来分析函数代码了。
我们进入到主函数：int commander_thread_main(int argc, char *argv[])
该函数是整个指挥官线程完成的大循环函数体部分。在读完函数的过程中，我们比较的“问题3”是比较复杂的。对吧？那就来吧！！！）
3、“相关”数据的“来、去”
这个点需要搞清楚三个问题：相关数据具体指什么数据，数据来自哪里。完事又去了哪里？
“阿木”伙伴们在现场探路让我学习到了一些知识点，非常感谢。但凡凡是csdn上关于本模块的可见说明——发现说的感觉已经很完整了。（很清楚）一层模糊的窗户纸）
因此，还是自己戳了石头望指我们。（能力有限，批评正）
对于这一模块代码是大而杂的，为了明确问题，但不要帖（看一个整就是几大篇，的还是会帖出来的重要说明代码量，比较这里我想的代码）提供思路——我们把复杂的事情搞起来。
在这个模块中——软件对提供数据的“理解范围”还是比较高综合性的。
首先，紧接函数int command_thread_main ( int argc, char_main ( int argc, char_main ( int argc , charv [])“进入函数函数*，首先判断是针对”对重要的“状态-开始”参数、初始化（初始化）：
预报名：
而（！thread_should_exit）
{不明确的
orb_check(param_changed_sub,&updated);
...
orb_check(sp_man_sub,&更新);
...
orb_check(offboard_control_mode_sub,&updated);
...
orb_check(sensor_sub,&updated);
...
orb_check(diff_pres_sub,&更新);
..
orb_check(system_power_sub,&更新);
...
orb_check(safety_sub,&updated);
如果（更新）{未定义
bool previous_safety_off = 安全。安全关闭；
orb_copy（ORB_ID（安全），安全子，&安全）；
/* 解除武装的安全现在已开启且仍处于武装状态 */
if ( status.hil_state == vehicle_status_s ::HIL_STATE_OFF && safety.safety_switch_available && ! safety.safety_off &&
武装。武装）{未定义
arming_state_t new_arming_state = ( status.arming_state == vehicle_status_s ::ARMING_STATE_ARMED ?
车辆状态::ARMING_STATE_STANDBY :车辆状态::ARMING_STATE_STANDBY_ERROR ) ;
if ( TRANSITION_CHANGED == arming_state_transition( &status,
＆电池，
＆安全，
新武装状态，
&武装，
真 /*fRunPreArmChecks */ ,
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
arm_without_gps，
arm_mission_required,
hrt_elapsed_time (&commander_boot_timestamp))){未定义
// mavlink_log_info( &mavlink_log_pub , 'DISARMEDby 安全开关');
arming_state_changed = true ;
}
}
//如果安全开关的状态发生变化，通知用户
if ( safety.safety_switch_available &&previous_safety_off != safety.safety_off ) {undefined
如果（安全。安全关闭）{未定义
set_tune( TONE_NOTIFY_POSITIVE_TUNE );
} else {undefined
tune_neutral(true);
}
status_changed = true;
}
}
orb_check(vtol_vehicle_status_sub,&updated);
...
orb_check(global_position_sub,&updated);
...
orb_check(local_position_sub,&updated);
...
orb_check(attitude_sub,&updated);
...
orb_check(land_detector_sub,&updated);
...
orb_check(cpuload_sub,&updated);
...
orb_check(battery_sub,&updated);
...
orb_check(subsys_sub,&updated);
...
orb_check(pos_sp_triplet_sub,&updated);
/* 在初始化状态下——试着尝试arming_STANDBY状态 */
if(!status_flags.condition_calibration_enabled && status.arming_state== vehicle_status_s::ARMING_STATE_INIT) {undefined
arming_ret = arming_state_transition(&status,
&battery,
&safety,
vehicle_status_s::ARMING_STATE_STANDBY,
&armed,
true /*fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
arm_without_gps,
arm_mission_required,
hrt_elapsed_time(&commander_boot_timestamp));
if(arming_ret == TRANSITION_CHANGED) {undefined
arming_state_changed= true;
} else if (arming_ret== TRANSITION_DENIED) {undefined
/* do notcomplain if not allowed into standby */
arming_ret = TRANSITION_NOT_CHANGED;
}
}
...
orb_check(gps_sub,&updated);
...
orb_check(mission_result_sub,&updated);
...
/* 电子围栏 */
orb_check(geofence_result_sub,&updated);
...
// revertgeofence failsafe transition if sticks are moved and we werepreviously in a manual mode
// but only if notin a low battery handling action
if(rc_override != 0 && !critical_battery_voltage_actions_done &&(warning_action_on &&
(main_state_before_rtl == commander_state_s::MAIN_STATE_MANUAL ||
main_state_before_rtl == commander_state_s::MAIN_STATE_ALTCTL ||
main_state_before_rtl == commander_state_s::MAIN_STATE_POSCTL ||
main_state_before_rtl == commander_state_s::MAIN_STATE_ACRO ||
main_state_before_rtl == commander_state_s::MAIN_STATE_RATTITUDE ||
main_state_before_rtl == commander_state_s::MAIN_STATE_STAB))) {undefined
// transition toprevious state if sticks are touched
if((_last_sp_man.timestamp != sp_man.timestamp) &&
((fabsf(sp_man.x -_last_sp_man.x) > min_stick_change) ||
(fabsf(sp_man.y - _last_sp_man.y) >min_stick_change) ||
(fabsf(sp_man.z - _last_sp_man.z) > min_stick_change)||
(fabsf(sp_man.r - _last_sp_man.r) >min_stick_change))) {undefined
// revert to position control in anycase
main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL,main_state_prev,
&status_flags, &internal_state);
mavlink_log_critical(&mavlink_log_pub,'Autopilotoff, returned control to pilot');
}
}
// abort landing orauto or loiter if sticks are moved significantly
// but only if notin a low battery handling action
if(rc_override != 0 && !critical_battery_voltage_actions_done &&
(internal_state.main_state== commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state.main_state== commander_state_s::MAIN_STATE_AUTO_MISSION ||
internal_state.main_state== commander_state_s::MAIN_STATE_AUTO_LOITER)) {undefined
// transition toprevious state if sticks are touched
if((_last_sp_man.timestamp != sp_man.timestamp) &&
((fabsf(sp_man.x -_last_sp_man.x) > min_stick_change) ||
(fabsf(sp_man.y - _last_sp_man.y) > min_stick_change)||
(fabsf(sp_man.z - _last_sp_man.z) >min_stick_change) ||
(fabsf(sp_man.r - _last_sp_man.r) >min_stick_change))) {undefined
// revert toposition control in any case
main_state_transition(&status, commander_state_s::MAIN_STATE_POSCTL,main_state_prev,
&status_flags,&internal_state);
mavlink_log_critical(&mavlink_log_pub,'Autopilotoff, returned control to pilot');
}
}
/* Check for missionflight termination */
if (armed.armed&& _mission_result.flight_termination &&
!status_flags.circuit_breaker_flight_termination_disabled) {undefined
armed.force_failsafe = true;
status_changed = true;
static boolflight_termination_printed = false;
if(!flight_termination_printed) {undefined
mavlink_log_critical(&mavlink_log_pub,'Geofenceviolation: flight termination');
flight_termination_printed= true;
}
if (counter %(1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {undefined
mavlink_log_critical(&mavlink_log_pub,'Flighttermination active');
}
}
/* Only evaluatemission state if home is set,
* this prevents false positives for themission
* rejection. Back off 2 seconds to not overlay
* home tune.
*/
if(status_flags.condition_home_position_valid &&
(hrt_elapsed_time(&_home.timestamp)> 2000000) &&
_last_mission_instance !=_mission_result.instance_count) {undefined
if(!_mission_result.valid) {undefined
/* the mission isinvalid */
tune_mission_fail(true);
warnx('missionfail');
} else if (_mission_result.warning){undefined
/* the mission has awarning */
tune_mission_fail(true);
warnx('missionwarning');
} else {undefined
/* the mission isvalid */
tune_mission_ok(true);
}
/* prevent furtherfeedback until the mission changes */
_last_mission_instance =_mission_result.instance_count;
}
/* RC input check */
if(!status_flags.rc_input_blocked && sp_man.timestamp != 0 &&
(hrt_absolute_time() < sp_man.timestamp (uint64_t)(rc_loss_timeout* 1e6f))) {undefined
/* handle the casewhere RC signal was regained */
if(!status_flags.rc_signal_found_once) {undefined
status_flags.rc_signal_found_once = true;
status_changed = true;
} else {undefined
if (status.rc_signal_lost){undefined
mavlink_log_info(&mavlink_log_pub,'(xiwei)MANUAL control regained after %llums',
(hrt_absolute_time() -rc_signal_lost_timestamp) / 1000);
status_changed = true;
}
}
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) ) {undefined
if ((stick_on_counter == rc_arm_hyst &&stick_off_counter < rc_arm_hyst) || arm_switch_to_arm_transition) {undefined
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) ) {undefined
print_reject_arm('NOT ARMING: Switch need a manual modefirst.');
} else if (!status_flags.condition_home_position_valid &&geofence_action == geofence_result_s::GF_ACTION_RTL) { print_reject_arm('NOTARMING: Geofence RTL requires valid home');
} else if (status.arming_state== vehicle_status_s::ARMING_STATE_STANDBY) {undefined
arming_ret = arming_state_transition( &status,
&battery,
&safety,
vehicle_status_s::ARMING_STATE_ARMED,
&armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub,
&status_flags,
avionics_power_rail_voltage,
arm_without_gps,
arm_mission_required,
hrt_elapsed_time(&commander_boot_timestamp)); if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
} else {undefined
usleep(100000);
print_reject_arm('NOT ARMING: Preflight checksfailed');
}
}
}
stick_on_counter ;
} else if(!(arm_switch_is_button == 1 &&
sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {undefined
stick_on_counter= 0;
}
_last_sp_man_arm_switch = sp_man.arm_switch;
/* evaluate the main state machine according to mode switches */
bool first_rc_eval = (_last_sp_man.timestamp== 0) && (sp_man.timestamp > 0);
/* 遥控模式输入 */
transition_result_t main_res = set_main_state_rc(&status);
...
orb_check(cmd_sub, &updated);
...
/* checkif we are disarmed and there is a better mode to wait in */
if (!armed.armed){undefined
/* if there is no radio control butGPS lock the user might want to fly using
* just a tablet. Since the RC will force itsmode 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== command_state_s :: MAIN_STATE_MANUAL )
&&status_flags。condition_home_position_valid ) {未定义
( void ) main_state_transition (&status, command_state_s ::MAIN_STATE_AUTO_LOITER ,
main_state_prev, &status_flags, &internal_state);
}
}
/* 最后处理命令，因为系统需要更新才能处理它们 */
orb_check(cmd_sub, &updated);
如果（更新）{未定义
/* 得到命令 */
orb_copy(ORB_ID(vehicle_command),cmd_sub,&cmd);
/* 处理它 */
if (handle_command(&status, &safety, &cmd, &armed, &_home,&global_position, &local_position,
&attitude,&home_pub, &command_ack_pub, &command_ack, &_roi,&roi_pub)) {undefined
status_changed = true ;
}
}
...
/* 现在根据故障安全和主状态设置导航状态 */
bool nav_state_changed = set_nav_state (&status, &armed, &internal_state, &mavlink_log_pub, (link_loss_actions_t)datalink_loss_act, _mission_result.finished , _mission_result.stay_in_failsafe，&status_flags，land_detector。着陆，（link_loss_actions_t）rc_loss_act，offboard_loss_act，offboard_loss_rc_act）；
...
如果（main_state_changed || nav_state_changed）{未定义
status_changed = true ;
main_state_changed =假；
}
/*命令循环在过程中的20倍数，但我们知道命令循环的周期，以及_控制_模式的周期周期也不是的对于ms的运行速度，至少5HZ是可以保证循环的这用遥控模式或者地面站数据对模式切换是完全够用的*/
if (counter %(200000 / COMMANDER_MONITORING_INTERVAL（default:1000）) == 0 || status_changed) {undefined
设置控制模式（）；
控制模式。时间戳=现在；
orb_publish( ORB_ID(vehicle_control_mode),control_mode_pub, &control_mode);
地位。时间戳=现在；
orb_publish( ORB_ID(vehicle_status),status_pub, &status); //包含了vtol 、rc 、DLink 、arm状态、及飞行模式，
武装。时间戳=现在；
/*如果安全关闭，或者安全不存在且经过 5 秒，则设置预置状态 */
如果（安全。安全开关可用）{未定义
/* 安全关闭，进入预装状态*/
武装。预先武装=安全。安全关闭；
}其他{未定义
/* 安全不存在，进入预装状态
* (all output drivers should bestarted / unlocked last in the boot process
* when the rest of the system is fullyinitialized)
*/
armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp)> 5 * 1000 * 1000);
}
orb_publish(ORB_ID(actuator_armed),armed_pub, &armed); // 发布飞机 fmu 、及io的加锁解锁状态
}
...
counter ;
/* publishinternal state for logging purposes */
if(commander_state_pub != nullptr) {undefined
orb_publish(ORB_ID(commander_state), commander_state_pub,&internal_state);
} else {undefined
command_state_pub = orb_advertise(ORB_ID(commander_state),&internal_state);
}
usleep(COMMANDER_MONITORING_INTERVAL （默认：10000） );
}
ret = pthread_join (commander_low_prio_thread, nullptr );
if (ret) {未定义
warn( '加入失败: %d' , ret);
}
rgbled_set_color_and_mode ( led_control_s ::COLOR_WHITE , led_control_s ::MODE_OFF );
...
通过这个看似“很重要的一些职能”，我们可以抓住“密码”，知道这件事情——我们的毛看起来很像（这看起来是子的光彩，看起来很复杂？）是关键）因此，我们来一点逻辑。
一、职能基本作用
| 函数 | 作用 | 备注 | | arming_state_transition | 解锁、鉴定功能 | 注意：3个基本状态（模式） | | set_main_state_rc | RC_input切换模式、鉴定 | 简单 | | main_state_transition | 模式切换设置 | 简单 | | 设置导航状态 | 导航模式设置 | 简单 | | 设置控制模式（） | 模式设置 | 逻辑鉴定结论 |
待机（初始化）
ARMING_ERROR（发生并处理）
|
|
StandBy（真的&&没有错误）
ARMING（新的布防状态）
|
|
武装（现在）
|
|
撤防、初始化/预检
（回去）
|
|
基本信息量
安全_sw
传感器_校准
系统电源
差异压力
|
|
罗列的其他代码代码是为了说明清楚，这是为了替这个真正的角色来处理这个任务。花了时间的，不管啦，老话：在想改变一个规则之前，那么还是先组织它，打破重新制定。
这个基本思路，我们在往里看看（ARMING）：
transition_result_t arming_state_transition ( struct vehicle_status_s *status, //飞机状态数据集
struct battery_status_battery , // 电池状态 *s
const struct safety_s *safety, //
arming_state_t new_arming_state, // 新状态（期望值）
struct actuator_armed_s *armed, // 执行机构（如电机推力）
bool fRunPreArmChecks, // 是否进行抑制pre-fligt-fligt
orb_advert_t *mavlink_log_pub, ///< mavlink日志的 uORB 句柄
status_flags_s *status_flags,
float avionics_power_rail_voltage, //航电电压等检测
bool arm 是否允许下 GPS, // 是否允许
bool arm_mission_required, // 任务任务需求bool量标识
hrt_abstime time_since_boot) //绝对运行状态值
{不明确的
前期：
new_arming_state== current_arming_state // 状态相同。返回
fRunPreArmChecks // 是否需要转换前的状态检查bool值
| Yes | 1、try to disarm if ARMED or to STANDBY_ERROR if ARMED_ERROR 2、manual try to arming | | no | 1、Go back INIT/PREFLIGTH 2、Standby |
//需要pre_flight check的情形：sensor failing 切非 HIL（仿真）状态
如果需要preflight_check (){ 则fRunPreArmChecks==1
Commander::preflightCheck (mavlink_log_pub,true, true, true, true,checkAirspeed,
(status->rc_input_mode== vehicle_status_s::RC_IN_MODE_DEFAULT),!arm_without_gps,
true,status->is_vtol, reportFailures, prearm, time_since_boot);
其实就是包含了{ bool preflightCheck(orb_advert_t*mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro,
bool checkBaro,boolcheckAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool isVTOL,
bool reportFailures, bool prearm, hrt_abstimetime_since_boot) }
...
}
第二步：
新状态值（期望） 老状态值（current arming state）
bool valid_transition =arming_transitions[new_arming_state][status->arming_state];
到这里涉及一个arm_trans_table的东西，其实，模式之间能不能转换，都是已经定义好的，（因为每个模式条件下，涉及的传感器，状态值有所不同，因此需要有所了解）
于是乎我们跳转到state_machine_helper.cpp下的 arming_transition[new][cur]
{
| ROW==NEW\ COL==CURR | INIT | STANDBY | ARMED | ARMED_ERROR | STANDBY_ERROR | REBOOT | IN_AIR_RESTORE | | ARMING_STATE_INIT | T(1) | T(1) | F(0) | F(0) | T(1) | F(0) | F(0) | | ARMING_STATE_STANDBY | T(1) | T(1) | T(1) | T(1) | F(0) | F(0) | F(0) | | ARMING_STATE_ARMED | F(0) | T(1) | T(1) | F(0) | F(0) | F(0) | T(1) | | ARMING_STATE_ARMED_ERROR | F(0) | F(0) | T(1) | T(1) | F(0) | F(0) | F(0) | | ARMING_STATE_STANDBY_ERROR | T(1) | T(1) | T(1) | T(1) | T(1) | F(0) | F(0) | | ARMING_STATE_REBOOT | T(1) | T(1) | F(0) | F(0) | T(1) | T(1) | T(1) | | ARMING_STATE_IN_AIR_RESTORE | F(0) | F(0) | F(0) | F(0) | F(0) | F(0) | F(0) |
值得注意的是：这里虽然定义了转换允许的 Y/ N 但是有些在 True的情况下 附件条件的存在，也会使得转换不成功：
这也就是所谓的 second validation（二次确认）
a、在（row==new）ARMING_STATE_ARMED && （curr != IN_AIR_RESTORE）“栏中”尽管转换允许，但如果在pre-arm（出错）时，同
样最终转换为失败。再入，安全开关使能但是没按情况下，依旧转换失败。
b、stand_by本来就出错了（即就是：自检出错，try arming时候允许转换，但是“二次判断”后再次否决转换）
c、给出最后判断
}
if (valid_transition) {undefined
status->arming_state= new_arming_state;
mavlink（“输出信息”）；
}
第三部： 跟进结论——如果需要：则重置转换反馈信息；
否则：告诉地面站我转换成 后的 目标
}
做完这些总得有个结果：得到这些结果的下一步就是（特殊情况特殊对待的选项）：跟据home_position以及可能由于mission特殊情况下，要对某些情况做特殊对待。
比如： 1、在有mission的情况下，转换到mission前是pos_ctrl，刚起飞就碰到misssion_finished状况了，则要直接设定值。
2、DL、RcGPS 信号灯lost 要立马进行飞行状态的“特殊”处理了。
3、例外，（例外的例外——就是目标了）当然cmd(RC输入命令的进入，正常切换了)则相应的处理时根本“初衷”。
因此，需要引出另一个函数：main_state_transition进行模式设置
transition_result_tmain_state_transition(struct vehicle_status_s *status, main_state_tnew_main_state, uint8_t &main_state_prev,
status_flags_s*status_flags, struct commander_state_s*internal_state)
{undefined
switch (new_main_state)
{undefined
case xxx:
case xxx:
}
}
参数：
| Satuts： | 导航状态，飞行器应该进入的状态 | | | new_main_state： | 期望切换到的新状态 | | | main_state_prev： | 之前的状态 | | | status_flags： | commander内部的状态标志 | | | status_flags： | 主状态，用户想要的状态。由遥控器设置，也可以由地面站通过数传控制 | |
返回值：
| | | | | ret = TRANSITION_DENIED; | // 表示不满足切换条件，拒绝状态切换 | | | ret = TRANSITION_CHANGED; | // 切换到某个状态，不一定是用户想要的目标状态，会根据降级策略，切换至一个最接近的状 | | | ret = TRANSITION_NOT_CHANGED; | | |
解锁成功了、转换逻辑通过确认（这里应该这样称呼才对）了，——>就是该去：模式设置成功了，
bool handle_command( struct vehicle_status_s*status_local,
const struct safety_s*safety_local,
struct vehicle_command_s *cmd, //飞行器操作cmd
struct actuator_armed_s*armed_local,
struct home_position_s *home,
struct vehicle_global_position_s*global_pos,
struct vehicle_local_position_s*local_pos,
结构 vehicle_attitude_s *attitude, orb_advert_t *home_pub,
orb_advert_t *command_ack_pub,
结构 vehicle_command_ack_s *command_ack,
struct vehicle_roi_s *roi, orb_advert_t * roi_pub ) //飞行器其他的区域（或位置）相关
{不明确的
...
无符号cmd_result= vehicle_command_s ::VEHICLE_CMD_RESULT_UNSUPPORTED；
/* 请求设置不同的系统模式 */
开关（cmd->命令）{未定义
case vehicle_command_s ::VEHICLE_CMD_DO_REPOSITION : {undefined
// 这里只切换飞行模式，导航器负责
// 对坐标做一些有意义的事情。它的设计
// 不需要导航器和命令来接收/处理
// 同一时间的数据。
// 检查是否请求了模式切换
if (((( uint32_t )cmd-> param2 )& 1) > 0)
{不明确的
transition_result_t main_ret = main_state_transition（status_local，commander_state_s ::MAIN_STATE_AUTO_LOITER，
main_state_prev, &status_flags,&internal_state);
如果（（main_ret！= TRANSITION_DENIED））{未定义
cmd_result = vehicle_command_s ::VEHICLE_CMD_RESULT_ACCEPTED ;
}其他{未定义
cmd_result = vehicle_command_s ::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED ;
mavlink_log_critical(&mavlink_log_pub, '拒绝重新定位命令' );
}
}其他{未定义
cmd_result = vehicle_command_s ::VEHICLE_CMD_RESULT_ACCEPTED ;
}
}
休息;
case vehicle_command_s ::VEHICLE_CMD_DO_SET_MODE
休息;
...
case vehicle_command_s ::VEHICLE_CMD_DO_SET_MODE
休息;
...
answer_command(*cmd,cmd_result, *command_ack_pub, *command_ack);
}
模式确定最后总算在这里肯定了下来（这么称呼比较好说）。
到这里，我们做好了大量工作:
① Arming(允许了)——>
② 模式转换判断（Ture）——>
③ 根据RC、Mavlink（vehile_CMD）——>
④ Mian_state（最后确定模式）并回答给发出CMD的控制的主体——>
⑤ 根据Mode Cmd/failsafe设定Set_nav_sate（）——>因此我们在进入函数
bool set_nav_state(struct vehicle_status_s *status, // 导航状态数据集，飞行器应该进入的状态
struct actuator_armed_s *armed,
struct commander_state_s*internal_state, //模式CMD值，用户主要状态值
orb_advert_t *mavlink_log_pub,
const link_loss_actions_t data_link_loss_act, //地面站数据链丢失
const bool mission_finished,
const bool stay_in_failsafe, //故障保护数据
status_flags_s *status_flags, //指挥官内部状态标志
布尔降落，
常量link_loss_actions_t rc_loss_act,
const int offb_loss_act,
const int offb_loss_rc_act)
{不明确的
//记录old_参数
navigation_state_t nav_state_old = 状态-> nav_state ;
...
开关(internal_state-> main_state )
{不明确的
case command_state_s ::MAIN_STATE_ACRO：
case command_state_s ::MAIN_STATE_MANUAL：
case command_state_s :: MAIN_STATE_RATTITUDE：
case command_state_s ::MAIN_STATE_STAB：
case command_state_s ::MAIN_STATE_ALTCTL：
/* 所有手动模式都需要 RC */
如果（rc_lost&& is_armed）{未定义
enable_failsafe（状态，old_failsafe，mavlink_log_pub，reason_no_rc）；
set_rc_loss_nav_state（状态，武装，状态标志，rc_loss_act）；
}其他{未定义
开关（internal_state-> main_state）{未定义
case command_state_s ::MAIN_STATE_ACRO：
状态- > nav_state = vehicle_status_s ::NAVIGATION_STATE_ACRO ;
休息;
case command_state_s ::MAIN_STATE_MANUAL：
状态- > nav_state = vehicle_status_s ::NAVIGATION_STATE_MANUAL ;
休息;
case command_state_s ::MAIN_STATE_RATTITUDE：
状态- > nav_state = vehicle_status_s ::NAVIGATION_STATE_RATTITUDE；
休息;
案例 command_state_s ::MAIN_STATE_STAB：
状态-> nav_state = vehicle_status_s ::NAVIGATION_STATE_STAB；
休息；
case command_state_s ::MAIN_STATE_ALTCTL：
状态-> nav_state = vehicle_status_s ::NAVIGATION_STATE_ALTCTL；
休息；
默认：
- > nav_state = vehicle_status_s ::NAVIGATION_STATE_MANUAL ;
休息；
}
}
休息；
...
}
…
//以下如法炮制
…
…
返回状态- > nav_state != nav_state_old; //设置成功逻辑为1
}
返回值：
一切鉴定，做完最后是设置了：那是函数：
⑥ set_control_mode ()；
函数简单简单的模式设置了这个模式，这就是正常的过程，因为在这个过程中，为了加速，XYZ的过程中加速，直到VTOL的加速，直到在中间的过程中加速，转换的有误（加速\角速度控制）有类似的最后设定，即其设定control_mode.flag_xxx值。
，到这个地方原定草图的细度和路子走完了，还有很多但是代码细节还是分析的。
剩下的就是及时的各种发布并列的消息给相应的模块提供指示和参考了。
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
orb_publish（ORB_ID（commander_state），commander_state_pub，&internal_state）；
。。。
等等，最后就是，
rgbled_set_color_and_mode ( led_control_s ::COLOR_WHITE, led_control_s ::MODE_OFF);
它——>orb_publish(ORB_ID(led_control),led_control_pub, &led_control);给led驱动模块参考用。
基本将统帅的顺序理解完了，还有一件大事，耶，对了，突然想起来了，哥们我们在干活嘞，是Low_prio_thread了，你到底是干子的嘞。怎么看都不能休息哈，那我们继续干完完再休息么。那还是继续……
void * command_low_prio_loop ( void *arg) //指针函数提供posix跨平台api——创建线程入口
{ //这个处理的任务名称为：commander_low_prio
订阅了一个消息 // ORB_ID(vehicle_command)
…
}
可以看到在这个函数中加载了high_prio command 执行了低优先级任务：包含：
案例 vehicle_command_s ::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN：
案例 vehicle_command_s ::VEHICLE_CMD_PREFLIGHT_CALIBRATION：
case vehicle_command_s ::VEHICLE_CMD_START_RX_PAIR :
case vehicle_command_s:: VEHICLE_CMD_PREFLIGHT_STORAGE:
低优先级函数到底怎么被调用执行了呢？
很高兴的告诉你：地面站mavlink消息很开心承担这个函数的“使用权”，为了测试这个函数的调用我加了一点点mavlink
测添加mavlink通话消息：
地面站进行水平施工操作：
直到今天，经典走完流程了，剩下的就是细节，。
告诉大家一个自动化过程，但是对于自动化过程中需要很多细节，比如我们的问题，就是问题，例如，问题，问题，问题4麻烦了），最后飞起来了几下，或者炸掉了（又不发生了），重新链接地面站——发现地面站上，耶，NND不给大爷了（或者不准确了，有木有发现鸡） ）这就是个基本问题点之一啊，大神们，慢慢来做自己的工作吧。
因此，我相信，表达在这里，读过代码的——明眼人都知道了。谢谢，不厌其烦的展开了本文的学习记录。水平，漏水，望批评指正。
我是一只奔跑中的蜗牛，再次感谢伙伴，奋斗在同行中的我们读到这里；
|
