[작성중]비행제어 오픈소스 분석 - Ardupilot 태스크 스케줄링과 메인루프

Ardupilot 4.0.3의 Copter를 기준으로 본 글을 작성했습니다.

본 글은 Ardupilot의 스케줄링 기법에 대한 문서[1]와 코드를 분석한다.

 

1. Ardupilot Threading 문서 분석

 [1] "Ardupilot의 setup()/loop()구조는 arduino의 고유한 속성에서 기인하는데, 이때문에 Ardupilot이 단일 스레드 시스템이라고 생각할 수 있다. 그러나 사실은 아니다.", "스레딩 접근 방법은 빌드할 보드에 따라 다른데, APM1, APM2 같이 스레드를 지원하지 않는 보드는 간단한 타이머와 콜백으로 이를 처리한다. PX4와 Linux 같은 SW를 탑재한 보드는 실시간성을 가지는 풍부한 POSIX 스레딩 모델을 지원하며, 이를 Ardupilot에서 널리 사용하고 있다"

 Ardupilot에서 이해할 필요가 있는 threading에 대해서 몇 가지 중요한 개념이 있다.

• 타이머 콜백
• HAL/Driver에 특성화된 스레드
• 플랫폼 드라이버에 대한 ardupilot 드라이버
• 플랫폼 특성화된 스레드와 태스크
• AP_Scheduler 시스템
• semaphores
• lockless 데이터 구조

간단한 설명 작성하기

2. 스케줄링 코드 분석

앞서 타겟 보드의 운영 환경에 따라서 Threading 접근 방법이 다르다는 것을 명시하고 있다.

실시간성을 가지는 POSIX 스레딩 모델을 지원하는 보드(PX4, Linux 탑재)는 pthread를 바탕으로,

스레드를 지원하지 않는 보드(APM)는 타이머와 콜백을 바탕으로 태스크를 처리해나간다.

따라서 스레드의 지원 유무에 따른 스케줄러 코드를 분석하고 소 결론을 지어보도록 하자.

2.1. APM 보드의 스케줄러 코드 분석

Ardupilot의 스케줄링 라이브러리 Ardupilot/libraries는 AP_Scheduler.h를 AP_Scheduler.cpp를 참고한다.

AP_Scheduler.h의 생성자 선언 근처를 보면 스케줄러 클래스는 싱글톤 패턴을 사용하여 단 한 인스턴스만 존재하도록 한다.

주 루프의 속도는 Copter 혹은 ArduSub 이라면 400Hz로, 아니라면 50Hz로 설정한다. (L.34~38)

그리고 이에 대한 정보를 등록해둔다.

#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduSub)
#dubefine SCHEDULER_DEFAULT_LOOP_RATE 400
#else
#define SCHEDULER_DEFAULT_LOOP_RATE  50
#endif
 
#define debug(level, fmt, args...)   do { if ((level) <= _debug.get()) { hal.console->printf(fmt, ##args); }} while (0)
 
extern const AP_HAL::HAL& hal;
 
const AP_Param::GroupInfo AP_Scheduler::var_info[] = {
    // @Param: DEBUG
    // @DisplayName: Scheduler debug level
    AP_GROUPINFO("DEBUG",    0, AP_Scheduler, _debug, 0),
 
    // @Param: LOOP_RATE
    // @DisplayName: Scheduling main loop rate
    // @Values: 50:50Hz,100:100Hz,200:200Hz,250:250Hz,300:300Hz,400:400Hz
    AP_GROUPINFO("LOOP_RATE",  1, AP_Scheduler, _loop_rate_hz, SCHEDULER_DEFAULT_LOOP_RATE),
 
    // @Param: OPTIONS
    // @DisplayName: Scheduling options
    AP_GROUPINFO("OPTIONS",  2, AP_Scheduler, _options, 0),
 
    AP_GROUPEND
};

AP_Scheduler는 생성자에서 fast_loop를 입력으로 받고 setup_object_default 함수를 호출하여 등록된 그룹을 설정해준다.

AP_Scheduler::init에서 태스크의 초기화를

AP_Scheduler::loop는 1틱 계산, fast_loop와 run 호출, loop 함수의 실행 시간을 계산한다.

AP_Scheduler::run는 _tasks와 _common_tasks를 실행하는 것을 볼 수 있다.

 

AP_Scheduler::run은 AP_Scheduller::loop에서 호출되어 매 틱마다 돌면서 수행된다.

호출 될 때 마다 등록된 태스크에 대해서 성능 측정기(_perf_counters)를 등록하고 (L.158~168)

모든 태스크에 대해서 (L.170)

어떤 태스크가 가장 최근 실행된 이후로 지난 시간(dt)을 측정하고 (L.173)

그 태스크의 실행 주기(interval_ticks)를 계산한 후 (L.175)

아직 실행할 시간이 안되었으면 다음 순위의 태스크에 대해서 다시 확인해본다. (L179~182)

- 실행되야할 때라면 시작 시간(_task_time_started)를 기록하고 실행(task.function())하고 나서 끝나면 실행 시간을 측정해서(time_taken) 스케줄러 오버런을 검사한다.

- 태스크를 실행하기 전후로 성능측정기에 시작과 끝을 등록한다. (L.204~214)

- 태스크를 실행하고 있고(L.204) 태스크가 끝났음을 기록한다.(L.215)

마지막으로 태스크 정보(태스크 색인, 걸린 시간, 오버런 함?)를 등록하게 된다.(L.235)

 

/*
  run one tick
  this will run as many scheduler tasks as we can in the specified time
 */
void AP_Scheduler::run(uint32_t time_available)
{
    uint32_t run_started_usec = AP_HAL::micros();
    uint32_t now = run_started_usec;
 
    if (_debug > 1 && _perf_counters == nullptr) {
        _perf_counters = new AP_HAL::Util::perf_counter_t[_num_tasks];
        if (_perf_counters != nullptr) {
            for (uint8_t i=0; i<_num_unshared_tasks; i++) {
                _perf_counters[i] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _tasks[i].name);
            }
            for (uint8_t i=_num_unshared_tasks; i<_num_tasks; i++) {
                _perf_counters[i] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _common_tasks[i].name);
            }
        }
    }
    
    for (uint8_t i=0; i<_num_tasks; i++) {
        const AP_Scheduler::Task& task = (i < _num_unshared_tasks) ? _tasks[i] : _common_tasks[i - _num_unshared_tasks];
 
        uint32_t dt = _tick_counter - _last_run[i];
        // we allow 0 to mean loop rate
        uint32_t interval_ticks = (is_zero(task.rate_hz) ? 1 : _loop_rate_hz / task.rate_hz);
        if (interval_ticks < 1) {
            interval_ticks = 1;
        }
        if (dt < interval_ticks) {
            // this task is not yet scheduled to run again
            continue;
        }
 
        /****Some Code********/
 
        // run it
        _task_time_started = now;
        hal.util->persistent_data.scheduler_task = i;
        if (_debug > 1 && _perf_counters && _perf_counters[i]) {
            hal.util->perf_begin(_perf_counters[i]);
        }
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        fill_nanf_stack();
#endif
        task.function();
        if (_debug > 1 && _perf_counters && _perf_counters[i]) {
            hal.util->perf_end(_perf_counters[i]);
        }
        hal.util->persistent_data.scheduler_task = -1;
 
        /****Some Code********/
 
        perf_info.update_task_info(i, time_taken, overrun);
 
    /****Some Code********/
 
    }
}

 

Ardupilot의 태스크 스케줄링 방법을 대충 분석해 본 결과는 다음과 같다.

가장 빠른 스케줄러 루프가 존재하고

해당 루프가 우선순위에 맞추어 실행 주기가 등록된 태스크의 실행 여부를 확인한다.

 

그러면 이 스케줄러 루프가 실행해야 할 태스크들은 어디에 등록되어있을까?

이는 Copter.cpp와 AP_vehicle.cpp를 참고하도록 하자.

 

2.2. APM Copter의 주 루프 분석

주 루프의 구조는 다음과 같다.

// Main loop - 400hz
void Copter::fast_loop()
{
    // update INS immediately to get current gyro data populated
    ins.update();
 
    // run low level rate controllers that only require IMU data
    attitude_control->rate_controller_run();
 
    // send outputs to the motors library immediately
    motors_output();
 
    // run EKF state estimator (expensive)
    // --------------------
    read_AHRS();
 
#if FRAME_CONFIG == HELI_FRAME
    update_heli_control_dynamics();
    #if MODE_AUTOROTATE_ENABLED == ENABLED
        heli_update_autorotation();
    #endif
#endif //HELI_FRAME
 
    // Inertial Nav
    // --------------------
    read_inertia();
 
    // check if ekf has reset target heading or position
    check_ekf_reset();
 
    // run the attitude controllers
    update_flight_mode();
 
    // update home from EKF if necessary
    update_home_from_EKF();
 
    // check if we've landed or crashed
    update_land_and_crash_detectors();
 
#if HAL_MOUNT_ENABLED
    // camera mount's fast update
    camera_mount.update_fast();
#endif
 
    // log sensor health
    if (should_log(MASK_LOG_ANY)) {
        Log_Sensor_Health();
    }
 
    AP_Vehicle::fast_loop();
}

메인 루프의 과정은 다음과 같다.

1) INS에서 관성센서(가속도계/각속도계)를 업데이트 한다.

ins.update() -> (AP_IntertialSensor.cpp, L.1345) _backend[i]->update()

2) 자세 각속도 제어 연산을 수행한다.

3) 각속도 제어 명령을 바탕으로 모터에 제어 명령을 출력한다.

4) EKF 상태 추정기를 실행시켜서 AHRS 연산을 하고 데이터를 가져온다.

5) 관성 항법 정보를 읽어온다.

6) 4의 EKF가 초기화되었는지 확인한다.

7) 비행 모드를 갱신해주고 (모드 별로 다른 제어 연산 수행)

8) EKF 연산을 바탕으로 홈 포인트를 업데이트 한다.

9) 착륙/충돌 감지 연산을 한다.

*) 마지막으로 AP_Vehicle의 메인루프를 돈다.

 

여기서 보면 태스크 등록이 어디 되는지 알 수가 없다. 태스크 등록은 AP_Vehicle의 setup에서 진행된다. 태스크는 아래에서 선언된다.

AP_Vehicle.cpp의 AP_Vehicle::scheduler_tasks[]

Copter.cpp의 Copter::scheduler_tasks[]

태스크를 가져오는 함수는 다음과 같다.

AP_Vehicle.cpp의 AP_Vehicle::get_common_scheduler_tasks

Copter.cpp의 Copter::get_scheduler_tasks

이 함수들은 여기서 호출된다.

AP_Vehicle::get_common_scheduler_tasks는 AP_Scheduler.cpp에서 AP_Scheduler::init 에서

Copter::get_scheduler_tasks는 AP_Vehicle.cpp에서 AP_Vehicle::setup에서 실행된다.

 

이러한 구조를 봤을 때, 코드 레벨은 다음으로 나누면 좋을 것 같다.

Relativly HIGH : Copter

Relativly MID : AP_Vehicle

Relativly LOW: AP_Scheduler

 

 

scheduler_tasks에 대한 함수는 Copter.cpp의 fast_loop 아래로 쭉 선언되어있으며

common_scheduler_tasks에 대한 함수는 특정 클래스 혹은 아래쪽에 선언되어있다.

 

기체 별 공통적으로 사용하는 태스크는

AP_RunCam, AP_GyroFFT, AP_VideoTX, update_dynamic_notch, sendwatchdog_reset_statustext가 있다.

자세에 대한 주파수 해석이나, 카메라 촬영과 송신, 다이나믹 노치, 와치독 알림 등인데,

자세에 대한 주파수 해석을 하는 이유는 update_dynamic_notch 함수에서 이를 사용하고 있었다.[2]

이는 모터 RPM이나 헬리콥터의 RPM 등의 노이즈 소스가 자이로 센서에 영향을 주기 때문에 이를 처리하기 위한 함수라고 한다.[3]

2.3 소 결론

AP_Scheduler의 Threading은 비선점형 스케줄링이며 우선순위 스케줄링을 사용한다.

또한 APM을 대상으로 하기 때문에 SW 아키텍쳐는 수퍼루프 아키텍쳐 (Super-loop Architecture)를 사용한다.

 

3.1. POSIX를 지원하는 보드의 스케줄러 코드 분석

 

작성하기

 

3.2. 스레드 루프 분석하기

 

작성하기

 

3. 결론

Ardupilot의 태스크 스케줄링/스레딩은..

1) AP_Scheduler는 우선순위 스케줄링 기법인 것 같다.

운영 환경은 수퍼 루프 아키텍쳐를 사용하고 있다고 생각된다.

2) 실시간성을 가지는 POSIX를 지원하는 OS(Chibi, Linux) 가 올라가면 AP_HAL에 선언된 가상함수를 통해 외부에서 접근할 수 있고, 각각의 함수들은 AP_HAL의 Scheduler클래스를 상속받아서 보드-특성화되어 함수를 정의하고 이를 호출한다.

 

[1] Ardupilot Theading, ardupilot.org/dev/docs/learning-ardupilot-threading.html

[2] Harmonic notch filter, ardupilot.org/copter/docs/common-imu-notch-filtering.html

[3] SuperLoop Architexture, locolabs.com/how-to-choose-the-right-firmware-architecture-for-your-iot-device-2/

[4] Apprendix 1, Comparison of Commands with Other SDKs, link-springer-com-443.webvpn.jnu.edu.cn/chapter/10.1007%2F978-1-4842-5531-5_7

끝