Go to the documentation of this file.00001
00006
00007
00008
00009
00010
00011
00012
00013
00014 #pragma once
00015
00016 #include <algorithm>
00017 #include <mavros_msgs/Mavlink.h>
00018 #include <mavconn/mavlink_dialect.h>
00019
00020 namespace mavros_msgs {
00021 namespace mavlink {
00022
00030 inline bool convert(const mavros_msgs::Mavlink &rmsg, mavlink_message_t &mmsg)
00031 {
00032 if (rmsg.payload64.size() > sizeof(mmsg.payload64) / sizeof(mmsg.payload64[0])) {
00033 return false;
00034 }
00035
00036 mmsg.magic = MAVLINK_STX;
00037 mmsg.len = rmsg.len;
00038 mmsg.seq = rmsg.seq;
00039 mmsg.sysid = rmsg.sysid;
00040 mmsg.compid = rmsg.compid;
00041 mmsg.msgid = rmsg.msgid;
00042 mmsg.checksum = rmsg.checksum;
00043 std::copy(rmsg.payload64.begin(), rmsg.payload64.end(), mmsg.payload64);
00044 return true;
00045 };
00046
00054 inline bool convert(const mavlink_message_t &mmsg, mavros_msgs::Mavlink &rmsg)
00055 {
00056 const size_t payload64_len = (mmsg.len + 7) / 8;
00057
00058
00059 rmsg.is_valid = true;
00060
00061 rmsg.len = mmsg.len;
00062 rmsg.seq = mmsg.seq;
00063 rmsg.sysid = mmsg.sysid;
00064 rmsg.compid = mmsg.compid;
00065 rmsg.msgid = mmsg.msgid;
00066 rmsg.checksum = mmsg.checksum;
00067 rmsg.payload64 = std::move(mavros_msgs::Mavlink::_payload64_type(mmsg.payload64, mmsg.payload64 + payload64_len));
00068
00069 return true;
00070 };
00071
00072 };
00073 };