首先，感谢“阿木社区”小伙伴们在PX4方面做出的贡献。在学习px4的过程中，我也是个小学生，以下作为个人的小心得，纰漏蛮多，还望各位同仁包涵、期待批评指正。但我们的目的只有一个——在想改变一个规则之前，先学会它，打破再立（不破不立）
闲话少絮，书归正文。
Commander文件夹是px4源码中的飞行控制命令切换模块（比如由Stab_Mode，Alti_Mode，Pos_Mode，Auto_Mode在 相互切换的过程）其次，所有模式能不能成功切换，都在这个模块中做了相应的飞行条件检查。
自稳模式的切换，不需要太多的飞行条件。如果是定高，定点，自动模式，需要很多的传感器数据有效的条件，这就需要很多的检查，只有条件满足才能够成功切换。
须知点：
模式切换就是MODE_SW （地面站选择模式切换开关，通常我们的遥控器只有3个档，因此需要3个以上的模式切换，则需要“改造”遥控器了——改装个多档位切换开关）：但值得注意的是——Mode_Switch开关除了模式通道定义的模式外，还包含独立模式通道，比如空中停转（brake，return、等等）。
如此种种都是在Commander内完成的内容，最终能不能成功的切换就看各种模式需要的传感器满足条件，和切换间隙、出错问题的判断了。
一 模式基本分类：阿木小伙伴先探过路 + 自己补充一点点
在整个过程中我们需要有明确的思路:这样有助于我们对commander模块的理解。为了能说明白直观理解能上图的地方我尽量少说话(为了截图方便 那就在windows下登着QQ截图，随心所欲啊，包涵了伙伴们)，来点小伙伴们没有人提到过的东西。
1、对于Commander函数文件夹内文件之间关系；
这些部分暂时放在这里稍后我会伟伟道来（haha）
2、函数整个基本层次结构。
首先，需要明确：commander.cpp、commander_params.c是关于整个commander模块线程（暂这样叫、道线程是活跃的哦，当然这个需要基于操作系统）的任务调度的入口文件：
int commander_main(int argc, char *argv[])
这函数的“入口和跟进方”法大家都很熟悉了就不累赘了、不过我还是把入口文件附在下面，便于阅读。
int commander_main(int argc, char*argv[])
{
if (argc <2) {
usage("missingcommand");
return 1;
}
if(!strcmp(argv[1], "start")) {
if(thread_running) {
warnx("alreadyrunning");
/* this is not anerror */
return 0;
}
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 40,
3600,
commander_thread_main,
(char * const*)&argv[0]);
unsigned constexprmax_wait_us = 1000000;
unsigned constexprmax_wait_steps = 2000;
unsigned i;
for (i = 0; i< max_wait_steps; i++) {
usleep(max_wait_us /max_wait_steps);
if(thread_running) {
break;
}
}
return !(i <max_wait_steps);
}
if(!strcmp(argv[1], "stop")) {
if(!thread_running) {
warnx("commanderalready stopped");
return 0;
}
thread_should_exit = true;
while(thread_running) {
usleep(200000);
warnx(".");
}
warnx("terminated.");
return 0;
}
/* commands needing the app torun below */
if(!thread_running) {
warnx("\tcommandernot started");
return 1;
}
if(!strcmp(argv[1], "status")) {
print_status();
return 0;
}
if(!strcmp(argv[1], "calibrate")) {
if (argc >2) {
int calib_ret= OK;
if(!strcmp(argv[2], "mag")) {
calib_ret =do_mag_calibration(&mavlink_log_pub);
} else if(!strcmp(argv[2], "accel")) {
calib_ret =do_accel_calibration(&mavlink_log_pub);
} else if(!strcmp(argv[2], "gyro")) {
calib_ret =do_gyro_calibration(&mavlink_log_pub);
} else if(!strcmp(argv[2], "level")) {
calib_ret =do_level_calibration(&mavlink_log_pub);
} else if(!strcmp(argv[2], "esc")) {
calib_ret =do_esc_calibration(&mavlink_log_pub, &armed);
} else if(!strcmp(argv[2], "airspeed")) {
calib_ret =do_airspeed_calibration(&mavlink_log_pub);
} else {
warnx("argument%s unsupported.", argv[2]);
}
if(calib_ret) {
warnx("calibrationfailed, exiting.");
return 1;
} else {
return 0;
}
} else {
warnx("missingargument");
}
}
if(!strcmp(argv[1], "check")) {
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")) {
if (TRANSITION_CHANGED !=arm_disarm(true, &mavlink_log_pub, "commandline")) {
warnx("armingfailed");
}
return 0;
}
if(!strcmp(argv[1], "disarm")) {
if (TRANSITION_DENIED ==arm_disarm(false, &mavlink_log_pub, "commandline")) {
warnx("rejecteddisarm");
}
return 0;
}
if(!strcmp(argv[1], "takeoff")) {
/* see if we got ahome position */
if(status_flags.condition_local_position_valid) {
if (TRANSITION_DENIED !=arm_disarm(true, &mavlink_log_pub, "commandline")) {
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 {
warnx("armingfailed");
}
} else {
warnx("rejectingtakeoff, no position lock yet. Please retry..");
}
return 0;
}
if(!strcmp(argv[1], "land")) {
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")) {
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;
/* 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], "mode")) {
if (argc > 2) {
uint8_t new_main_state = commander_state_s::MAIN_STATE_MAX;
if (!strcmp(argv[2], "manual")) {
new_main_state = commander_state_s::MAIN_STATE_MANUAL;
} else if (!strcmp(argv[2], "altctl")) {
new_main_state = commander_state_s::MAIN_STATE_ALTCTL;
} else if (!strcmp(argv[2], "posctl")) {
new_main_state = commander_state_s::MAIN_STATE_POSCTL;
} else if (!strcmp(argv[2], "auto:mission")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
} else if (!strcmp(argv[2], "auto:loiter")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_LOITER;
} else if (!strcmp(argv[2], "auto:rtl")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
} else if (!strcmp(argv[2], "acro")) {
new_main_state= commander_state_s::MAIN_STATE_ACRO;
} else if (!strcmp(argv[2], "offboard")) {
new_main_state = commander_state_s::MAIN_STATE_OFFBOARD;
} else if (!strcmp(argv[2], "stabilized")) {
new_main_state = commander_state_s::MAIN_STATE_STAB;
} else if (!strcmp(argv[2], "rattitude")) {
new_main_state = commander_state_s::MAIN_STATE_RATTITUDE;
} else if (!strcmp(argv[2], "auto:takeoff")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF;
} else if (!strcmp(argv[2], "auto:land")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
} else {
warnx("argument%s unsupported.", argv[2]);
}
if (TRANSITION_DENIED ==main_state_transition(&status, new_main_state, main_state_prev, &status_flags,
&internal_state)) {
warnx("modechange failed");
}
return 0;
} else {
warnx("missingargument");
}
}
if(!strcmp(argv[1], "lockdown")) {
if (argc < 3) {
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[])
该函数是整个commander模块线程完成的大loop函数体部分。在读完改函数的过程中，我们比较感兴趣的“问题点3”是比较复杂的。（但是也得啃，啃清楚就明白啦，对吧？那就来吧！！！）
3、“相关”数据的“来、去”
这个问题点需要搞懂三个问题：相关数据具体指什么数据，数据来自哪里。完事又去了哪里？
“阿木”伙伴们前期探路让我学习到了一些知识点，对此表示十分感谢。但凡是看过csdn上关于本模块的说明内容后——发现说的不够完全清楚。(读过感觉依旧有一层模糊的窗户纸)
因此，我们还是自己捅了捅窗户。（能力有限，望批评指正）
对于这一模块代码量是比较大而杂的，为了能说明清楚问题，尽量不帖代码（看代码一整就是几大篇，但是重要的代码还是会帖出来一起说说），这里我尽量的提供思路——我们把复杂的事情搞简单。
在这个模块中——软件对提供的数据的“理解范围”还是比较高综合性的。
首先，紧接进入函数int commander_thread_main(int argc, char *argv[])在一进入函数，首先是针对“函数参数”对重要“状态-环节”判断、初始化（标志初始化）：
大致骨架梳理：
while(!thread_should_exit)
{
orb_check(param_changed_sub,&updated);
...
orb_check(sp_man_sub,&updated);
...
orb_check(offboard_control_mode_sub,&updated);
...
orb_check(sensor_sub,&updated);
...
orb_check(diff_pres_sub,&updated);
..
orb_check(system_power_sub,&updated);
...
orb_check(safety_sub,&updated);
if (updated){
bool previous_safety_off = safety.safety_off;
orb_copy(ORB_ID(safety), safety_sub, &safety);
/* disarmif safety is now on and still armed */
if (status.hil_state == vehicle_status_s::HIL_STATE_OFF &&safety.safety_switch_available && !safety.safety_off&&
armed.armed) {
arming_state_t new_arming_state = (status.arming_state== vehicle_status_s::ARMING_STATE_ARMED ?
vehicle_status_s::ARMING_STATE_STANDBY : vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status,
&battery,
&safety,
new_arming_state,
&armed,
true /*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 safety switch");
arming_state_changed = true;
}
}
//Notify the user ifthe 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;
}
}
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) {
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) {
arming_state_changed= true;
} else if (arming_ret== TRANSITION_DENIED) {
/* 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))) {
// 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))) {
// 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)) {
// 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))) {
// 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) {
armed.force_failsafe = true;
status_changed = true;
static boolflight_termination_printed = false;
if(!flight_termination_printed) {
mavlink_log_critical(&mavlink_log_pub,"Geofenceviolation: flight termination");
flight_termination_printed= true;
}
if (counter %(1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
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) {
if(!_mission_result.valid) {
/* the mission isinvalid */
tune_mission_fail(true);
warnx("missionfail");
} else if (_mission_result.warning){
/* the mission has awarning */
tune_mission_fail(true);
warnx("missionwarning");
} else {
/* 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))) {
/* handle the casewhere RC signal was regained */
if(!status_flags.rc_signal_found_once) {
status_flags.rc_signal_found_once = true;
status_changed = true;
} else {
if (status.rc_signal_lost){
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) ) {
if ((stick_on_counter == rc_arm_hyst &&stick_off_counter < rc_arm_hyst) || arm_switch_to_arm_transition) {
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 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) {
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 {
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)) {
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){
/* 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 == commander_state_s::MAIN_STATE_MANUAL)
&&status_flags.condition_home_position_valid) {
(void) main_state_transition (&status,commander_state_s::MAIN_STATE_AUTO_LOITER,
main_state_prev, &status_flags, &internal_state);
}
}
/* handle commands last, as thesystem needs to be updated to handle them */
orb_check(cmd_sub, &updated);
if (updated){
/* got command */
orb_copy(ORB_ID(vehicle_command),cmd_sub, &cmd);
/* handle it */
if(handle_command(&status, &safety, &cmd, &armed, &_home,&global_position, &local_position,
&attitude,&home_pub, &command_ack_pub, &command_ack, &_roi,&roi_pub)) {
status_changed = true;
}
}
...
/* now set navigation state according to failsafe andmain state */
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.landed, (link_loss_actions_t)rc_loss_act, offboard_loss_act, offboard_loss_rc_act);
...
if(main_state_changed || nav_state_changed) {
status_changed = true;
main_state_changed = false;
}
/* 我们知道commander不断循环，但是在循环的过程中为20的倍数，因而commandloop的周期，以及set_control_mode函数调用周期也是不固定的但是对于ms运行速度来说，速度至少是5HZ是可以保证的，这使得遥控或者地面站数据对外部模式切换是完全够用的 */
if (counter %(200000 / COMMANDER_MONITORING_INTERVAL（default:1000）) == 0 || status_changed) {
set_control_mode();
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); // 包含了vtol判断、rc、DLink、arm状态、及飞行模式，
armed.timestamp = now;
/* set prearmedstate if safety is off, or safety is not present and 5 seconds passed */
if (safety.safety_switch_available){
/* safetyis off, go into prearmed */
armed.prearmed = safety.safety_off;
} else {
/* safety is notpresent, go into prearmed
* (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) {
orb_publish(ORB_ID(commander_state), commander_state_pub,&internal_state);
} else {
commander_state_pub = orb_advertise(ORB_ID(commander_state),&internal_state);
}
usleep(COMMANDER_MONITORING_INTERVAL（default: 10000）);
}
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);
...
通过上面“凸显的函数”名我们可以抓住主要“code骨架”，光知道函数骨架——那我们还是太单纯了（函数本身并不复杂，为毛是这样子的骨架么？）（这才是关键）因此，我们来一点点逻辑。
1、函数基本作用
| 函数 | 作用 | 备注 |
| arming_state_transition | 解锁判断、执行函数 | 注意：3 basic state(mode) |
| set_main_state_rc | RC_input模式切换、判断 | Easy |
| main_state_transition | Mode切换设置 | Easy |
| set_nav_state | 导航模式设置 | Easy |
| set_control_mode() | 模式设置 | 逻辑判断 + 结论 |
|
|
| |||
|
|
上面罗列了那么多代码，就是为了说清楚这个基本逻辑。源代码中多处用到了函数arming_state_transition——都是按照这个思路来做事的。所以代码还是要看的，毕竟，px4的这帮人还是花了时间的，不管啦，还是老话：在想改变一个规则之前，那就先学会它，打破再制定。
有了这个基本思路，我们在往里看看( ARMING )：
transition_result_t arming_state_transition( struct vehicle_status_s *status, //飞机状态数据集
struct battery_status_s *battery, // 电量
const struct safety_s *safety, //
arming_state_tnew_arming_state, // 新状态（期望值）
struct actuator_armed_s *armed, // 执行机构（如 电机推力）
boolfRunPreArmChecks, // 是否进行前期pre-fligt-check
orb_advert_t*mavlink_log_pub, ///<uORB handle for mavlink log
status_flags_s*status_flags,
floatavionics_power_rail_voltage, //航电 电压等检测
boolarm_without_gps, // 是否允许no GPS 下解锁
boolarm_mission_required, // mission任务需求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) {
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)
{
switch (new_main_state)
{
case xxx:
case xxx:
}
}
参数：
| 导航状态，飞行器应该进入的状态 |
| |
| 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,
struct vehicle_attitude_s *attitude,orb_advert_t *home_pub,
orb_advert_t*command_ack_pub,
struct vehicle_command_ack_s*command_ack,
struct vehicle_roi_s *roi, orb_advert_t *roi_pub) //飞行器感兴趣的区域（or位置）misssion 有关
{
...
unsigned cmd_result= vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
/* request to set different systemmode */
switch (cmd->command){
case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: {
// Just switch theflight mode here, the navigator takes care of
// doing somethingsensible with the coordinates. Its designed
// to not requirenavigator and command to receive / process
// the data at theexact same time.
// Check if a modeswitch had been requested
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);
if ((main_ret!= TRANSITION_DENIED)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
mavlink_log_critical(&mavlink_log_pub,"Rejectingreposition command");
}
} else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
}
break;
case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE
break;
...
case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE
break;
...
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 boolstay_in_failsafe, //故障保护数据
status_flags_s *status_flags, //commander内部状态标志
bool landed,
const 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 = status->nav_state;
...
switch(internal_state->main_state)
{
case commander_state_s::MAIN_STATE_ACRO:
case commander_state_s::MAIN_STATE_MANUAL:
case commander_state_s::MAIN_STATE_RATTITUDE:
case commander_state_s::MAIN_STATE_STAB:
case commander_state_s::MAIN_STATE_ALTCTL:
/* require RC for all manual modes */
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, rc_loss_act);
} else {
switch(internal_state->main_state) {
case commander_state_s::MAIN_STATE_ACRO:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
break;
case commander_state_s::MAIN_STATE_MANUAL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
case commander_state_s::MAIN_STATE_RATTITUDE:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
break;
case commander_state_s::MAIN_STATE_STAB:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
break;
case commander_state_s::MAIN_STATE_ALTCTL:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
break;
default:
status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
break;
}
}
break;
...
}
…
//以下如法炮制
…
…
return status->nav_state != nav_state_old; //设置成功逻辑为1
}
返回值：
| 1 | 状态改变成功 |
| 0 | 状态未变（失败） |
一切判断，做完了最后是设置了：那就是函数：
⑥ set_control_mode();
这个函数简单就是最后的模式设定了，但是值得注意的是，对于不同的模式在加速度，速度，XY平面内\Z方向上的 速度加速度，tilt、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驱动模块参考用。
基本将commander的顺序理解完了，还有一件大事，耶，对了突然想起来还有个哥们在干活嘞，那就是Low_prio_thread了，你到底是干啥子的嘞。看来还不能休息哈,那我们一口气干完再说休息么。那还是继续…
void *commander_low_prio_loop(void *arg)//指针函数提供posix跨平台api——创建线程入口
{ //这个线程的函数名字为：commander_low_prio
订阅了一个msg // ORB_ID(vehicle_command)
…
}
可以看到在这个函数中忽略了high_prio command 执行了 低优先级任务：包含：
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case 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通话消息：
地面站进行水平校准操作：
截止目前为止，基本上算是走完流程了，剩下的就是细节，。
但是，告诉大家一个不好的消息，对于自动化过程中px4还是需要很多细节要做的，比如，校准问题，就是个大问题，我们在“啪啪啪”在地面站上整了一通校准（整起来麻烦），最后飞了几下，或者炸鸡（希望不要发生吗），再次链接地面站——发现地面站上，耶，NND又不给大爷水平了（或者不准确了，有木有发现）这个就是个基本问题点之一啊，大神们，慢慢来做自己工作吧。
因此，我相信，说道这里，读过代码的——明眼人都懂得了。好了，感谢不厌其烦的 看完了这篇学习记录。水平有限，纰漏之处，望批评指正。
我是一只奔跑中的蜗牛，再次感谢，奋斗在同行中的伙伴们读到这里；
联系邮箱：xiwei@adicn.com
联系方式：QQ 344278952
L1_control 待续….
2967
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
