當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

pixhawk px4 commander.cpp

發布時間：2024/4/18 编程问答 31 豆豆

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

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

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 logstatus_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&&1prearm_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, &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則跳出本函數；少于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 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;} }

根據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)

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進程了。

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。

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