Emlid Navio2で動かすardupilotにもzeroMQを導入する

navio2はEmlid社の開発するRaspberry Piベースのオートパイロット用ボードで、オープンソースのドローンシステムardupilotに対応しています。
emlid.com
詳細はこちら。
Navio2 docs

まず、マニュアルに従ってAPMを普通にインストールしてください。
ここでは、planeでもcopterでもなくroverをターゲットとすることを前提にします。

基本的にRCサーボモータを出力対象として設計されていますので、ロボット用コマンドサーボやもっと大型の汎用モータを動かすためにはインターフェースを自分で用意してやる必要があります。
navio2ボード上にI2CやUARTのコネクタがありますので、それを使って出力を取っても良いですし、RaspberryPiのUSBやLANも使えますのでそれらを使って外部のデバイスとつなげても良いです。

ただ、あらゆるデバイスはzeroMQで繋げてしまえばいいじゃん、という思想があるので、このページではardupilotでzeroMQを使う方法を調査します。

準備

1. wafを見てみる

新しいビルドシステムwafで、ライブラリを追加する方法がわからない。
[1]を勉強して、

def build(bld):
bld(...
lib = ['m'],
libpath = ['/usr/lib'],
...)

のように書けばいいのだろうな、などと推測はつくものの、
ardupilotのwscriptが大きくて、あちらこちらに散在するので、何がどういう順番で呼ばれるのかよくわからない。
*1

2. Configure/Makeを見てみる

> cd ardupilot
> make -C APMrover2 navio2

で試してみるとまだ使える。廃止されると書かれているが。こちらなら使い慣れていてどこに何を書けばいいのかもわかるので、当面こちらで乗り切ることにする。

3. zeroMQのインストール

zmqは普通にライブラリをインストールしてください。ソースからが無難だと思います。
c++用のbindingを/usr/local/includeに置いてください。

Makefileの編集

ardupilotのmakefileが存在する場所は、~/ardupilot/mkの下です。
zeroMQを追加するときに編集するべきファイルは、board_native.mkです。

CXXOPTS = -ffunction-sections -fdata-sections -fno-exceptions -fsigned-char

と書かれた一文を探して、zmqを使うのに都合がいいように次のように変更します。

CXXOPTS = -ffunction-sections -fdata-sections -fexceptions -fsigned-char
LIBS += -lzmq -lpthread

no-exceptionsを外してしまったので、予期せずエラー時の挙動が何か変わってしまうかもしれません。注意してください。
とりあえず、これでbuildは通るようになります。

モータの出力をpubで垂れ流す準備

APMrover2/Rover.hにコンテキストとソケットの定義を追加

#include <zmq.hpp> // 追加

class Rover
{
・・・

public:
    zmq::context_t *context;
    zmq::socket_t *publisher;
};

APMrover2/Rover.cppのRoverコンストラクタ内で初期化

Rover::Rover(void) :
・・・
{
    context = new zmq::context_t(1);
    publisher = new zmq::socket_t(*context, ZMQ_PUB);
    int v = 1;
    size_t size = sizeof(v);
    publisher->setsockopt(ZMQ_SNDHWM, &v, size);
    publisher->bind("tcp://*:6636"); // 送信先IPとポート番号 
}

RCサーボモータへの出力をzmqで流す

出力っぽい名前の関数呼び出しを探していくと、サーボモータへの出力はStering.cpp内で行われてることが見つかります。

void Rover::set_servos(void){
・・・
channel_steer->output();
channel_throttle->output();
・・・
}

ここのPWM出力していると思しき値の、元になった値をもらって来ればよさそうです。

channel_steerもchannel_throttleもRC_Channelというクラスのインスタンスのようなので、RC_Channelのクラスの定義を探しますが、APMrover2の中には見当たりません。
探すと、~/ardupilot/libraries/RC_Channelの下に関連ファイルがありそうです。
RC_Channel.hの中を見ていくと、

int_16t get_servo_out() const { return _servo_out; }
void set_servo_out(int_16t val) {_servo_out = val; }

というのがあり、RC_Channel.cppの中でoutput()から辿ってcalc_pwmの中を見ていくと、range_to_pwm()などの中で_servo_out変数が随所で使われていますので、とりあえずコレで良さそうです。
レンジをpwm用に整形された値を得たい場合は、get_pwm_out()関数を使います。

ということで、
Stering.cppのser_servos(void)関数の中に、zeroMQ出力を追加します。

channel_steer->output();
channel_throttle->output();

int16_t l = channel_steer->get_servo_out();
int16_t r = channel_throttle->get_servo_out();

int msg_size = 20;
zmq::message_t msg(msg_size);
memset (msg.data (), 0, msg_size);
snprintf((char*)msg.data(), msg_size,
"/servo_out %d %d", l, r);
publisher->send(msg);

これでひとまず行けそうです。

お片付け

行儀よくデストラクタで、contextとsocketを削除するようにしないといけません。
Rover::~Rover(void);の定義をRover.hの中に追加して、

Rover::~Rover(void)
{
    if(publisher){
         int v = 0;
         size_t size = sizeof(v);
         publisher->setsockopt(ZMQ_LINGER, &v, size);
         // publisher->close();
         delete publisher;
    }

    if(context){
         // context->close();
         delete context;
    }
}

とします。

実行は、別のコントローラ用PCを用意してAPMPlanner2などを立ち上げておいてから、
raspberrypi側で、