PX4 Firmware 코드 읽기

대상

  • px4 코드 이해할려고 하는 분

Tech Map

PX4 이해를 위해 필요한 기술

  • control
  • embedded programming
  • cpp
  • nuttx

사전

  • px4_overview, px4_hardware, nuttx101

PX4 doxygen

https://pixhawk.ethz.ch/docs/files.html

System

Flight mode

http://dev.px4.io/concept-flight-modes.html

Architecture

http://dev.px4.io/concept-architecture.html

Flight stack

http://dev.px4.io/concept-flight-stack.html

Middleware, uORB

uORB(micro Object Request Broker)

uORB Messaging (pub/sub)

센서의 값주고 받고!

http://dev.px4.io/advanced-uorb.html

주로 탑재하는 센서들의 디바이스 드라이버와 비행 제어를 실행하는 어플리케이션에 이 센서를 연결하기 위해서 미들웨어는 publish-subscribe로 구성된다.

  • App간 통신, px4 Middle ware,
  • publish() / subcribe() messaging API.
  • 반응형 시스템
  • 병렬성
  • thread-safe

uORB 용어 정리

  • node: 프로세스
  • topic: 메시지 버스
  • advertising: publish하기전에 알리기.
  • publish: 메시지 보내기
  • subscribing: 구독

uORB

uORB 사용 순서

토픽 Publishing 순서

  1. 토픽 선언 defining the topic,
  2. 토픽 핸들 만들기 advertising the topic (publish 되기전에 한번은 advertising해야함)
  3. 토픽 업데이트 publishing updates.

토픽 Subscribing

  1. 토픽 아이디 정의 ORB_DEFINE
  2. 토픽 업데이트 체크 Checking for Updates
  3. 업데이트된 토픽을의 값을 복사하여 사용 Copying Data from a Topic

관련함수

  • ORB_DEFINE/ ORB_DECLARE
  • ORB_ID
  • orb_subscribe
  • orb_advertise
  • orb_publish

Firmware 디렉토리 구조

해보기

  • https://github.com/PX4/Firmware/ 을 읽어보자.
  • 각 디렉토리의 역활은?
  • /etc/init.d/ 디렉토리를 찾아보자.
  • uORB 모듈은 어디에?
  • multicopter position 제어 앱?

Read uORB Code

시작 https://github.com/PX4/Firmware/blob/master/src/modules/uORB/uORBMain.cpp

        g_dev = uORB::Manager::get_instance()->get_device_master(uORB::PUBSUB);

Read App Code

PX4 Apps

px4_app

어떤 순서로 읽는가?

  • 부팅 순서 px4_boot
  • 앱 (테스크)
    • 통신(publish/subscribe pattern)
    • 시나리오로 읽기
  • github issue 하나 잡아서 구현 또는 버그fix commit
  • drivers는 인터페이스 읽고 사용예 찾아보기

APP 코드 읽기.

  1. 읽으려는 기능과 관련된 메인 토픽 찾기. (subscribe, publish)
  2. 읽으려는 기능의 시나리오 정해서 실행순서대로 코드 읽기.
  3. 그외에 조연 토픽읽기.

mc_att_control app 읽어보자.

읽을 시나리오: RC input 데이터를 받아서 드론의 자세 제어(att_control)

mc_att_control app uORB Topics

Published messages

  • ... Subscribed messages
  • ...

시나리오 관련 토픽1

읽으려는 기능: 조정기 수신기에서 조정데이터를 받아서드론의 자세제어를 해보자.

  1. 읽으려는 기능의 모듈에서 관련된 토픽 찾기.

multicopter 자세제어. 첫시작? mc_att_control 모듈


https://github.com/PX4/Firmware/blob/master/src/modules/mc_att_control/mc_att_control_main.cpp

/Firmware/src/modules/mc_att_control/mc_att_control_main.cpp

사용하는 토픽 
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
#include <uORB/topics/actuator_controls_virtual_mc.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/mc_att_ctrl_status.h>

시나리오 관련 토픽 2

메인 ORB topics 찾기

기능의 앱코드에서 ORB_ID 키워드 검색해서 토픽을 찾는다.

subscriptions

  • ORB_DECLARE(manual_control_setpoint); // Manual RC 입력

publications

  • ORB_DECLARE(actuator_controls_0); // 모터 제어

관련 코드 1

사용하는 토픽과 연결된 다른 앱의 코드를 찾는다.

  • manual_control_setpoint
  • actuator_controls_0 
관련코드 찾기
ag manual_control_setpoint |grep ORB_ID |grep orb_publish

ag actuator_controls_0 |grep ORB_ID |grep orb_subscribe

➜  Firmware git:(master) ✗  ag manual_control_setpoint |grep ORB_ID  |grep orb_publish
src/modules/mavlink/mavlink_receiver.cpp:1456:      orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
src/modules/sensors/sensors.cpp:2216:       orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);

src/modules/sensors/sensors.cpp:2216 앞을 읽어본다.

해보기

  • manual_control_setpoint는 rc 입력에서 받기 위해 또다른 토픽을 구독한다. rc 입력을 직접 관장하는 토픽은 무엇인지 찾아보자. 
topic declear
 Firmware/build_px4fmu-v2_default/src/modules/uORB/topics/

src/modules/sensors/sensors.cpp:2493       _rc_sub = orb_subscribe(ORB_ID(input_rc));

해보기

  • actuator_controls_0는 모터제어를 하는 토픽이다. actuator_controls_0이 모터를 제어하는 흐름을 찾아보자.
  • mc_att_control 실행을 순서도로 표현해보자.

actuator_controls_0 -> motor

ag actuator_controls_0 |grep ORB_ID |grep orb_subscribe

src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp
    orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);

src/drivers/px4io/px4io.cpp:
    orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
    orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &motor_limits);

mc_att_control 순서도

PX4의 Attitude rates controller 코드를 읽어보자

MulticopterAttitudeControl::control_attitude_rates(float dt)

https://gist.github.com/donghee/e900bed6623e4dca42b49fa5f6746493

PX4의 Attitude controller 코드를 읽어보자

void
MulticopterAttitudeControl::control_attitude(float dt)

https://gist.github.com/donghee/47861a70fb1df25afef51c87d898ef07

PX4의 Sensor Fusion 코드를 읽어보자.

https://github.com/PX4/Firmware/blob/master/src/modules/ekf2/ekf2_main.cpp

_ekf.setIMUData() // 1. Set accel, gyro 

_ekf.update() // 2. Fusion. Ref: https://github.com/PX4/ecl/blob/master/EKF/ekf.cpp#L197  https://github.com/PX4/ecl/blob/master/EKF/ekf.cpp#L553

_ekf.copy_quaternion(q.data()); // 3. Get attitude
// Attitude quaternion
ctrl_state.q[0] = q(0);
ctrl_state.q[1] = q(1);
ctrl_state.q[2] = q(2);
ctrl_state.q[3] = q(3);

참고:

px4 코드 읽기. 다시!

  1. 읽으려는 기능과 관련된 메인 토픽 찾기. (subscribe, publish)
  2. 읽으려는 기능의 시나리오 정해서 실행순서대로 코드 읽기.
  3. 그외에 조연 토픽읽기.
  4. 토픽데이터를 계산하는 코드 읽기.
  5. 잘 이해안가면px4 용어와 컨셉을 다시 체크한다.

주요앱 분석: mc_pos_control

APP 실행 순서대로 읽어보자.

mc_pos_control

/Firmware/ROMFS/px4fmu_common/init.d/rc.mc_apps

mc_pos_control start

/Firmware/src/modules/mc_pos_control/mc_pos_control_main.cpp
  1. start()
  2. task_main_trampoline()
  3. task_main() subscribe topic
    • control_manual() start
    • control_offboard() start
    • control_auto() start
  4. task_main() topic publish
    • vehicle_global_velocity_setpoint
    • vehicle_local_position_setpoint
    • _attitude_setpoint_id

Making Apps

APP 만들기.

  1. 기존의 예제 앱을 변형 한다.
  2. 새로운 앱 만들기
    1. 토픽을 연결한다.
    2. 토픽이 없으면 새로운 토픽을 만든다.
    3. 드라이버가 없으면 드라이버를 만든다.

PX4 Daemon App

https://github.com/PX4/Firmware/blob/master/src/examples/px4_daemon_app/px4_daemon_app.c

px4_code-daemon_app

PX4 AUXOUT1의 신호선과 GND사이에 LED를 연결하자.

/Firmware/src/modules/gpio_led/gpio_led.c

을 실행하자.

해보기

  • gpio_slow_led 앱을 추가하여 gpio_led 앱에서 실행하는 것보다 10배 천천히 LED가 제어되도록 만들어 보자. (힌트 src/modules/gpio_slow_led, cmake/configs/nuttx_px4fmu-v2_default.cmake

참고