MAVLink をc/c++プログラムから使う簡単な例です. MAVLinkの基本的なインストールや使い方については mavlink github のREADME.mdにある通りですなのですがc/c++プログラムから使う場合にはheaderだけでできている c_library_v2 を使うのがお手軽です.

• STEP.1 git clone https://github.com/mavlink/c_library_v2.git
• STEP.2 cloneしたc_library_v2をどこかに置いてそれがc/c++プログラムのincludeパスに入るようにします. これで準備はokです.

基本的には

#include <common/mavlink.h>

でいろいろなMAVLinkのライブラリ関数が使えるのですが場合によってはそれだけではダメな時があります. 例えばtcpなどでMAVLimkのパケットをやり取りしたいときは上のREADME.mdに書いてあるようにMAVLINK_USE_CONVENIENCE_FUNCTIONSを定義するのですがこの場合には補助的な宣言が少しだけ必要です.

arduinoのWiFiClientというtcp clientのライブラリクラスといっしょに使った時の例です.

#undef F
#include <mavlink_types.h>
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS 1
#define MAVLINK_SEND_UART_BYTES send_tcp_bytes
static void send_tcp_bytes(mavlink_channel_t chan, const uint8_t *buf, uint16_t len);
extern mavlink_system_t mavlink_system;
#include <common/mavlink.h>
// ...
// ...
mavlink_system_t mavlink_system = { 20, 99 };

static void send_tcp_bytes(mavlink_channel_t chan, const uint8_t *buf,
                 uint16_t len)
{
    client.write(buf, len);
}
//...
//...
void check_telemetry(void)
{
    if (!telemetry_connected)
        return;
    for(;;) {
        if (client.available() > 0) {
            uint8_t c = client.read();
            mavlink_message_t msg;
            mavlink_status_t status;
            if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {
                switch(msg.msgid) {
                    case MAVLINK_MSG_ID_HEARTBEAT:
                        target_sysid = msg.sysid;
                        target_compid = msg.compid;
                        if (!request_sent) {
                            const uint8_t stream_id = MAV_DATA_STREAM_EXTRA1;
                            mavlink_msg_request_data_stream_send(MAVLINK_COMM_0,
                                                       target_sysid,
                                                       target_compid,
                                                       stream_id,
                                                       10,
                                                       1);
                            request_sent = true;
                        }
                        break;
                    case MAVLINK_MSG_ID_ATTITUDE:
                        //Serial.println("ATTITUDE");
                        mavlink_attitude_t attitude;
                        mavlink_msg_attitude_decode(&msg, &attitude);
                        telemetry_yaw = attitude.yaw;
                        telemetry_yaw_ok = true;
                        //Serial.println(telemetry_yaw);
                        break;
                    default:
                        break;
                }
            }
        } else
            break;
    }
}

mavlink_system = { 20, 99 };

で自分のIDを20,種別を99としています. 種別の100番以降にはCAMERA,GIMBAL,GPSなどいろいろなコンポーネントの番号が定義されています.

// common/common.h
// ...
typedef enum MAV_COMPONENT
{
 MAV_COMP_ID_ALL=0, /*  | */
 MAV_COMP_ID_AUTOPILOT1=1, /*  | */
 MAV_COMP_ID_CAMERA=100, /*  | */
 MAV_COMP_ID_CAMERA2=101, /*  | */
 MAV_COMP_ID_CAMERA3=102, /*  | */
 MAV_COMP_ID_CAMERA4=103, /*  | */
 MAV_COMP_ID_CAMERA5=104, /*  | */
 MAV_COMP_ID_CAMERA6=105, /*  | */
 MAV_COMP_ID_SERVO1=140, /*  | */
 MAV_COMP_ID_SERVO2=141, /*  | */
 MAV_COMP_ID_SERVO3=142, /*  | */
 MAV_COMP_ID_SERVO4=143, /*  | */
 MAV_COMP_ID_SERVO5=144, /*  | */

プログラムは簡単で1byte読んではmavlink_parse_charに食わせ、mavlinkパケット読み込み成功の場合に.msgidをみてそれぞれの処理をしています.

最初はHEARTBEATしか来ないのですがそれで相手のID(.sysid)と種別(.compid)がわかるのでその相手にMAV_DATA_STREAM_EXTRA1というデータストリームを10Hzのレートで送ってくれるようにmavlink_msg_request_data_stream_sendで頼んでいます. この例で実際の相手はArduCopterのtelemetryでMAV_DATA_STREAM_EXTRA1で指定されるデータストリームではATTITUDEやAHRS2のパケットが流れてくるのでATTITUDEのパケットを拾ってyawの値を得ています.

同じような方法でいろいろな情報を得たり、制御を行うことが可能です.