对于复杂的函数，要做的就是看函数的输入是什么、来自哪里，经过处理后得到什么、给谁用，这样就可以把程序逻辑理清(中间的分析就是看函数如何处理的)
extern "C" __EXPORT int commander_main(int argc, char *argv[]);argc和argv是commander_main函数的形参，它们是程序的“命令行参数”。agrc(argument count的缩写，意思是参数个数)，argv(argument vector的缩写，意思是参数向量)，它是一个*char指针数组，数组中每一个元素指向命令行中的一个字符串。main函数是操作系统调用的，实参只能由操作系统给出。在操作命令状态下，实参是和执行文件的命令一起给出的。例如在DOS、UNIX或Linux等系统的操作命令状态下，在命令行中包括了命令名和需要传给main函数的参数。
命令行的一般形式为：
命令名 参数1 参数2 …… 参数n
命令名和各参数之间用空格分隔。命令名是可执行文件名(此文件包含main函数)。
在rcS执行的时候，比如commander_main start
那么agrc就等于2，agrv[0]就是commander_main这个字符串，argv[1]就是start。
所以要判断agrv[1]是start还是stop。
就像你在dos命令行里输入commander start，自然就给agrc和agrv[]赋值。NuttX系统下的模块的主函数名字都是以”_main”开始的，但是调用的时候不加“_main”。
例如：extern “C” _EXPORT int commander_main (int argc, char *argv[]);
这里extern “C”告诉编译器在编译commander_main这个函数时按照C的规则去翻译相关的函数名而不是C++的；__EXPORT 表示将函数名输出到链接器(Linker)。
然后跳转到函数的定义部分int commander_main (int argc, char *argv[])，判断系统给出的命令行的参数，一系列的判断，C++在大型项目上的优势这里有没有发挥出来！
进入commander_main()
int commander_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("already running");
/* this is not an error */
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 constexpr max_wait_us = 1000000;
unsigned constexpr max_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("commander already stopped");
return 0;
}
thread_should_exit = true;
while (thread_running) {
usleep(200000);
warnx(".");
}
warnx("terminated.");
return 0;
}
/* commands needing the app to run below */
if (!thread_running) {
warnx("\tcommander not 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("calibration failed, exiting.");
return 1;
} else {
return 0;
}
} else {
warnx("missing argument");
}
}
if (!strcmp(argv[1], "check")) {
int checkres = 0;
checkres = preflight_check(&status, &mavlink_log_pub, false, true, &status_flags, &battery, false, hrt_elapsed_time(&commander_boot_timestamp));
warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
checkres = preflight_check(&status, &mavlink_log_pub, true, true, &status_flags, &battery, true, hrt_elapsed_time(&commander_boot_timestamp));
warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
return 0;
}
if (!strcmp(argv[1], "arm")) {
if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "command line")) {
warnx("arming failed");
}
return 0;
}
if (!strcmp(argv[1], "disarm")) {
if (TRANSITION_DENIED == arm_disarm(false, &mavlink_log_pub, "command line")) {
warnx("rejected disarm");
}
return 0;
}
if (!strcmp(argv[1], "takeoff")) {
/* see if we got a home position */
if (status_flags.condition_home_position_valid) {
if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) {
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 */
/* param 2-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("arming failed");
}
} else {
warnx("rejecting takeoff, 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-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);
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 the other 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-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);
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("mode change failed");
}
return 0;
} else {
warnx("missing argument");
}
}
if (!strcmp(argv[1], "lockdown")) {
if (argc < 3) {
usage("not enough arguments, 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;
}
此图参考了http://blog.csdn.net/oqqenvy12/article/details/69328147
每个argv[1]所对应的函数如下
px4_task_spawn_cmd()
守护进程daemon是运行在后台的进程。
在NuttX中守护进程是一个任务，在POSIX(Linux/Mac OS)中是一个线程
daemon_task = px4_task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 40,
3600,
commander_thread_main,
(char * const *)&argv[0]);以下是参数：
- arg0: 进程名 commander
- arg1: 调度类型（RR or FIFO）the scheduling type (RR or FIFO)
- arg2: 调度优先级
- arg3: 新进程或线程堆栈大小
- arg4: 任务/线程主函数
- arg5: 一个void指针传递给新任务,在这种情况下是命令行参数
在Unix和其他多任务操作系统中daemon程序是指作为一个后台进程运行的计算机程序，而不是由用户直接控制的程序，daemon概念的好处是它不需要被用户或者shell发送到后台就能被启动，并且当它在运行时可以通过shell查询它的状态，它也可以被终止。
后台应用程序只是暂时存在用与开始后台作业，在MakeFile中指定的堆栈大小仅适用于这个管理任务。实际的堆栈大小应在task_create( )调用中设置。
主函数由一个daemon控制函数代替，主函数中原来的部分现在由一个后台任务(task)/线程(thread)来代替。
print_status()
打印机型(旋翼还是固定翼)/USB插还是拔了、电源电压是否合法/航空电子管的电压/home点的经纬度、高度、偏航角(指南针)/home点的空间坐标xyz/数据链连接还是断开/main state/nav state/status.arming_state
calibrate
校正函数分别校正：mag、accel、gyro、level、esc、airspeed。这里对应着起飞前的校正步骤，其具体实现过程暂时不讨论。
preflight_check
预飞检查
checkres = preflight_check(&status,
&mavlink_log_pub,
false,
true,
&status_flags,
&battery,
false,
hrt_elapsed_time(&commander_boot_timestamp));int preflight_check(struct vehicle_status_s *status,
orb_advert_t *mavlink_log_pub,
bool prearm,
bool force_report,
status_flags_s *status_flags,
battery_status_s *battery,
bool can_arm_without_gps,
hrt_abstime time_since_boot)检查各项赋值给preflight_ok，首先是用函数Commander::preflightCheck(…)检查，然后是否连着USB，最后检查电压，返回值为!preflight_ok。
USB：
if (!status_flags->circuit_breaker_engaged_usb_check && status_flags->usb_connected && prearm) {
preflight_ok = false;
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: Flying with USB is not safe");
}
}由上述程序可知USB是否连接主要来自以下判断
status_flags.circuit_breaker_engaged_usb_check= circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY);
/* finally judge the USB connected state based on software detection */
status_flags.usb_connected = _usb_telemetry_active;prearm是自动传入的参数true/false
电压：
orb_copy(ORB_ID(battery_status), battery_sub, &battery);电压来源于px4io.cpp
_battery.updateBatteryStatus(timestamp, voltage_v, current_a, _last_throttle, _armed, &battery_status)bool preflight_ok = Commander::preflightCheck(mavlink_log_pub,
true,
true,
true,
true,
checkAirspeed,
(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
!can_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,
bool checkAirspeed,
bool checkRC,
bool checkGNSS,
bool checkDynamic,
bool isVTOL,
bool reportFailures,
bool prearm,
hrt_abstime time_since_boot)里面的检查函数主要为
magnometerCheck(mavlink_log_pub, i, !required, device_id, reportFailures)
accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, reportFailures)
gyroCheck(mavlink_log_pub, i, !required, device_id, reportFailures)
baroCheck(mavlink_log_pub, i, !required, device_id, reportFailures)
imuConsistencyCheck(mavlink_log_pub, checkAcc, checkGyro, reportFailures)
airspeedCheck(mavlink_log_pub, true, reportFailures, prearm, time_since_boot)
rc_calibration_check(mavlink_log_pub, reportFailures, isVTOL)
gnssCheck(mavlink_log_pub, reportFailures)
ekf2Check(mavlink_log_pub, true, reportFailures)分别检查mag、accel、gyro、baro、imu一致性、airspeed、rc校准、ekf2。返回值为!failed。
arm_disarm上锁解锁判定函数
arm_disarm(true, &mavlink_log_pub, "command line")---arm
arm_disarm(false, &mavlink_log_pub, "command line")---disarm
transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, const char *armedBy)参数：
bool arm上锁/解锁
orb_advert_t *mavlink_log_pub_local
const char *armedBy
返回TRANSITION_DENIED/TRANSITION_CHANGED/TRANSITION_NOT_CHANGED
实际飞行上锁解锁主要靠arming_state_transition()函数判断
arming_res = arming_state_transition(&status,
&battery,
&safety,
arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY,
&armed,
true /* fRunPreArmChecks */,
mavlink_log_pub_local,
&status_flags,
avionics_power_rail_voltage,
can_arm_without_gps,
hrt_elapsed_time(&commander_boot_timestamp));transition_result_t arming_state_transition(struct vehicle_status_s *status,
struct battery_status_s *battery,
const struct safety_s *safety,
arming_state_t new_arming_state,
struct actuator_armed_s *armed,
bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log
status_flags_s *status_flags,
float avionics_power_rail_voltage,
bool can_arm_without_gps,
hrt_abstime time_since_boot)参数：
当前锁定状态是从arming_state_t current_arming_state = status->arming_state传入的
新的锁定状态是从函数参数arming_state_t new_arming_state传入的
返回TRANSITION_DENIED/TRANSITION_CHANGED/TRANSITION_NOT_CHANGED
判断是否需要改变锁定状态
if (new_arming_state == current_arming_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
……
}if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {// 1&&1&&1
prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */,status_flags, battery, can_arm_without_gps, time_since_boot);
}在改变锁定状态前一定会进行预飞检查，而prearm_ret = !preflight_ok;
if (!status_flags->condition_system_sensors_initialized
&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */, false /* force_report */, status_flags, battery, can_arm_without_gps, time_since_boot);
status_flags->condition_system_sensors_initialized = !prearm_ret;
last_preflight_check = hrt_absolute_time();
last_prearm_ret = prearm_ret;
} else {
prearm_ret = last_prearm_ret;
}
}status_flags.condition_system_sensors_initialized = Commander::preflightCheck(……) //commander里
如果预飞检查有问题，那么还需要再检查一遍
bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
行是新的锁定状态，列是当前锁定状态。True表示转变合法，false表示转变不合法。虽然有的标记为true，但是仍然需要额外的检查判断。
二次判断：如果新的锁定状态是armed(解锁)，那么预飞检查失败、安全开关没开、电源分离器没连接或航空电源电压低于4.5v，都不能转变成armed；如果新的锁定状态是STANDBY&&当前锁定状态是armed_error，那么新的锁定状态赋为STANDBY_error。
如果当前锁定状态为STANDBY_error，而操作者正打算解锁，那么需要根据预飞检测传感器的状态告知操作者重启和一些提示信息。
经过上述多次判断处理就可以改变锁定状态了
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;说到底arm_disarm()和arming_state_transition()处理最后只是返回一个该不该改变锁定状态的标志位
main_state_transition飞行模式切换的条件判断函数
main_state_transition(&status,
main_state_transition(&status,
new_main_state,
main_state_prev,
&status_flags,
&internal_state)
main_state_transition(struct vehicle_status_s *status,
main_state_t new_main_state,
uint8_t &main_state_prev,
status_flags_s *status_flags,
struct commander_state_s *internal_state)参数：
status：导航状态，飞行器应该进入的状态；
new_main_state： 期望切换到的新状态
&main_state_prev： 之前的状态
status_flags： commander内部的状态标志；
internal_state： 内部状态
返回值：
ret = TRANSITION_DENIED; // 表示不满足切换条件，拒绝状态切换
ret = TRANSITION_CHANGED; // 切换到某个状态，不一定是用户想要的目标状态，会根据降级策略，切换至一个最接近的状态。
ret = TRANSITION_NOT_CHANGED;
根据status_flags->condition_local_altitude_valid、status_flags->condition_global_position_valid、status_flags->condition_local_position_valid等信息看是否可以转换成新的飞行状态，判断通过则ret = TRANSITION_CHANGED，如果没写判断，那么默认ret = TRANSITION_DENIED。
/**********************commander_thread_main**********************/
(1)param_t xxx=param_find(const char *name)将*name所对应的参数值赋给xxx
(2)给status、status_flags结构体赋初始值
(3)用dm_read ()读取mission结构体
/** Retrieve from the data manager file */
dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)
dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
ssize_t
dm_read(
dm_item_t item, /* The item type to retrieve */
unsigned char index, /* The index of the item */
void *buffer, /* Pointer to caller data buffer */
size_t buflen /* Length in bytes of data to retrieve */
)(4)status、armed、control_mode、_home、_roi、command_ack、safety、geofence_result、sp_man—ORB_ID(manual_control_setpoint)、offboard_control_mode、global_position、gps_position、sensors、diff_pres、cmd、battery、info、pos_sp_triplet、system_power、actuator_controls、vtol_status、cpuload初始化为0
(5)control_status_leds(&status, &armed, true, &battery, &cpuload);
void control_status_leds(vehicle_status_s *status_local,
const actuator_armed_s *actuator_armed,
bool changed,
battery_status_s *battery_local,
const cpuload_s *cpuload_local)
功能：根据飞行器的锁定状态、传感器状态检测情况和CPU负载情况选择如何亮灯。
(6)获取飞机机型
/* update vehicle status to find out vehicle type (required for preflight checks) */
param_get(_param_sys_type, &(status.system_type)); // get system type
status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status);
status.is_vtol = is_vtol(&status);(7)获取参数
param_get(param_t param, void *val)
从param获取参数，存到*val
(8)创建一个低优先级的进程，并运行commander_low_prio_loop()，随后使用pthread_attr_destroy(&commander_low_prio_attr)将低优先级进程终结。
/* 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(3000));
#ifndef __PX4_QURT
// This is not supported by QURT (yet).
struct sched_param param;
(void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
(void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m);
#endif
pthread_create(&commander_low_prio_thread,&commander_low_prio_attr, commander_low_prio_loop, nullptr);
pthread_attr_destroy(&commander_low_prio_attr);其中void *commander_low_prio_loop(void *arg)
①等待cmd更新，等待1s，超出1s则跳出本函数；少于1s则进行以下处理
②
/* ignore commands the high-prio loop or the navigator handles */
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_MODE ||
cmd.command == vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM ||
cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF ||
cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_SERVO ||
cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
continue;
}以上cmd属于高优先级，直接利用continue跳出本次循环，进行下一次循环
continue表示进行下一次循环，只管for、while，不看if，不管多少if都直接无视
③switch (cmd.command)
cmd来自
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
// Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: //Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot //onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| //Empty| Empty|
用answer_command()函数驱动蜂鸣器不同的音调回应操作命令cmd
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
// Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro //calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, //1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| //Empty|
arming_state_transition()
根据cmd.param的值选择校准的传感器
Commander::preflightCheck()
arming_state_transition()
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE:
// Request storage of different parameter values and logs. This command will be only //accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: //WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: //WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
cmd.param1=0时，ret = param_load_default();
cmd.param1=1时，ret = param_save_default();
cmd.param1=2时， param_reset_all(); int ret = param_save_default();
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
// Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|(9)进入循环while (!thread_should_exit)
[1]orb_check(param_changed_sub, &updated);检查参数更新没有，若更新了，那么重新get参数。ORB_ID(parameter_update)来自src/modules/systemlib/param/param.c、src/modules/systemlib/param/param_shmeem.c
[2]orb_check(sp_man_sub, &updated);
ORB_ID(manual_control_setpoint)来自src/modules/sensors/rc_update.cpp
[3]orb_check(offboard_control_mode_sub, &updated);
并根据offboard_control_mode.timestamp给status_flags.offboard_control_signal_lost、status_flags.offboard_control_loss_timeout、status_changed赋值
ORB_ID(offboard_control_mode)来自src/modules/mavlink/mavlink_reciever.cpp
[4]orb_check(telemetry_subs[i], &updated);//数传
//当连接了新的数传，进行系统检查
Commander::preflightCheck()
hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)//计算上一次到当前获取的时间戳增量
ORB_ID(telemetry_status)来自src/modules/mavlink/mavlink_reciever.cpp
[5]orb_check(sensor_sub, &updated);
if (hrt_elapsed_time(&baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT)//根据两次获取气压计数据的时间差判断status_flags.barometer_failure
//单独检查气压计，是因为气压计检测的绝对海拔高度，与其他的空中操作不相关
ORB_ID(sensor_combined)来自src/modules/sensors/sensors.cpp
[6]orb_check(diff_pres_sub, &updated);//空速计用于VTOL的固定翼模式
check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status_flags.condition_airspeed_valid), &status_changed);根据两次获取空速计数据的时间差判断status_flags.condition_airspeed_valid
ORB_ID(differential_pressure)来自src/drivers/ets_airspeed/ets_airspeed.cpp、src/drivers/meas_airspeed/meas_airspeed.cpp
[7]orb_check(system_power_sub, &updated);
根据system_power结构体判断status_flags.condition_power_input_valid
如果USB移除，但仍然接着电源，发布警告“重新启动”
判断是否连接了USB，status_flags.usb_connected = _usb_telemetry_active
_usb_telemetry_active来自于数传的结构体
/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
_usb_telemetry_active = true;
}ORB_ID(system_power)来自src/drivers/stm32adc/adc.cpp
[8]orb_check(safety_sub, &updated);//安全开关
//如果安全开关被打开(锁定飞机)而此时飞机处于解锁状态，那么飞机利用arming_state_transition()上锁
//根据安全开关状态更新，控制闪光和蜂鸣器，示意操作者
ORB_ID(safety)来自src/drivers/px4io/px4io.cpp(v2版本)、src/drivers/px4fmu/px4fmu.cpp(v4版本)
[9]orb_check(vtol_vehicle_status_sub, &updated);
判断是否是VTOL，如果是，那么将vtol_status结构体赋值给status和status_flags
ORB_ID(vtol_vehicle_status)来自src/modules/votl_att_control/votl_att_control.cpp
[10]orb_check(global_position_sub, &updated);
根据status_flags.condition_global_position_valid和gpos.eph情况，判断是否orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
根据两次获取global_position数据的时间差判断status_flags.condition_global_position_valid
ORB_ID(vehicle_global_position)来自src/modules/ekf2/ekf2_main.cpp或src/modules/position_estmator_inav/position_estmator_inav_main.cpp或src/modules/local_position_estmator/local_position_estmator_main.cpp
[11]orb_check(local_position_sub, &updated);
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
根据两次获取local_position数据的时间差，同时需要综合local_position.xy_valid && local_eph_good和local_position.z_valid，判断condition_local_position_valid和condition_local_altitude_valid
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid
&& local_eph_good, &(status_flags.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid,
&(status_flags.condition_local_altitude_valid), &status_changed);ORB_ID(vehicle_local_position)来自src/modules/ekf2/ekf2_main.cpp或src/modules/position_estmator_inav/position_estmator_inav_main.cpp或src/modules/local_position_estmator/local_position_estmator_main.cpp
[12]orb_check(attitude_sub, &updated);
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
ORB_ID(vehicle_attitude)来自src/modules/ekf2/ekf2_main.cpp或src/modules/attitude_estmator_q/attitude_estmator_q_main.cpp或src/modules/local_position_estmator/local_position_estmator_main.cpp或
[13]orb_check(land_detector_sub, &updated);
根据上一次以及当前的land和fall状态判断应该发送什么mavlink信息
was_landed = land_detector.landed;
was_falling = land_detector.freefall;自动上锁。滞后时间是param_get(_param_disarm_land, &disarm_when_landed)，若为起飞状态，但没有及时起飞，5倍的disarm_when_landed时间后自动上锁。
ORB_ID(vehicle_land_detected)来自src/modules/land_detector/LandDetector.cpp
[14]orb_check(cpuload_sub, &updated);
orb_copy(ORB_ID(cpuload), cpuload_sub, &cpuload);
ORB_ID(cpuload)来自src/modules/load_mon/load_mon.cpp
[15]orb_check(battery_sub, &updated);
根据电池警告的标志
battery.warning == battery_status_s::BATTERY_WARNING_LOW
battery.warning == battery_status_s::BATTERY_WARNING_CRITICAL做不同的处理，其中battery.warning来自src/modules/systemlib/battery.cpp
Battery::determineWarning()
{
// Smallest values must come first
if (_remaining < _param_crit_thr.get()) {
_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
} else if (_remaining < _param_low_thr.get()) {
_warning = battery_status_s::BATTERY_WARNING_LOW;
}
}根据armed.armed、low_bat_action的状态，也就是飞机正处于什么样的低电压飞行状态，采取main_state_transition()将飞机飞行状态转变成commander_state_s::MAIN_STATE_AUTO_RTL或commander_state_s::MAIN_STATE_AUTO_LAND
其中low_bat_action来自param_get(_param_low_bat_act, &low_bat_action);(此处分析，这些param参数是会实时改变的，不一定是用户实现全部设定好了的，不然也不需要放在while (!thread_should_exit) {}循环里面)
ORB_ID(battery_status)来自src/drivers/px4io/px4io.cpp
[16]orb_check(subsys_sub, &updated);
将info结构体变量赋值给status.onboard_control_sensors_present、status.onboard_control_sensors_enabled、status.onboard_control_sensors_health
ORB_ID(subsystem_info)来自airspeed、px4flow等传感器驱动
[17]orb_check(pos_sp_triplet_sub, &updated);
如果处于init状态，那么尝试解锁arming_state_transition()进入standby状态
ORB_ID(position_setpoint_triplet) 来自src/modules/navigator/navigator_main.cpp
[18]orb_check(gps_sub, &updated);
如果没有初始化&&GPS精度可用&&两次数据采集时间在1s内，使用globallocalconverter_init()函数里的map_projection_global_init(lat_0, lon_0, timestamp)将经纬度的球面地图到平面直角坐标地图
为什么需要使用投影函数？
地球椭球体表面是曲面，为了将球面坐标转换成平面坐标，而转换时首先要把曲面展为平面。然而球面是个不可展的曲面，换句话说，就是把它直接展为平面时，不可能不发生破裂或皱纹，于是需要地图投影理论，也就构成了map_projection()函数。
http://blog.sciencenet.cn/blog-99337-651267.html
http://baike.baidu.com/link?url=VtAENt2GBYLoYXnTxSfHFxARu9rsQacqT3EMAYitm3bd1nvfngwBKsiasR1lSoluFZlY2G4fQ5wNBnXb7AF6115lPSxz8iTYdbnbNklxAoAjn35Nb3VK2AQd8-bBZrsq
根据gpsIsNoisy、gps_position.fix_type、hrt_elapsed_time(&gps_position.timestamp)判断status_flags.gps_failure和status_changed
ORB_ID(vehicle_gps_position)来自src/drivers/gps/gps.cpp
[19]orb_check(mission_result_sub, &updated);
更新status.mission_failure(status.mission_failure = _mission_result.mission_failure;)
ORB_ID(mission_result)来自src/drivers/navigator/navigator_main.cpp
[20]orb_check(geofence_result_sub, &updated); 地理围栏
地理围栏是指当飞机进入、离开某个特定地理区域,或在该区域内活动时,操作者可以自动接收通知和警告。
根据geofence_result.geofence_action的情况分别采取如下动作：none、warn、主状态转变为loiter、主状态转变为rtl、armed.force_failsafe = true。
更新geofence_loiter_on、geofence_rtl_on，如果不在loiter/rtl模式或手动转换到loiter/rtl模式时，该标志位复位。
如果紧急动作激活了&&在rtl模式之前处于手动和协助模式下，此时遥控摇杆拨动了，那么回到rtl模式之前的模式
ORB_ID(geofence_result)来自src/drivers/navigator/navigator_main.cpp
[21]根据_mission_result.flight_termination和counter(记录commander循环次数)发送警告信息。
根据_mission_result.valid通过tune_mission_fail()和tune_mission_ok()指示操作者。_mission_result.instance_count是任务的实例数，任务改变时单调增加。只有在确定home点之后才评估任务完成的状态，这样可以避免任务拒绝的误报。
[22]遥控输入的处理
①上锁解锁
根据是否有操作者上锁或解锁的命令(可以是遥控摇杆左下/右下，档位开关)和旋翼机是否在MANUAL, Rattitude, or AUTO_READY 模式下(固定翼是否在land模式下)，判断是否运行arming_state_transition()函数以完成锁定状态status->arming_state的更新。
②根据遥控模式选择开关评估飞机的主状态
transition_result_t main_res = set_main_state_rc(&status);
如果手动模式设定没有更改，那么不运行手动模式设定更改了的代码，只需要_last_sp_man结构体=sp_man结构体，因为判断后会运行return TRANSITION_NOT_CHANGED(函数中无论什么地方，遇到return函数就结束了)；
sp_man.offboard_switch开关如果打开了，那么运行main_state_transition()函数，飞机主状态转变为commander_state_s::MAIN_STATE_OFFBOARD；
sp_man.return_switch开关如果打开了，那么运行main_state_transition()函数，飞机主状态转变为commander_state_s::MAIN_STATE_AUTO_RTL，如果转变为rtl，那么降级转变为commander_state_s::MAIN_STATE_AUTO_LOITER；
如果sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE，那么利用main_state_transition()函数将飞机主状态转变为_flight_mode_slots[sp_man.mode_slot]，其中_flight_mode_slots[sp_man.mode_slot]来自
param_get(_param_fmode_1, &_flight_mode_slots[0]);
param_get(_param_fmode_2, &_flight_mode_slots[1]);
param_get(_param_fmode_3, &_flight_mode_slots[2]);
param_get(_param_fmode_4, &_flight_mode_slots[3]);
param_get(_param_fmode_5, &_flight_mode_slots[4]);
param_get(_param_fmode_6, &_flight_mode_slots[5]);如果转变不了，那么降级转变
MAIN_STATE_AUTO_MISSION==>MAIN_STATE_AUTO_LOITER
MAIN_STATE_AUTO_RTL==>MAIN_STATE_AUTO_LOITER
MAIN_STATE_AUTO_LAND==>MAIN_STATE_AUTO_LOITER
MAIN_STATE_AUTO_TAKEOFF==>MAIN_STATE_AUTO_LOITER
MAIN_STATE_AUTO_FOLLOW_TARGET==>MAIN_STATE_AUTO_LOITER
MAIN_STATE_AUTO_LOITER==>MAIN_STATE_POSCTL
MAIN_STATE_POSCTL==>MAIN_STATE_ALTCTL
MAIN_STATE_ALTCTL==>MAIN_STATE_STAB
MAIN_STATE_STAB==>MAIN_STATE_MANUAL如果offboard and RTL开关off或denied，那么检查sp_man.mode_switch，sp_man.mode_switch可分为SWITCH_POS_OFF(MANUAL)、SWITCH_POS_MIDDLE(ASSIST)、SWITCH_POS_ON(AUTO)
sp_man.mode_switch= SWITCH_POS_OFF(MANUAL) 时，由sp_man.acro_switch、sp_man.rattitude_switch两个开关选择模式
sp_man.mode_switch= SWITCH_POS_MIDDLE(ASSIST)时，由sp_man.posctl_switch选择模式
sp_man.loiter_switch= SWITCH_POS_ON(AUTO) 时，由sp_man.loiter_switch选择模式
里面的逻辑为，其中也包含了降级策略(如果不能转变成想要的状态，那么转变成与其最接近的状态，还不能转变，再下一级)
set_main_state_rc ()返回TRANSITION_DENIED、TRANSITION_CHANGED 、TRANSITION_NOT_CHANGED，并且根据遥控器指令将internal_state->main_state赋予新的状态。
根据set_main_state_rc()是否能转变飞行模式，发送指示给操作者。
检测油门锁定开关(Set to true if actuators are forced to being disabled (due to emergency or HIL))，根据检测sp_man.kill_switch 状态，确定armed.manual_lockdown状态。
③若遥控信息丢失，那么status.rc_signal_lost = true，记录下当前的时间戳rc_signal_lost_timestamp = sp_man.timestamp;
[23]检测无线数传的数据链是否连接
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {…}循环四次，输出have_link状态量，根据have_link 确定status.data_link_lost状态量
循环里if(telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6)符合该条件表示数据链连接了
循环里if (telemetry_lost[i] && hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) 符合该条件表示上次数据链是断开的
[24] orb_check(actuator_controls_sub, &updated);
针对固定翼机型检测发动机是否有故障。
actuator_controls主题包含actuator_controls_0、actuator_controls_1、actuator_controls_2、actuator_controls_3
ORB_ID_VEHICLE_ATTITUDE_CONTROLS来自于
或，分析如下
中
#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
旋翼机型中
_actuators_id = ORB_ID(actuator_controls_0);
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
vtol机型中
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
[25]orb_check(cmd_sub, &updated);
ORB_ID(vehicle_command)来自或也就是接收数传指令，远距离在地面站中传输操作者指令
handle_command(&status,
&safety,
&cmd,
&armed,
&_home,
&global_position,
&local_position,
&attitude,
&home_pub,
&command_ack_pub,
&command_ack,
&_roi,
&roi_pub)bool handle_command(struct vehicle_status_s *status_local,
const struct safety_s *safety_local,
struct vehicle_command_s *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)
switch (cmd->command) {
case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION:
case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE:
case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM:
case vehicle_command_s::VEHICLE_CMD_OVERRIDE_GOTO:
case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION:
case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME:
case vehicle_command_s::VEHICLE_CMD_NAV_GUIDED_ENABLE:
case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH:
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF:
case vehicle_command_s::VEHICLE_CMD_NAV_LAND:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI:
case vehicle_command_s::VEHICLE_CMD_MISSION_START:每个case是一个大类，同时根据传入的cmd->param，调用main_state_transition()转变飞行模式，转变结果赋值给cmd_result，最后调用answer_command(*cmd, cmd_result, *command_ack_pub, *command_ack)回应操作者。
[26]如果数据链(无线数传)和GPS工作故障了，并且飞行器不在手动模式(MAIN_STATE_MANUAL、MAIN_STATE_ACRO、MAIN_STATE_RATTITUDE、MAIN_STATE_STAB、MAIN_STATE_ALTCTL、MAIN_STATE_POSCTL)那么结束飞行
如果遥控和GPS工作故障了，并且飞行器在手动模式，那么结束飞行
[27]commander_set_home_position()
这个函数初始化飞行器的home点的位置，此函数运行在第一次得到fix的GPS信号时和每次解锁时(并且此时GPS为fix状态)
[28]
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);bool set_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
struct commander_state_s *internal_state,
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,
bool landed,
const link_loss_actions_t rc_loss_act,
const int offb_loss_act,
const int offb_loss_rc_act)bool set_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
struct commander_state_s *internal_state,
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,
bool landed,
const link_loss_actions_t rc_loss_act,
const int offb_loss_act,
const int offb_loss_rc_act)status：导航状态，飞行器应该进入的状态；
internal_state： 主状态，用户想要的状态；
data_link_loss_enabled：与地面站的数据链丢失；
mission_finished： 任务完成；
stay_in_failsafe：故障保护；
status_flags：commander内部的状态标志；
landed： 着陆；
rc_loss_enabled：遥控信号丢失；
offb_loss_act：
offb_loss_rc_act：
检查故障和internal_state->main_state，按照降级策略，给status->nav_state赋状态status->nav_state用于navigator.cpp。
一般情况为：
①global位置有效 && 起飞点(home)有效 && GPS有效
进入NAVIGATION_STATE_AUTO_**RCRECOVER**模式
②local位置有效 && GPS有效
进入NAVIGATION_STATE_AUTO_**LAND**模式(自动降落)
③local高度有效
进入NAVIGATION_STATE_**DESCEND**模式(无位置控制的下降模式)
④终止飞行
进入NAVIGATION_STATE_**TERMINATION**模式
[29]set_control_mode()
根据status->nav_state，确定control_mode结构体的状态，用于mc_att_control.cpp和mc_pos_control.cpp
[30]最后做一些指示信号、发布一些主题就可以终止commander进程了。
