Skip to content

Latest commit

 

History

History
166 lines (135 loc) · 6.05 KB

mavlink.md

File metadata and controls

166 lines (135 loc) · 6.05 KB

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