기존 PX4FLOW 예제의 부실함

먼저 기존 예제를 살펴보자

ardupilot/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test

#include <AP_AHRS/AP_AHRS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h>
#include <AP_RangeFinder/AP_RangeFinder.h>

const AP_HAL::HAL& hal = AP_HAL::get_HAL();

class DummyVehicle {
public:
    AP_GPS gps;
    AP_Baro barometer;
    Compass compass;
    AP_InertialSensor ins;
    AP_SerialManager serial_manager;
    RangeFinder sonar {serial_manager};
    AP_AHRS_NavEKF ahrs{ins, barometer, gps, sonar, EKF, EKF2,
                        AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};
    NavEKF EKF{&ahrs, barometer, sonar};
    NavEKF2 EKF2{&ahrs, barometer, sonar};
};

static DummyVehicle vehicle;
static OpticalFlow optflow(vehicle.ahrs);

void setup()
{
    hal.console->println("OpticalFlow library test ver 1.6");

    hal.scheduler->delay(1000);

    // flowSensor initialization
    optflow.init();

    if (!optflow.healthy()) {
        hal.console->print("Failed to initialise PX4Flow ");
    }

    hal.scheduler->delay(1000);
}

void loop()
{
    hal.console->println("this only tests compilation succeeds");

    hal.scheduler->delay(5000);
}

AP_HAL_MAIN();

뭐, 예제를 보면 알겠지만 센서를 읽어서 확인하는 부분은 없다. 단지 초기화를 한번 할 뿐이다.
이 예제의 목적은 어디까지나 "컴파일이 되는지 안되는지" 확인하기 위한 것으로 그다지 실용성이 없다.
(도대체 왜 이런 예제밖에 안만들었을까)
(사실 굳이 밖에서 날리지 왜 안에서 날리려고 opticalflow 센서를 쓰느냐는 무언의 압박)
(그래도 난 쓰고싶은데?)

그래서 직접 센서값을 읽어오는 과정까지 포함하는 예제를 만들기로 했다.

waf 빌드시스템 이용을 위한 준비

먼저 waf는 파이썬을 이용해서 빌드 시스템을 구성하는 사람이 어떤 경로를 찾아 빌드할 지 결정할 수 있다.

ardupilot/wscript

278 def _build_recursion(bld):
279     common_dirs_patterns = [
280         # TODO: Currently each vehicle also generate its own copy of the
281         # libraries. Fix this, or at least reduce the amount of
282         # vehicle-dependent libraries.
283         '*',
284         'Tools/*',
285         'libraries/*/examples/*',
286         'libraries/*/tests',
287         'libraries/*/utility/tests',
288         'libraries/*/benchmarks',
289     ]
290 
291     common_dirs_excl = [
292         'modules',
293         'libraries/AP_HAL_*',
294         'libraries/SITL',
295     ]
296 
297     hal_dirs_patterns = [
298         'libraries/%s/*/tests',
299         'libraries/%s/*/benchmarks',
300         'libraries/%s/examples/*',
301     ]

대충 위에 적힌 경로에 적당히 타 예제들과 비슷하게 경로를 맞춰주면 알아서 잡아준다.
그래서 본인은 다음과 같이 AP_OpticalFlow_test 예제가 있는 폴더 옆에 AP_OpticalFlow_test2 라는 예제용 폴더를 만들었다.

이후 아래와 같이 wscript와 예제파일 AP_OpticalFlow_test2.cpp 파일을 만들었다.
nobuild 라는 택스트 파일은 사실 없어도 빌드가 잘 되는것을 보면, 큰 의미는 없어보이지만 다른 예제 폴더에 있길레 일단 넣어줬다.

wscript의 내용은 별거 없다. waf가 이 예제파일을 인식할 수 있도록 넣어주는 파일인 모양이다.

  1 #!/usr/bin/env python
  2 # encoding: utf-8
  3 
  4 def build(bld):
  5     bld.ap_example(
  6         use='ap',
  7     )

위 작업까지 완료 후 ardupilot 메인 폴더에 이동 후,

./waf list

를 해보면

examples/AP_Common 
examples/AP_Compass_test 
examples/AP_Declination_test 
examples/AP_Mission_test 
examples/AP_Notify_test 
examples/AP_OpticalFlow_test 
examples/AP_OpticalFlow_test2 
examples/AP_Parachute_test 
examples/Airspeed 
examples/AnalogIn 
examples/BARO_generic 

위와 같이 내가 세로 만든 AP_OpticalFlow_test2가 빌드할 수 있게 되었음을 알 수 있다.

예제파일 작성

예제파일은 간단히 다음과 같은 내용을 가지고 있다.

• 200Hz로 optical flow 센서 값을 읽어드린다.
• 1Hz로 읽어드린 센서 값을 출력한다.

하려는 내용은 너무너무 간단한데, 개인적으로 너무너무 시간을 많이 썼다.
(다른 예제들 보면서 하는데, 완벽한거 같은데, 동작을 안한다고)
(그래서 잘되는 소스를 똑같이 복붙해서 해봤는데 안되잖아?)
(그래서 USB 뺏다 꼽았다 하니까 되더라고)
(그 이외에 안되는 이유 겁나 많았는데 나중에 알려드림)

AP_OpticalFlow/examples/AP_OpticalFlow_test2

/*
 *  Example of AP_OpticalFlow library.
 *  Code by ChoYG. [email protected]
 */

#include <AP_AHRS/AP_AHRS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_NavEKF2/AP_NavEKF2.h>
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h>
#include <AP_RangeFinder/AP_RangeFinder.h>

#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_BoardConfig/AP_BoardConfig.h>

const AP_HAL::HAL& hal = AP_HAL::get_HAL();

class DummyVehicle {
public:
    void setup(void);
    void loop(void);
private:
    //sensor & filter declaration
    AP_GPS gps;
    AP_Baro barometer;
    Compass compass;
    AP_InertialSensor ins;
    AP_SerialManager serial_manager;
    RangeFinder sonar { serial_manager };
    AP_AHRS_NavEKF ahrs { ins, barometer, gps, sonar, EKF, EKF2,
            AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF };
    NavEKF EKF { &ahrs, barometer, sonar };
    NavEKF2 EKF2 { &ahrs, barometer, sonar };

    OpticalFlow optflow { ahrs };

    AP_Scheduler scheduler;
    static const AP_Scheduler::Task scheduler_tasks[];

    uint8_t flowQuality;
    Vector2f flowRate;
    Vector2f bodyRate;

    void ins_update(void); //for scheduler timing
    void update_optical_flow(void);
    void one_hz_print(void);
};

//for scheduler timing
void DummyVehicle::ins_update(void) {
    ins.update();
}

void DummyVehicle::update_optical_flow(void) {
    static uint32_t last_of_update = 0;

    // read from sensor
    optflow.update();

    // write to log and send to EKF if new data has arrived
    if (optflow.last_update() != last_of_update) {
        last_of_update = optflow.last_update();
        flowQuality = optflow.quality();
        flowRate = optflow.flowRate();
        bodyRate = optflow.bodyRate();
    }
}

void DummyVehicle::one_hz_print(void) {
    hal.console->printf("Quality : %d\n", this->flowQuality);
    hal.console->printf("flowRate.x : %f\n", this->flowRate.x);
    hal.console->printf("flowRate.y : %f\n", this->flowRate.y);
    hal.console->printf("bodyRate.x : %f\n", this->bodyRate.x);
    hal.console->printf("bodyRate.y : %f\n", this->bodyRate.y);
}

static DummyVehicle vehicle;

#define SCHED_TASK(func, _interval_ticks, _max_time_micros) SCHED_TASK_CLASS(DummyVehicle, &vehicle, func, _interval_ticks, _max_time_micros)

const AP_Scheduler::Task DummyVehicle::scheduler_tasks[] = {
        SCHED_TASK(ins_update           ,50     ,1000   ), //for scheduler timing
        SCHED_TASK(update_optical_flow  ,200    ,160    ),
        SCHED_TASK(one_hz_print         ,1      ,1000   ),
};

void DummyVehicle::setup(void) {
    AP_BoardConfig { }.init();

    //setup optical flow sensor
    ahrs.set_optflow(&optflow);
    //if not enabled
    if (!optflow.enabled()) {
        hal.console->println("optical flow not enabled");
    }

    // initialize optical flow sensor
    optflow.init();

    hal.console->println("OpticalFlow library test");

    //setup loop timing
    ins.init(scheduler.get_loop_rate_hz());

    // initialize the scheduler
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
}

void DummyVehicle::loop(void) {
    // wait for an INS sample
    ins.wait_for_sample();

    // tell the scheduler one tick has passed
    scheduler.tick();

    // run all tasks that fit in 20ms
    scheduler.run(20000);
}

void setup() {
    vehicle.setup();
}

void loop() {
    vehicle.loop();
}

AP_HAL_MAIN();

APM이 자체적으로 제공하는 AP_Scheduler를 이용해서 만든 예제다. Task는 총 3개가 있고, 각각 다음과 같은 역할을 한다.

• int_update
  관성센서를 읽어드리는 부분이지만, 전체 loop 도는 주기를 일정하게 맞춰주기 위한 작업도 포함된다.
  여기서는 loop 주기를 맞춰주기 위해 사용했다.
• update_optical_flow
  센서값을 읽어오는 부분이다.
• one_hz_print
  1초마다 읽어드린 센서값을 프린트하는 부분이다.

부분별 설명

 22 class DummyVehicle {
 23 public:
 24     void setup(void);
 25     void loop(void);
 26 private:
 27     //sensor & filter declaration
 28     AP_GPS gps;
 29     AP_Baro barometer;
 30     Compass compass;
 31     AP_InertialSensor ins;
 32     AP_SerialManager serial_manager;
 33     RangeFinder sonar { serial_manager };
 34     AP_AHRS_NavEKF ahrs { ins, barometer, gps, sonar, EKF, EKF2,
 35             AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF };
 36     NavEKF EKF { &ahrs, barometer, sonar };
 37     NavEKF2 EKF2 { &ahrs, barometer, sonar };
 38 
 39     OpticalFlow optflow { ahrs };
 40 
 41     AP_Scheduler scheduler;
 42     static const AP_Scheduler::Task scheduler_tasks[];
 43 
 44     uint8_t flowQuality;
 45     Vector2f flowRate;
 46     Vector2f bodyRate;
 47 
 48     void ins_update(void); //for scheduler timing
 49     void update_optical_flow(void);
 50     void one_hz_print(void);
 51 };

실제 ardupilot에서 멀티콥터는 copter 라는 class를 이용해 센서들을 선언하고 이용한다.
위 예제는 DummyVehicle 틀만 만족하는 class를 만들었는데, 말 그대로 실제로 존제하지않는 vehicle이다.
위 내용은 기존에 있던 AP_OpticalFlow_test에서 사용하던 class를 참고해서 만들었다.

 53 //for scheduler timing
 54 void DummyVehicle::ins_update(void) {
 55     ins.update();
 56 }

ins_update는 말 그대로 관성센서를 업데이트 하는 부분인데, 실질적으로 의미없이 업데이트만 하며 이 값을 프린트 하지는 않는다.
AP_Scheduler 특성상 메인 loop가 몇hz로 반복되는가에 크게 영향을 받는다.
실제로 나도 처음에는 단순히 센서값을 읽어드리는 부분으로 생각해 해당 부분을 사용하지 않았는데, 자꾸 지나치게 빨리 프린트 되는 바람에 찾아보니 꼭 호출해줘야 하는 부분이더라.
(400Hz를 맞추기 위해 꼭 이렇게 해야 했나 싶을 정도로)
(덕분에 2일인가 3일인가 날려먹음)
(ins.update를 꼭 써줘야 한다고 예제같은데 좀 써두라고)
(실제로 주석에는 이 녀석이 loop timing을 결정한다고 되있긴 한다.)
(누가 이걸로 주기를 결정할지 상상이나 했겠냐;)

 58 void DummyVehicle::update_optical_flow(void) {
 59     static uint32_t last_of_update = 0;
 60 
 61     // read from sensor
 62     optflow.update();
 63 
 64     // write to log and send to EKF if new data has arrived
 65     if (optflow.last_update() != last_of_update) {
 66         last_of_update = optflow.last_update();
 67         flowQuality = optflow.quality();
 68         flowRate = optflow.flowRate();
 69         bodyRate = optflow.bodyRate();
 70     }
 71 }

실제로 센서값을 읽어드리는 부분이다.

optflow.update를 호출하면 백엔드의 update가 호출된다. 내가 사용하는 보드의 경우 px4계열 이므로 빌드 타임때 px4flow를 선택해 빌드하도록 되어있다.
업데이트 된 센서값은 private 맴버들에 저장해둔다. 이후 저장된 내용을 프린트 하도록 하기 위한 작업이다.

한가지 짚고 넘어가야 할 내용이 있는데, 해당 내용을 참고한 메인 소스쪽 update_optical_flow 에서는 다음과 같은 확인 작업을 한다.

    if (!optflow.enabled()) {
        return;
    }

언뜻보면 센서값이 준비가 안되면 끝내기 위해 존제하는 듯 보이지만, 사실 파라미터 값을 확인하기 위한 장치이다.
파라미터 값을 보니 기본 설정값이 0인 듯 하다.

const AP_Param::GroupInfo OpticalFlow::var_info[] = {
    // @Param: _ENABLE
    // @DisplayName: Optical flow enable/disable
    // @Description: Setting this to Enabled(1) will enable optical flow. Setting this to Disabled(0) will disable optical flow
    // @Values: 0:Disabled, 1:Enabled
    // @User: Standard
    AP_GROUPINFO("_ENABLE", 0,  OpticalFlow,    _enabled,   0),

현재는 파라미터 값에 상관없이 무조건 연결된 센서값을 읽어드려야 하므로 해당 enabled 를 체크하는 부분을 제거했다.

 73 void DummyVehicle::one_hz_print(void) {
 74     hal.console->printf("Quality : %d\n", this->flowQuality);
 75     hal.console->printf("flowRate.x : %f\n", this->flowRate.x);
 76     hal.console->printf("flowRate.y : %f\n", this->flowRate.y);
 77     hal.console->printf("bodyRate.x : %f\n", this->bodyRate.x);
 78     hal.console->printf("bodyRate.y : %f\n", this->bodyRate.y);
 79 }
 80 

맴버변수로 저장되어 있는 센서값들을 프린트 하는 부분이며, console로 프린트 하게 된다. console은

mavproxy.py --console

와 같이 터미널에 입력하여 띄울 수 있다. 처음에는 그냥 uart로 보내는 줄 알았더니, mavlink 프로토콜을 이용하는 모양이다. 어쨌든 console 창을 열게 되면 다음과 같이 프린트하게 된다.

(이걸 보기위해 1주일을 날려먹었다)
(USB 연결 상태에 따라 되기도 하고 안되기도 한다.)
(그럴땐 한번씩 뺏다 꼽았다 해주고 정 안되면 PC를 리부트 하자)
(특히 지나치게 빠르게 하면 console이 아예 먹통이 된다)
(일단 프린트 되는게 있어야 제대로 되는지 안되는지 확인할 수 있을꺼 아냐;)

 81 static DummyVehicle vehicle;
 82 
 83 #define SCHED_TASK(func, _interval_ticks, _max_time_micros) SCHED_TASK_CLASS(DummyVehicle, &vehicle, func, _interval_ticks, _max_time_micros)
 84 
 85 const AP_Scheduler::Task DummyVehicle::scheduler_tasks[] = {
 86         SCHED_TASK(ins_update           ,50     ,1000   ), //for scheduler timing
 87         SCHED_TASK(update_optical_flow  ,200    ,160    ),
 88         SCHED_TASK(one_hz_print         ,1      ,1000   ),
 89 };

그 후 저 DummyVehicle이란 녀석을 vehicle로 인스턴스화 해준다.

AP_Scheduler에 등록 후 setup과 loop문을 작성하면 알아서 각 Task를 돌려줄 것이다.

 91 void DummyVehicle::setup(void) {
 92     AP_BoardConfig { }.init();
 93 
 94     //setup optical flow sensor
 95     ahrs.set_optflow(&optflow);
 96     //if not enabled
 97     if (!optflow.enabled()) {
 98         hal.console->println("optical flow not enabled");
 99     }
100 
101     // initialize optical flow sensor
102     optflow.init();
103 
104     hal.console->println("OpticalFlow library test");
105 
106     //setup loop timing
107     ins.init(scheduler.get_loop_rate_hz());
108 
109     // initialize the scheduler
110     scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
111 }

AP_BoardConfig { }.init();는 말 그대로 각 보드들의 기본적인 셋업을 하는 부분이며, ahrs.set_optflow(&optflow); 부분과 optflow.init();부분은 optical flow 센서 초기화를 위한 부분이다. 초기화 관련 부분은 매인 소스의 copter 에서 관련 부분의 setup 과정을 찾아 가져왔다.

 97     if (!optflow.enabled()) {
 98         hal.console->println("optical flow not enabled");
 99     }

이 부분은 사실 현제로서는 큰 의미는 없다. 단지 파라미터값이 읽도록 설정이 되있는지 확인만 하는 부분이며, 현재 예제에서는 파라미터 셋팅이 안되있어도 그냥 안되있다고 프린트만 하고 읽는건 계속한다.

106     //setup loop timing
107     ins.init(scheduler.get_loop_rate_hz());
108 
109     // initialize the scheduler
110     scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));

AP_Scheduler 관련 초기화 부분으로 ins.init은 메인 loop가 몇hz로 돌 지 결정해주기 위한 요소중 하나이기 때문에 꼭 써주어야 한다.
(그리고 당연히 이부분에서 꽤나 삽질했다.)

113 void DummyVehicle::loop(void) {
114     // wait for an INS sample
115     ins.wait_for_sample();
116 
117     // tell the scheduler one tick has passed
118     scheduler.tick();
119 
120     // run all tasks that fit in 20ms
121     scheduler.run(20000);
122 }

ins.wait_for_sample();또한 메인 loop의 주기를 결정하는 요소이기 때문에 반드시 넣어줘야 한다.(짜증)
위와 같이 작성한 후 최종적으로 메인 setup과 loop를 작성해준다.

124 void setup() {
125     vehicle.setup();
126 }
127 
128 void loop() {
129     vehicle.loop();
130 }
131 
132 AP_HAL_MAIN();

이제 단순히 컴파일만 확인해보기 위한 예제가 아닌 제대로 센서값까지 읽어드리는 예제가 생겼다.

여담

지나치게 시간 많이씀
잃어버린 1주일
정작 결과물은 그냥 예제
그래도 APM소스에 관해 티끌만큼은 안거 같이 다행
그래도 할일은 더럽게 많다는것
이제 이걸 APM메인 소스에서 써먹어야 되는데,,,