ArduPilot 入門 (8):新しい Flight Mode を作成する

Copter を例に、新しい飛行モード (Flight Mode) の作成方法を説明します。

・対象者
C++ のコードが読める方

・本手順の作成に使用した環境
OS: Ubuntu 14.04、64bit、English
ArduPilot: Copter V3.5-dev
IDE: Eclipse、Neon.1a Release (4.6.1)
GCS: Mission Planner 1.3.41

※ 説明中に記載しているコードの行数は 2016/10/31 時点の情報です。

 

Flight Mode の作成手順

手順 1.

defines.h に飛行モードの定義を作成します。
変更するファイル:ardupilot/ArduCopter/defines.h (91行目)
https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/defines.h#L91

// Auto Pilot Modes enumeration
enum control_mode_t {
    STABILIZE =     0,  // manual airframe angle with manual throttle
    ACRO =          1,  // manual body-frame angular rate with manual throttle
    ALT_HOLD =      2,  // manual airframe angle with automatic throttle
    AUTO =          3,  // fully automatic waypoint control using mission commands
    GUIDED =        4,  // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
    LOITER =        5,  // automatic horizontal acceleration with automatic throttle
    RTL =           6,  // automatic return to launching point
    CIRCLE =        7,  // automatic circular flight with automatic throttle
    LAND =          9,  // automatic landing with horizontal position control
    DRIFT =        11,  // semi-automous position, yaw and throttle control
    SPORT =        13,  // manual earth-frame angular rate control with manual throttle
    FLIP =         14,  // automatically flip the vehicle on the roll axis
    AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
    POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
    BRAKE =        17,  // full-brake using inertial/GPS system, no pilot input
    THROW =        18,  // throw to launch mode using inertial/GPS system, no pilot input
    AVOID_ADSB =   19,  // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
    GUIDED_NOGPS = 20,  // guided mode but only accepts attitude and altitude
    NEWFLIGHTMODE =21,  // new flight mode description
};

赤字部分を追加します。21 が既に使われていれば、その次の値で定義します。

 

Tips

Eclipse の場合、Ctrl+Shift+R でファイル検索を行えます。

new_mode1

 

手順2.

control_<飛行モード名>.cpp というファイルを作成します。
(例. control_stabilize.cpp、control_loiter.cpp)
ファイルの中には _init() 関数と _run() 関数を用意します。

例)
追加するファイル:ardupilot/ArduCopter/control_newflightmode.cpp
ファイルの中身:

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

/*
 * control_newflightmode.cpp - init and run calls for new flight mode
 */
#include "Copter.h"

// newflightmode_init - initialise flight mode
bool Copter::newflightmode_init(bool ignore_checks)
{
    // do any required initialisation of the flight mode here
    // this code will be called whenever the operator switches into this mode

    // return true initialisation is successful, false if it fails
    // if false is returned here the vehicle will remain in the previous flight mode
    return true;
}

// newflightmode_run - runs the main controller
// will be called at 100hz or more
void Copter::newflightmode_run()
{
    // if not armed or throttle at zero, set throttle to zero and exit immediately
    if(!motors.armed() || channel_throttle->get_control_in() get_control_in() : pilots roll input in the range -4500 ~ 4500
    //   channel_pitch->get_control_in() : pilot pitch input in the range -4500 ~ 4500
    //   channel_throttle->get_control_in() : pilot's throttle input in the range 0 ~ 1000
    //   channel_yaw->get_control_in() : pilot's yaw input in the range -4500 ~ 4500

    // call one of attitude controller's attitude control functions like
    //   attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(roll, pitch, yaw rate, smoothing gain);

    // call position controller's z-axis controller or simply pass through throttle
    //   attitude_control.set_throttle_out(desired throttle, true, g.throttle_filt);
}

 

手順3.

手順2 で作成した _init() 関数と _run() 関数を Copter.h で宣言します。
変更するファイル:ardupilot/ArduCopter/Copter.h
https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/Copter.h

bool sport_init(bool ignore_checks);
void sport_run();
bool stabilize_init(bool ignore_checks);
void stabilize_run();
bool newflightmode_init(bool ignore_checks);
void newflightmode_run();

 

手順4.

flight_mode.cpp の set_mode() 関数内にある switch 文に、手順2 で作成した _init() 関数の呼び出しを追記します。
変更するファイル:ardupilot/ArduCopter/flight_mode.cpp
https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/flight_mode.cpp#L27

// set_mode - change flight mode and perform any necessary initialisation
static bool set_mode(uint8_t mode)
{
    // boolean to record if flight mode could be set
    bool success = false;
    bool ignore_checks = !motors.armed();   // allow switching to any mode if disarmed.  We rely on the arming check to perform

    // return immediately if we are already in the desired mode
    if (mode == control_mode) {
        return true;
    }

    switch(mode) {
        case ACRO:
            #if FRAME_CONFIG == HELI_FRAME
                success = heli_acro_init(ignore_checks);
            #else
                success = acro_init(ignore_checks);
            #endif
            break;

        case NEWFLIGHTMODE:
            success = newflightmode_init(ignore_checks);
            break;
    }
}

 

手順5.

flight_mode.cpp の update_flight_mode() 関数内にある switch 文に、手順2 で作成した _run() 関数の呼び出しを追記します。
変更するファイル:ardupilot/ArduCopter/flight_mode.cpp
https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/flight_mode.cpp#L163

// update_flight_mode - calls the appropriate attitude controllers based on flight mode
// called at 100hz or more
static void update_flight_mode()
{
    switch (control_mode) {
        case ACRO:
            #if FRAME_CONFIG == HELI_FRAME
                heli_acro_run();
            #else
                acro_run();
            #endif
            break;
        case NEWFLIGHTMODE:
            newflightmode_run();
            break;
    }
}

 

手順6.

flight_mode.cpp の print_flight_mode() 関数内にある switch 文に、Flight Mode 名の文字列出力を追記します。。
変更するファイル:ardupilot/ArduCopter/flight_mode.cpp
https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/flight_mode.cpp#L369

//
// print_flight_mode - prints flight mode to serial port.
//
void Copter::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
{
    switch (mode) {
    case STABILIZE:
        port->print("STABILIZE");
        break;
    case NEWFLIGHTMODE:
        port->print("NEWFLIGHTMODE");
        break;

 

Flight Mode を動作させる

上記で作成した Flight Mode の動作テストを行います。
MAVProxy コマンドプロンプト上で、手順1 で定義したFlight Mode の番号を指定することで、Flight Mode を切り替えます。
mode <Flight Mode の番号>

mode 21

new_mode_confirm

21 といった数字ではなく、NEWFLIGHTMODE (モード名) で切り替えを行いたい場合には、MAVLink 側のソースコード (mavutil.py) を変更します。
同様に、Mission Planner 上にモード名を表示したい場合には Mission Planner のソースコードを変更します。

その他、デバッグ用メッセージを利用したい場合には、下記のメソッドを利用します。
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char message);

例えば、手順2 の newflightmode_init 関数が呼ばれたときにメッセージを表示したい場合には下記のように記載します。

bool Copter::newflightmode_init(bool ignore_checks)
{
    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_DEBUG,"NEW FLIGHT MODE!");
    return true;
}

GCS_MAVLINK::send_statustext_all を利用したメッセージ出力は、Console や Mission Planner で確認することができます。
debuglog_console

Console へのメッセージ出力

debuglog_missionplanner

Mission Planner へのメッセージ出力

 

参考情報

Adding a New Flight Mode (Copter Code Overview)
http://ardupilot.org/dev/docs/apmcopter-adding-a-new-flight-mode.html

Building Mission Planner with Visual Studio
http://ardupilot.org/dev/docs/buildin-mission-planner.html
※ 告知
ドローンソフトウェアエンジニア養成塾第3期 (4月1日より開始) の開催が決定しました。
現在、受講者を募集中です。

ArduPilot や Mission Planner の使い方を知りたい方、DroneKit を用いたアプリケーション開発をしたい方、ArduPilot をソースコードレベルで理解してオープンソース コミッターになりたい方、目的別に合わせてコースを用意しておりますので、是非募集ページをご確認ください。

 

著者紹介

yamaguchi_picture山口達也
ドローンソフトウェアエンジニア養成塾 第1期卒業生。

記事の誤りを見つけた方は下記のメールアドレスまでご連絡ください。
techblog@drone-j.com
(週末にメールを確認していますので、対応が遅くなること、休みの日にメールを返信させていただくこと、ご了承ください。)