PX4 비즈니스 로직 읽기

PX4 동작 흐름을 이해할려면 어떤 코드를 읽어야 할까요? 답은 Commander 코드 입니다.

Commander App은 비행 명령과 비행체 정보를 모아 수행할 명령을 결정하고, 다른 App에게 명령을 내립니다. 즉 Commander App은 PX4의 비즈니스 로직입니다. 결국 PX4의 동작 흐름을 이해하는데는 Commander App 코드를 읽는게 핵심이겠죠?

저는 PX4의 App 코드를 볼때 다음 내용을 읽습니다.

  1. 구독(Subscribe) 또는 출판(Publish)하는 uORB 토픽
  2. 동작 시나리오를 하나 정해서 시나리오 실행에 따라 순서대로 코드 읽기

Commander App

코드: http://subak.io/code/px4/Firmware/src/modules/commander/commander.cpp.html

PX4 App들의 지휘관! 미션, 센서, RC의 상태에 대한 토픽을 읽어서 비행제어 토픽으로 전달.

  • 비행모드 전환 및 수행지시 (상태머신으로 구현)
  • 파라미터 업데이트 (parameter_update)
  • RC입력 읽기(manual_control_setpoint)
  • 센서값(sensor_combined) 읽기
  • 시스템 파워 관리: 파워를 선택
  • Safety 스위치(safety) 체크
  • 기체상태 관리(vtol_vehicle_status)
  • 기체 위치(vehicle_global_position)
  • 기체 로컬 위치(vehicle_local_position)
  • 기체 자세(vehicle_attitude)
  • 기체 착륙 여부(vehicle_land_detected)
  • cpuload, batterystatus
  • 그외…

Commander App은 PX4의 주요 uORB 토픽을 관리합니다. 그래서 Commander App이 구독 또는 출판하는 uORB 토픽를 읽고 이와 관련된 주변의 App 코드를 읽으면 이 지휘관이 하는일의 큰그림이 그려집니다.

TODO: 주변의 App과 연결관계 그림 추가

Published Topic

다음은 Commander App에서 출판하는 토픽입니다.

  • armed_pub
  • commander_state_pub
  • homePub
  • led_control_pub
  • mission_pub
  • command_ack_pub
  • control_mode_pub
  • roi_pub
  • status_pub
  • status_pub
  • vehicle_status_flags_pub

코드

출판하는 토픽의 코드 위치 입니다. commander앱 디렉토리에서 orb_publish 함수로 검색하면 찾는데 도움이 됩니다.

cd Firmware/src/modules; ag "orb_publish" |grep ^commander

commander/commander_helper.cpp:332:    orb_publish(ORB_ID(led_control), led_control_pub, &led_control);
commander/commander.cpp:1024:                    orb_publish(ORB_ID(home_position), *home_pub, home);
commander/commander.cpp:1129:            orb_publish(ORB_ID(vehicle_roi), *roi_pub, roi);
commander/commander.cpp:1240:        orb_publish(ORB_ID(home_position), homePub, &home);
commander/commander.cpp:1494:        orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
commander/commander.cpp:3053:            orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
commander/commander.cpp:3056:            orb_publish(ORB_ID(vehicle_status), status_pub, &status);
commander/commander.cpp:3072:            orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
commander/commander.cpp:3153:            orb_publish(ORB_ID(commander_state), commander_state_pub, &internal_state);
commander/commander.cpp:3992:        orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
commander/commander.cpp:4434:        orb_publish(ORB_ID(vehicle_status_flags), vehicle_status_flags_pub, &v_flags);
commander/state_machine_helper.cpp:516:        orb_publish(ORB_ID(vehicle_status), status_pub, current_status);

Subscribed Topic

다음은 Commander App에서 구독하는 토픽입니다. PX4의 왠만한 토픽은 다 구독합니다.

  • _VEHICLE_ATTITUDE_CONTROLS
  • battery_status
  • cpuload
  • differential_pressure
  • estimator_status
  • geofence_result
  • manual_control_setpoint
  • mission_result
  • offboard_control_mode
  • parameter_update
  • position_setpoint_triplet
  • safety
  • sensor_accel
  • sensor_combined
  • sensor_correction
  • sensor_gyro
  • sensor_mag
  • sensor_preflight
  • subsystem_info
  • system_power
  • telemetry_status
  • vehicle_attitude
  • vehicle_command
  • vehicle_global_position
  • vehicle_gps_position
  • vehicle_land_detected
  • vehicle_local_position
  • vehicle_status
  • vtol_vehicle_status

코드

구독한 토픽의 코드 위치 입니다. orb_copy 함수로 검색하면 찾는데 도움이 됩니다.

cd Firmware/src/modules; ag "orb_copy" |grep ^commander

commander/accelerometer_calibration.cpp:318:            orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction);
commander/accelerometer_calibration.cpp:486:        orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);
commander/accelerometer_calibration.cpp:491:        (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
commander/accelerometer_calibration.cpp:521:            (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
commander/accelerometer_calibration.cpp:590:    if (orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction) != 0) {
commander/accelerometer_calibration.cpp:613:                    orb_copy(ORB_ID(sensor_accel), subs[s], &arp);
commander/accelerometer_calibration.cpp:785:        orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
commander/airspeed_calibration.cpp:140:            orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
commander/airspeed_calibration.cpp:221:            orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
commander/calibration_routines.cpp:563:            orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor);
commander/calibration_routines.cpp:838:        orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd);
commander/esc_calibration.cpp:68:    orb_copy(ORB_ID(battery_status), batt_sub, &battery);
commander/esc_calibration.cpp:101:    orb_copy(ORB_ID(battery_status), batt_sub, &battery);
commander/esc_calibration.cpp:156:                orb_copy(ORB_ID(battery_status), batt_sub, &battery);
commander/gyro_calibration.cpp:86:    if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) {
commander/gyro_calibration.cpp:116:            orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction);
commander/gyro_calibration.cpp:128:                    orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report);
commander/gyro_calibration.cpp:137:                        orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0);
commander/gyro_calibration.cpp:287:        orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report);
commander/gyro_calibration.cpp:381:                    orb_copy(ORB_ID(sensor_correction), worker_data.sensor_correction_sub, &sensor_correction);
commander/commander.cpp:606:    orb_copy(ORB_ID(vehicle_status), state_sub, &state);
commander/commander.cpp:1745:            orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
commander/commander.cpp:1851:            orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
commander/commander.cpp:1857:            orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode);
commander/commander.cpp:1904:                orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry);
commander/commander.cpp:1949:            orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
commander/commander.cpp:1980:            orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
commander/commander.cpp:1986:            orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
commander/commander.cpp:2025:            orb_copy(ORB_ID(safety), safety_sub, &safety);
commander/commander.cpp:2067:            orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
commander/commander.cpp:2088:            orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos);
commander/commander.cpp:2096:                    orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
commander/commander.cpp:2100:                    orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
commander/commander.cpp:2110:            orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
commander/commander.cpp:2118:            orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
commander/commander.cpp:2167:            orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
commander/commander.cpp:2226:            orb_copy(ORB_ID(cpuload), cpuload_sub, &cpuload);
commander/commander.cpp:2233:            orb_copy(ORB_ID(battery_status), battery_sub, &battery);
commander/commander.cpp:2321:            orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
commander/commander.cpp:2356:            orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
commander/commander.cpp:2395:            orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
commander/commander.cpp:2445:            orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result);
commander/commander.cpp:2461:            orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
commander/commander.cpp:2867:            orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
commander/commander.cpp:2929:            orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
commander/commander.cpp:4054:            orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
commander/mag_calibration.cpp:388:            orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro);
commander/mag_calibration.cpp:445:                    orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);
commander/mag_calibration.cpp:586:            orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &mag_report);
commander/PreflightCheck.cpp:169:    orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors);
commander/PreflightCheck.cpp:374:    if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
commander/PreflightCheck.cpp:431:        if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) ||
commander/PreflightCheck.cpp:455:    orb_copy(ORB_ID(estimator_status), sub, &status);
commander/rc_calibration.cpp:67:    orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);

Commander 동작 시나리오 : Change flight mode

PX4에서 Flight mode를 바꾸는 시나리오는 두가지가 있습니다.

  1. RC Input(RC 트랜스미터의 RTL 스위치) 받아서 RTL Flightmode로 전환
  2. QGroundControl(MAVLINK Command)을 통한 RTL Flightmode 전환

RC INPUT -> RTL

RC Input(manual_control_setpoint)토픽을 읽어서 RTL mode(vehicle_status) 전환

실행 순서

-> commander_thread_main(int argc, char *argv[])
  -> set_main_state_rc(struct vehicle_status_s *status_local);
   -> main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state);

함수

  • commander_thread_main() https://goo.gl/rzG6Th 역할: commander 메인 쓰레드! px4의 기체 동작은 commander_thread_main() 에서 시작한다!

  • set_main_state_rc() https://goo.gl/zFS2fM 역할: RC컨트롤러 (manual_control_setpoint)으로 기체 상태를 수정. 주로 flightmode 전환

  • main_state_transition() https://goo.gl/1ZNASU 역할: 비행 상태를 읽어 Flight mode를 바꿀 수 있으면 바꾼다

MAVLINK Command -> RTL

MAVLINK Command 관련 토픽 (vehicle_command) 읽어서 RTL mode(vehicle_status)로 전환

실행 순서

-> commander_thread_main(int argc, char *argv[])
  -> 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)

    -> main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, main_state_prev, &status_flags, &internal_state);

함수

  • handle_command() https://goo.gl/kk4h2v 역할: vehicle_command 처리. 커맨드 실행하고 실행결과를 vehicle_command_ack 에 전달