MAVLink Messaging

전체 메시지에 대한 개요는 다음 링크를 참고하세요. 여기.

커스텀 MAVLink Messages 생성

여기 튜터리얼에서는 msg/ca_trajectory.msg에 있는 custom uORB ca_trajectory와 mavlink/include/mavlink/v1.0/custom_messages/mavlink_msg_ca_trajectory.h에 있는 커스텀 mavlink ca_trajectory 메시지를 가지고 있다고 가정합니다. ( 여기에서 커스텀 mavlink 메시지와 헤더 생성하는 방법을 참고).

커스텀 MAVLink Message 보내기

이 섹션에서는 커스텀 uORB 메시지를 사용하는 방법과 이를 mavlink 메시지로 보내는 방법을 설명합니다.

mavlink의 헤더와 uorb 메시지를 mavlink_messages.cpp에 추가합니다.

#include <uORB/topics/ca_trajectory.h>
#include <v1.0/custom_messages/mavlink_msg_ca_trajectory.h>

새로운 class를 mavlink_messages.cpp에 추가

class MavlinkStreamCaTrajectory : public MavlinkStream
{
public:
    const char *get_name() const
    {
        return MavlinkStreamCaTrajectory::get_name_static();
    }
    static const char *get_name_static()
    {
        return "CA_TRAJECTORY";
    }
    uint8_t get_id()
    {
        return MAVLINK_MSG_ID_CA_TRAJECTORY;
    }
    static MavlinkStream *new_instance(Mavlink *mavlink)
    {
        return new MavlinkStreamCaTrajectory(mavlink);
    }
    unsigned get_size()
    {
        return MAVLINK_MSG_ID_CA_TRAJECTORY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
    }

private:
    MavlinkOrbSubscription *_sub;
    uint64_t _ca_traj_time;

    /* do not allow top copying this class */
    MavlinkStreamCaTrajectory(MavlinkStreamCaTrajectory &);
    MavlinkStreamCaTrajectory& operator = (const MavlinkStreamCaTrajectory &);

protected:
    explicit MavlinkStreamCaTrajectory(Mavlink *mavlink) : MavlinkStream(mavlink),
        _sub(_mavlink->add_orb_subscription(ORB_ID(ca_trajectory))),  // make sure you enter the name of your uORB topic here
        _ca_traj_time(0)
    {}

    void send(const hrt_abstime t)
    {
        struct ca_traj_struct_s _ca_trajectory;    //make sure ca_traj_struct_s is the definition of your uORB topic

        if (_sub->update(&_ca_traj_time, &_ca_trajectory)) {
            mavlink_ca_trajectory_t _msg_ca_trajectory;  //make sure mavlink_ca_trajectory_t is the definition of your custom mavlink message

            _msg_ca_trajectory.timestamp = _ca_trajectory.timestamp;
            _msg_ca_trajectory.time_start_usec = _ca_trajectory.time_start_usec;
            _msg_ca_trajectory.time_stop_usec  = _ca_trajectory.time_stop_usec;
            _msg_ca_trajectory.coefficients =_ca_trajectory.coefficients;
            _msg_ca_trajectory.seq_id = _ca_trajectory.seq_id;

            _mavlink->send_message(MAVLINK_MSG_ID_CA_TRAJECTORY, &_msg_ca_trajectory);
        }
    }
};

마지막으로 stream class를 mavlink_messages.cpp 끝에 있는 streams_list에 추가합니다.

StreamListItem *streams_list[] = {
...
new StreamListItem(&MavlinkStreamCaTrajectory::new_instance, &MavlinkStreamCaTrajectory::get_name_static),
nullptr
};

다음으로 stream을 사용하기 위해서 startup 스크립트에 다음 한 줄을 추가할 수 있습니다. (-r은 streaming rate를 설정하며 -u은 UDP port 14556로 mavlink channel을 식별함) :

mavlink stream -r 50 -s CA_TRAJECTORY -u 14556

커스텀 MAVLink Message 수신하기

이 섹션에서는 mavlink로 메시지를 받고 이를 uORB로 publish하는 방법을 설명합니다.

들어오는 mavlink 메시지를 처리하는 함수를 mavlink_receiver.h에 추가

#include <uORB/topics/ca_trajectory.h>
#include <v1.0/custom_messages/mavlink_msg_ca_trajectory.h>

수신하는 mavlink 메시지를 처리하는 함수를 mavlink_receiver.h 에 있는 MavlinkReceiver class에 추가

void handle_message_ca_trajectory_msg(mavlink_message_t *msg);

uORB publisher를 mavlink_receiver.h에 있는 MavlinkReceiver class에 추가

orb_advert_t _ca_traj_msg_pub;

handle_message_ca_trajectory_msg 함수 구현을 mavlink_receiver.cpp에 추가

void
MavlinkReceiver::handle_message_ca_trajectory_msg(mavlink_message_t *msg)
{
    mavlink_ca_trajectory_t traj;
    mavlink_msg_ca_trajectory_decode(msg, &traj);

    struct ca_traj_struct_s f;
    memset(&f, 0, sizeof(f));

    f.timestamp = hrt_absolute_time();
    f.seq_id = traj.seq_id;
    f.time_start_usec = traj.time_start_usec;
    f.time_stop_usec = traj.time_stop_usec;
    for(int i=0;i<28;i++)
        f.coefficients[i] = traj.coefficients[i];

    if (_ca_traj_msg_pub == nullptr) {
        _ca_traj_msg_pub = orb_advertise(ORB_ID(ca_trajectory), &f);

    } else {
        orb_publish(ORB_ID(ca_trajectory), _ca_traj_msg_pub, &f);
    }
}

그리고 마지막으로 MavlinkReceiver::handle_message()에서 호출되는지 확인

MavlinkReceiver::handle_message(mavlink_message_t *msg)
 {
     switch (msg->msgid) {
        ...
    case MAVLINK_MSG_ID_CA_TRAJECTORY:
        handle_message_ca_trajectory_msg(msg);
        break;
        ...
     }

일반

streaming rate 설정

가끔은 개별 topic의 streaming rate를 올리는 것이 유용합니다. (예로 QGC에서 inspection용도로) 다음과 같은 라인으로 처리할 수 있습니다.

mavlink stream -u <port number> -s <mavlink topic name> -r <rate>

mavlink status으로 port number를 가져올 수 있습니다. 출력 중에 transport protocol: UDP (<port number>)를 확인합니다. 예제로

mavlink stream -u 14556 -s OPTICAL_FLOW_RAD -r 300