當(dāng)前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

pixhawk px4 commander.cpp

發(fā)布時(shí)間：2024/4/18 编程问答 30 豆豆

生活随笔收集整理的這篇文章主要介紹了 pixhawk px4 commander.cpp 小編覺得挺不錯(cuò)的,現(xiàn)在分享給大家,幫大家做個(gè)參考.

對(duì)于復(fù)雜的函數(shù)，要做的就是看函數(shù)的輸入是什么、來自哪里，經(jīng)過處理后得到什么、給誰用，這樣就可以把程序邏輯理清(中間的分析就是看函數(shù)如何處理的)

extern "C" __EXPORT int commander_main(int argc, char *argv[]);

argc和argv是commander_main函數(shù)的形參，它們是程序的“命令行參數(shù)”。agrc(argument count的縮寫，意思是參數(shù)個(gè)數(shù))，argv(argument vector的縮寫，意思是參數(shù)向量)，它是一個(gè)*char指針數(shù)組，數(shù)組中每一個(gè)元素指向命令行中的一個(gè)字符串。main函數(shù)是操作系統(tǒng)調(diào)用的，實(shí)參只能由操作系統(tǒng)給出。在操作命令狀態(tài)下，實(shí)參是和執(zhí)行文件的命令一起給出的。例如在DOS、UNIX或Linux等系統(tǒng)的操作命令狀態(tài)下，在命令行中包括了命令名和需要傳給main函數(shù)的參數(shù)。
命令行的一般形式為：
命令名參數(shù)1 參數(shù)2 …… 參數(shù)n
命令名和各參數(shù)之間用空格分隔。命令名是可執(zhí)行文件名(此文件包含main函數(shù))。
在rcS執(zhí)行的時(shí)候，比如commander_main start
那么agrc就等于2，agrv[0]就是commander_main這個(gè)字符串，argv[1]就是start。
所以要判斷agrv[1]是start還是stop。
就像你在dos命令行里輸入commander start，自然就給agrc和agrv[]賦值。NuttX系統(tǒng)下的模塊的主函數(shù)名字都是以”_main”開始的，但是調(diào)用的時(shí)候不加“_main”。
例如：extern “C” _EXPORT int commander_main (int argc, char *argv[]);
這里extern “C”告訴編譯器在編譯commander_main這個(gè)函數(shù)時(shí)按照C的規(guī)則去翻譯相關(guān)的函數(shù)名而不是C++的；__EXPORT 表示將函數(shù)名輸出到鏈接器(Linker)。
然后跳轉(zhuǎn)到函數(shù)的定義部分int commander_main (int argc, char *argv[])，判斷系統(tǒng)給出的命令行的參數(shù)，一系列的判斷，C++在大型項(xiàng)目上的優(yōu)勢(shì)這里有沒有發(fā)揮出來！
進(jìn)入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
每個(gè)argv[1]所對(duì)應(yīng)的函數(shù)如下

px4_task_spawn_cmd()
守護(hù)進(jìn)程daemon是運(yùn)行在后臺(tái)的進(jìn)程。
在NuttX中守護(hù)進(jìn)程是一個(gè)任務(wù)，在POSIX(Linux/Mac OS)中是一個(gè)線程

daemon_task = px4_task_spawn_cmd("commander",SCHED_DEFAULT,SCHED_PRIORITY_DEFAULT + 40,3600,commander_thread_main,(char * const *)&argv[0]);

以下是參數(shù)：
- arg0: 進(jìn)程名 commander
- arg1: 調(diào)度類型（RR or FIFO）the scheduling type (RR or FIFO)
- arg2: 調(diào)度優(yōu)先級(jí)
- arg3: 新進(jìn)程或線程堆棧大小
- arg4: 任務(wù)/線程主函數(shù)
- arg5: 一個(gè)void指針傳遞給新任務(wù),在這種情況下是命令行參數(shù)
在Unix和其他多任務(wù)操作系統(tǒng)中daemon程序是指作為一個(gè)后臺(tái)進(jìn)程運(yùn)行的計(jì)算機(jī)程序，而不是由用戶直接控制的程序，daemon概念的好處是它不需要被用戶或者shell發(fā)送到后臺(tái)就能被啟動(dòng)，并且當(dāng)它在運(yùn)行時(shí)可以通過shell查詢它的狀態(tài)，它也可以被終止。
后臺(tái)應(yīng)用程序只是暫時(shí)存在用與開始后臺(tái)作業(yè)，在MakeFile中指定的堆棧大小僅適用于這個(gè)管理任務(wù)。實(shí)際的堆棧大小應(yīng)在task_create( )調(diào)用中設(shè)置。
主函數(shù)由一個(gè)daemon控制函數(shù)代替，主函數(shù)中原來的部分現(xiàn)在由一個(gè)后臺(tái)任務(wù)(task)/線程(thread)來代替。
print_status()
打印機(jī)型(旋翼還是固定翼)/USB插還是拔了、電源電壓是否合法/航空電子管的電壓/home點(diǎn)的經(jīng)緯度、高度、偏航角(指南針)/home點(diǎn)的空間坐標(biāo)xyz/數(shù)據(jù)鏈連接還是斷開/main state/nav state/status.arming_state
calibrate
校正函數(shù)分別校正：mag、accel、gyro、level、esc、airspeed。這里對(duì)應(yīng)著起飛前的校正步驟，其具體實(shí)現(xiàn)過程暫時(shí)不討論。
preflight_check
預(yù)飛檢查

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)

檢查各項(xiàng)賦值給preflight_ok，首先是用函數(shù)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是自動(dòng)傳入的參數(shù)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)

里面的檢查函數(shù)主要為

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校準(zhǔn)、ekf2。返回值為!failed。
arm_disarm上鎖解鎖判定函數(shù)

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)

參數(shù)：
bool arm上鎖/解鎖
orb_advert_t *mavlink_log_pub_local
const char *armedBy
返回TRANSITION_DENIED/TRANSITION_CHANGED/TRANSITION_NOT_CHANGED
實(shí)際飛行上鎖解鎖主要靠arming_state_transition()函數(shù)判斷

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 logstatus_flags_s *status_flags,float avionics_power_rail_voltage,bool can_arm_without_gps,hrt_abstime time_since_boot)

參數(shù)：
當(dāng)前鎖定狀態(tài)是從arming_state_t current_arming_state = status->arming_state傳入的
新的鎖定狀態(tài)是從函數(shù)參數(shù)arming_state_t new_arming_state傳入的
返回TRANSITION_DENIED/TRANSITION_CHANGED/TRANSITION_NOT_CHANGED
判斷是否需要改變鎖定狀態(tài)

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&&1prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */,status_flags, battery, can_arm_without_gps, time_since_boot); }

在改變鎖定狀態(tài)前一定會(huì)進(jìn)行預(yù)飛檢查，而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

里
如果預(yù)飛檢查有問題，那么還需要再檢查一遍

bool valid_transition = arming_transitions[new_arming_state][status->arming_state];

行是新的鎖定狀態(tài)，列是當(dāng)前鎖定狀態(tài)。True表示轉(zhuǎn)變合法，false表示轉(zhuǎn)變不合法。雖然有的標(biāo)記為true，但是仍然需要額外的檢查判斷。
二次判斷：如果新的鎖定狀態(tài)是armed(解鎖)，那么預(yù)飛檢查失敗、安全開關(guān)沒開、電源分離器沒連接或航空電源電壓低于4.5v，都不能轉(zhuǎn)變成armed；如果新的鎖定狀態(tài)是STANDBY&&當(dāng)前鎖定狀態(tài)是armed_error，那么新的鎖定狀態(tài)賦為STANDBY_error。
如果當(dāng)前鎖定狀態(tài)為STANDBY_error，而操作者正打算解鎖，那么需要根據(jù)預(yù)飛檢測傳感器的狀態(tài)告知操作者重啟和一些提示信息。
經(jīng)過上述多次判斷處理就可以改變鎖定狀態(tài)了

ret = TRANSITION_CHANGED; status->arming_state = new_arming_state;

說到底arm_disarm()和arming_state_transition()處理最后只是返回一個(gè)該不該改變鎖定狀態(tài)的標(biāo)志位

main_state_transition飛行模式切換的條件判斷函數(shù)
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)

參數(shù)：
? status：導(dǎo)航狀態(tài)，飛行器應(yīng)該進(jìn)入的狀態(tài)；
? new_main_state：期望切換到的新狀態(tài)
? &main_state_prev：之前的狀態(tài)
? status_flags： commander內(nèi)部的狀態(tài)標(biāo)志；
? internal_state：內(nèi)部狀態(tài)
返回值：
? ret = TRANSITION_DENIED; // 表示不滿足切換條件，拒絕狀態(tài)切換
? ret = TRANSITION_CHANGED; // 切換到某個(gè)狀態(tài)，不一定是用戶想要的目標(biāo)狀態(tài)，會(huì)根據(jù)降級(jí)策略，切換至一個(gè)最接近的狀態(tài)。
? ret = TRANSITION_NOT_CHANGED;
根據(jù)status_flags->condition_local_altitude_valid、status_flags->condition_global_position_valid、status_flags->condition_local_position_valid等信息看是否可以轉(zhuǎn)換成新的飛行狀態(tài)，判斷通過則ret = TRANSITION_CHANGED，如果沒寫判斷，那么默認(rèn)ret = TRANSITION_DENIED。

/**********************commander_thread_main**********************/
(1)param_t xxx=param_find(const char *name)將*name所對(duì)應(yīng)的參數(shù)值賦給xxx
(2)給status、status_flags結(jié)構(gòu)體賦初始值
(3)用dm_read ()讀取mission結(jié)構(gòu)體

/** 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)
功能：根據(jù)飛行器的鎖定狀態(tài)、傳感器狀態(tài)檢測情況和CPU負(fù)載情況選擇如何亮燈。
(6)獲取飛機(jī)機(jī)型

/* 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)獲取參數(shù)
param_get(param_t param, void *val)
從param獲取參數(shù)，存到*val
(8)創(chuàng)建一個(gè)低優(yōu)先級(jí)的進(jìn)程，并運(yùn)行commander_low_prio_loop()，隨后使用pthread_attr_destroy(&commander_low_prio_attr)將低優(yōu)先級(jí)進(jìn)程終結(jié)。

/* 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, &param);/* low priority */param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param); #endifpthread_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則跳出本函數(shù)；少于1s則進(jìn)行以下處理
②

/* 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屬于高優(yōu)先級(jí)，直接利用continue跳出本次循環(huán)，進(jìn)行下一次循環(huán)
continue表示進(jìn)行下一次循環(huán)，只管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()函數(shù)驅(qū)動(dòng)蜂鳴器不同的音調(diào)回應(yīng)操作命令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()根據(jù)cmd.param的值選擇校準(zhǔn)的傳感器 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時(shí)，ret = param_load_default(); cmd.param1=1時(shí)，ret = param_save_default(); cmd.param1=2時(shí)， 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)進(jìn)入循環(huán)while (!thread_should_exit)
[1]orb_check(param_changed_sub, &updated);檢查參數(shù)更新沒有，若更新了，那么重新get參數(shù)。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);
并根據(jù)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);//數(shù)傳
//當(dāng)連接了新的數(shù)傳，進(jìn)行系統(tǒng)檢查
Commander::preflightCheck()
hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)//計(jì)算上一次到當(dāng)前獲取的時(shí)間戳增量
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)

//根據(jù)兩次獲取氣壓計(jì)數(shù)據(jù)的時(shí)間差判斷status_flags.barometer_failure
//單獨(dú)檢查氣壓計(jì)，是因?yàn)闅鈮河?jì)檢測的絕對(duì)海拔高度，與其他的空中操作不相關(guān)
ORB_ID(sensor_combined)來自src/modules/sensors/sensors.cpp
[6]orb_check(diff_pres_sub, &updated);//空速計(jì)用于VTOL的固定翼模式

check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status_flags.condition_airspeed_valid), &status_changed);

根據(jù)兩次獲取空速計(jì)數(shù)據(jù)的時(shí)間差判斷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);
根據(jù)system_power結(jié)構(gòu)體判斷status_flags.condition_power_input_valid
如果USB移除，但仍然接著電源，發(fā)布警告“重新啟動(dòng)”
判斷是否連接了USB，status_flags.usb_connected = _usb_telemetry_active
_usb_telemetry_active來自于數(shù)傳的結(jié)構(gòu)體

/* 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);//安全開關(guān)
//如果安全開關(guān)被打開(鎖定飛機(jī))而此時(shí)飛機(jī)處于解鎖狀態(tài)，那么飛機(jī)利用arming_state_transition()上鎖
//根據(jù)安全開關(guān)狀態(tài)更新，控制閃光和蜂鳴器，示意操作者
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結(jié)構(gòu)體賦值給status和status_flags
ORB_ID(vtol_vehicle_status)來自src/modules/votl_att_control/votl_att_control.cpp
[10]orb_check(global_position_sub, &updated);
根據(jù)status_flags.condition_global_position_valid和gpos.eph情況，判斷是否orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
根據(jù)兩次獲取global_position數(shù)據(jù)的時(shí)間差判斷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);
根據(jù)兩次獲取local_position數(shù)據(jù)的時(shí)間差，同時(shí)需要綜合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);
根據(jù)上一次以及當(dāng)前的land和fall狀態(tài)判斷應(yīng)該發(fā)送什么mavlink信息

was_landed = land_detector.landed; was_falling = land_detector.freefall;

自動(dòng)上鎖。滯后時(shí)間是param_get(_param_disarm_land, &disarm_when_landed)，若為起飛狀態(tài)，但沒有及時(shí)起飛，5倍的disarm_when_landed時(shí)間后自動(dòng)上鎖。
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);
根據(jù)電池警告的標(biāo)志

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 firstif (_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;} }

根據(jù)armed.armed、low_bat_action的狀態(tài)，也就是飛機(jī)正處于什么樣的低電壓飛行狀態(tài)，采取main_state_transition()將飛機(jī)飛行狀態(tài)轉(zhuǎn)變成commander_state_s::MAIN_STATE_AUTO_RTL或commander_state_s::MAIN_STATE_AUTO_LAND
其中l(wèi)ow_bat_action來自param_get(_param_low_bat_act, &low_bat_action);(此處分析，這些param參數(shù)是會(huì)實(shí)時(shí)改變的，不一定是用戶實(shí)現(xiàn)全部設(shè)定好了的，不然也不需要放在while (!thread_should_exit) {}循環(huán)里面)
ORB_ID(battery_status)來自src/drivers/px4io/px4io.cpp
[16]orb_check(subsys_sub, &updated);
將info結(jié)構(gòu)體變量賦值給status.onboard_control_sensors_present、status.onboard_control_sensors_enabled、status.onboard_control_sensors_health
ORB_ID(subsystem_info)來自airspeed、px4flow等傳感器驅(qū)動(dòng)
[17]orb_check(pos_sp_triplet_sub, &updated);
如果處于init狀態(tài)，那么嘗試解鎖arming_state_transition()進(jìn)入standby狀態(tài)
ORB_ID(position_setpoint_triplet) 來自src/modules/navigator/navigator_main.cpp
[18]orb_check(gps_sub, &updated);
如果沒有初始化&&GPS精度可用&&兩次數(shù)據(jù)采集時(shí)間在1s內(nèi)，使用globallocalconverter_init()函數(shù)里的map_projection_global_init(lat_0, lon_0, timestamp)將經(jīng)緯度的球面地圖到平面直角坐標(biāo)地圖
為什么需要使用投影函數(shù)？
地球橢球體表面是曲面，為了將球面坐標(biāo)轉(zhuǎn)換成平面坐標(biāo)，而轉(zhuǎn)換時(shí)首先要把曲面展為平面。然而球面是個(gè)不可展的曲面，換句話說，就是把它直接展為平面時(shí)，不可能不發(fā)生破裂或皺紋，于是需要地圖投影理論，也就構(gòu)成了map_projection()函數(shù)。
http://blog.sciencenet.cn/blog-99337-651267.html
http://baike.baidu.com/link?url=VtAENt2GBYLoYXnTxSfHFxARu9rsQacqT3EMAYitm3bd1nvfngwBKsiasR1lSoluFZlY2G4fQ5wNBnXb7AF6115lPSxz8iTYdbnbNklxAoAjn35Nb3VK2AQd8-bBZrsq
根據(jù)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); 地理圍欄
地理圍欄是指當(dāng)飛機(jī)進(jìn)入、離開某個(gè)特定地理區(qū)域,或在該區(qū)域內(nèi)活動(dòng)時(shí),操作者可以自動(dòng)接收通知和警告。
根據(jù)geofence_result.geofence_action的情況分別采取如下動(dòng)作：none、warn、主狀態(tài)轉(zhuǎn)變?yōu)閘oiter、主狀態(tài)轉(zhuǎn)變?yōu)閞tl、armed.force_failsafe = true。
更新geofence_loiter_on、geofence_rtl_on，如果不在loiter/rtl模式或手動(dòng)轉(zhuǎn)換到loiter/rtl模式時(shí)，該標(biāo)志位復(fù)位。
如果緊急動(dòng)作激活了&&在rtl模式之前處于手動(dòng)和協(xié)助模式下，此時(shí)遙控?fù)u桿撥動(dòng)了，那么回到rtl模式之前的模式
ORB_ID(geofence_result)來自src/drivers/navigator/navigator_main.cpp
[21]根據(jù)_mission_result.flight_termination和counter(記錄commander循環(huán)次數(shù))發(fā)送警告信息。
根據(jù)_mission_result.valid通過tune_mission_fail()和tune_mission_ok()指示操作者。_mission_result.instance_count是任務(wù)的實(shí)例數(shù)，任務(wù)改變時(shí)單調(diào)增加。只有在確定home點(diǎn)之后才評(píng)估任務(wù)完成的狀態(tài)，這樣可以避免任務(wù)拒絕的誤報(bào)。
[22]遙控輸入的處理
①上鎖解鎖
根據(jù)是否有操作者上鎖或解鎖的命令(可以是遙控?fù)u桿左下/右下，檔位開關(guān))和旋翼機(jī)是否在MANUAL, Rattitude, or AUTO_READY 模式下(固定翼是否在land模式下)，判斷是否運(yùn)行arming_state_transition()函數(shù)以完成鎖定狀態(tài)status->arming_state的更新。
②根據(jù)遙控模式選擇開關(guān)評(píng)估飛機(jī)的主狀態(tài)
transition_result_t main_res = set_main_state_rc(&status);
如果手動(dòng)模式設(shè)定沒有更改，那么不運(yùn)行手動(dòng)模式設(shè)定更改了的代碼，只需要_last_sp_man結(jié)構(gòu)體=sp_man結(jié)構(gòu)體，因?yàn)榕袛嗪髸?huì)運(yùn)行return TRANSITION_NOT_CHANGED(函數(shù)中無論什么地方，遇到return函數(shù)就結(jié)束了)；
sp_man.offboard_switch開關(guān)如果打開了，那么運(yùn)行main_state_transition()函數(shù)，飛機(jī)主狀態(tài)轉(zhuǎn)變?yōu)閏ommander_state_s::MAIN_STATE_OFFBOARD；
sp_man.return_switch開關(guān)如果打開了，那么運(yùn)行main_state_transition()函數(shù)，飛機(jī)主狀態(tài)轉(zhuǎn)變?yōu)閏ommander_state_s::MAIN_STATE_AUTO_RTL，如果轉(zhuǎn)變?yōu)閞tl，那么降級(jí)轉(zhuǎn)變?yōu)閏ommander_state_s::MAIN_STATE_AUTO_LOITER；
如果sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE，那么利用main_state_transition()函數(shù)將飛機(jī)主狀態(tài)轉(zhuǎn)變?yōu)開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]);

如果轉(zhuǎn)變不了，那么降級(jí)轉(zhuǎn)變

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開關(guān)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) 時(shí)，由sp_man.acro_switch、sp_man.rattitude_switch兩個(gè)開關(guān)選擇模式
sp_man.mode_switch= SWITCH_POS_MIDDLE(ASSIST)時(shí)，由sp_man.posctl_switch選擇模式
sp_man.loiter_switch= SWITCH_POS_ON(AUTO) 時(shí)，由sp_man.loiter_switch選擇模式
里面的邏輯為，其中也包含了降級(jí)策略(如果不能轉(zhuǎn)變成想要的狀態(tài)，那么轉(zhuǎn)變成與其最接近的狀態(tài)，還不能轉(zhuǎn)變，再下一級(jí))

set_main_state_rc ()返回TRANSITION_DENIED、TRANSITION_CHANGED 、TRANSITION_NOT_CHANGED，并且根據(jù)遙控器指令將internal_state->main_state賦予新的狀態(tài)。
根據(jù)set_main_state_rc()是否能轉(zhuǎn)變飛行模式，發(fā)送指示給操作者。
檢測油門鎖定開關(guān)(Set to true if actuators are forced to being disabled (due to emergency or HIL))，根據(jù)檢測sp_man.kill_switch 狀態(tài)，確定armed.manual_lockdown狀態(tài)。
③若遙控信息丟失，那么status.rc_signal_lost = true，記錄下當(dāng)前的時(shí)間戳rc_signal_lost_timestamp = sp_man.timestamp;
[23]檢測無線數(shù)傳的數(shù)據(jù)鏈?zhǔn)欠襁B接
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {…}循環(huán)四次，輸出have_link狀態(tài)量，根據(jù)have_link 確定status.data_link_lost狀態(tài)量
循環(huán)里if(telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6)符合該條件表示數(shù)據(jù)鏈連接了
循環(huán)里if (telemetry_lost[i] && hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) 符合該條件表示上次數(shù)據(jù)鏈?zhǔn)菙嚅_的
[24] orb_check(actuator_controls_sub, &updated);
針對(duì)固定翼機(jī)型檢測發(fā)動(dòng)機(jī)是否有故障。
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)
旋翼機(jī)型中
_actuators_id = ORB_ID(actuator_controls_0);
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
vtol機(jī)型中
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
[25]orb_check(cmd_sub, &updated);
ORB_ID(vehicle_command)來自或也就是接收數(shù)傳指令，遠(yuǎn)距離在地面站中傳輸操作者指令

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:

每個(gè)case是一個(gè)大類，同時(shí)根據(jù)傳入的cmd->param，調(diào)用main_state_transition()轉(zhuǎn)變飛行模式，轉(zhuǎn)變結(jié)果賦值給cmd_result，最后調(diào)用answer_command(*cmd, cmd_result, *command_ack_pub, *command_ack)回應(yīng)操作者。
[26]如果數(shù)據(jù)鏈(無線數(shù)傳)和GPS工作故障了，并且飛行器不在手動(dòng)模式(MAIN_STATE_MANUAL、MAIN_STATE_ACRO、MAIN_STATE_RATTITUDE、MAIN_STATE_STAB、MAIN_STATE_ALTCTL、MAIN_STATE_POSCTL)那么結(jié)束飛行
如果遙控和GPS工作故障了，并且飛行器在手動(dòng)模式，那么結(jié)束飛行
[27]commander_set_home_position()
這個(gè)函數(shù)初始化飛行器的home點(diǎn)的位置，此函數(shù)運(yùn)行在第一次得到fix的GPS信號(hào)時(shí)和每次解鎖時(shí)(并且此時(shí)GPS為fix狀態(tài))
[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)

status：導(dǎo)航狀態(tài)，飛行器應(yīng)該進(jìn)入的狀態(tài)；
?internal_state：主狀態(tài)，用戶想要的狀態(tài)；
?data_link_loss_enabled：與地面站的數(shù)據(jù)鏈丟失；
?mission_finished：任務(wù)完成；
?stay_in_failsafe：故障保護(hù)；
?status_flags：commander內(nèi)部的狀態(tài)標(biāo)志；
?landed：著陸；
?rc_loss_enabled：遙控信號(hào)丟失；
?offb_loss_act：
?offb_loss_rc_act：

檢查故障和internal_state->main_state，按照降級(jí)策略，給status->nav_state賦狀態(tài)status->nav_state用于navigator.cpp。
一般情況為：
①global位置有效 && 起飛點(diǎn)(home)有效 && GPS有效
進(jìn)入NAVIGATION_STATE_AUTO_**RCRECOVER**模式
②local位置有效 && GPS有效
進(jìn)入NAVIGATION_STATE_AUTO_**LAND**模式(自動(dòng)降落)
③local高度有效
進(jìn)入NAVIGATION_STATE_**DESCEND**模式(無位置控制的下降模式)
④終止飛行
進(jìn)入NAVIGATION_STATE_**TERMINATION**模式
[29]set_control_mode()
根據(jù)status->nav_state，確定control_mode結(jié)構(gòu)體的狀態(tài)，用于mc_att_control.cpp和mc_pos_control.cpp
[30]最后做一些指示信號(hào)、發(fā)布一些主題就可以終止commander進(jìn)程了。

總結(jié)

以上是生活随笔為你收集整理的pixhawk px4 commander.cpp的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。

上一篇：植保飞控AB点功能
下一篇： pixhawk commander.cp