Send MAVLink messages to QGroundControl

[pablogarciaaunon]

Hi!

I have an airplane simulator, which sends some flight information through ROS topics. I would like to take it and send it to the QGroundControl using MAVLink.

To start testing how it works, I tried to send one message with a fake battery status (mavros_msgs/BatteryStatus.msg):

1 - First, I run the mavros_node with gcs_url = udp://:14570@127.0.0.1:14551. 2 - I run my node, which is supposed to send the battery message:

#include <ros/ros.h>
#include <mavros_msgs/BatteryStatus.h>

int main(int argc, char *argv[])

{
    ros::init(argc, argv, "battery_node");
    ros::NodeHandle n;
    ros::Publisher battery_pub = n.advertise<mavros_msgs::BatteryStatus>("/mavros/battery", 1000);
    ros::Rate loop_rate(10);

    while (ros::ok())
    {
        mavros_msgs::BatteryStatus baterry_msg;

        baterry_msg.voltage = 5;
        baterry_msg.current = 0.5;
        baterry_msg.remaining = 0.36;

        battery_pub.publish(baterry_msg);

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

3 - I connect QGroundControl to UDP: 127.0.0.1:14551

... and nothing happens =/

I am sure that the connexion between mavros and the QGC works, because if I run a simulation with MAVProxy though 127.0.0.1:14540, using mavros as a "bridge", the airplane is recognized.

What am I doing wrong?

I am using Ubuntu 14, ROS Indigo and QGC 2.9.7.

Thank you very much in advance, Pablo

[vooon]

sys_status: as you can see mavros is publisher, not subscriber.

[pablogarciaaunon]

Hello vooon,

Thank you for your answer. If I understood it well, this message cannot be sent, and it is only received and shown in ROS, am I right?

Could I use then mavlink/to to send those messages? How could I do it?

Thanks again!

[vooon]

mavlink/to sends message to FCU, not GCS.

But you may use gcs_bridge node and mavlink/from topic.

[pablogarciaaunon]

Hi again,

ok, I changed the code so that it sends a simple heartbeat message:

#include <ros/ros.h>
#include <mavros_msgs/Mavlink.h>

int main(int argc, char **argv){

    ros::init(argc, argv, "hertbeat_publisher");
    ros::NodeHandle n;

    ros::Publisher example_pub = n.advertise<mavros_msgs::Mavlink>("/mavlink/from", 1000);
    ros::Rate loop_rate(1); // 1 Hz

    mavros_msgs::Mavlink mavlink_msg;

    mavlink_msg.len = 2;
    mavlink_msg.sysid = 1;
    mavlink_msg.compid = 1;
    mavlink_msg.msgid = 0;

    std::vector<long unsigned int> payload64(7, 0);

    mavlink_msg.payload64 = payload64;

    while (ros::ok())
    {
        mavlink_msg.header.stamp = ros::Time::now();
        if (mavlink_msg.seq < 255){
            mavlink_msg.seq++;
        }
        else {mavlink_msg.seq = 0;}

        // publish message
        example_pub.publish(mavlink_msg);

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

1 - I run the mavros_node with fcu_url=udp://:14540@ and gcs_url = udp://:14570@127.0.0.1:14551, getting:

[ INFO] [1468659493.280504313]: FCU URL: udp://:14540@
[ INFO] [1468659493.289025098]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1468659493.289457240]: GCS URL: udp://:14570@127.0.0.1:14551
[ INFO] [1468659493.289691565]: udp1: Bind address: 0.0.0.0:14570
[ INFO] [1468659493.289824558]: udp1: Remote address: 127.0.0.1:14551
...
[ INFO] [1468659494.906066742]: MAVROS started. MY ID 1.240, TARGET ID 1.1

2 - I run the gcs_bridge with gcs_url:='udp://@127.0.0.1', getting:

[ INFO] [1468659758.392659411]: udp0: Bind address: 0.0.0.0:14555
[ INFO] [1468659758.392800665]: udp0: Remote address: 127.0.0.1:14550

3 - I run the above node, publishing in /mavlink/from at 1 Hz. 4 - I connect QGroundControl usign UDP 127.0.0.1:14550, using System ID 1

... and again the MAVLink inspector is empty.

At the beginning, it does not happen anything, but at some point, QGroundControl throws this error:

There is a MAVLink Version or Baud Rate Mismatch. Please check if the baud rates of QGroundControl and your autopilot are the same.

I think somehow QGC changes the UDP port at some point before this error.

What am I doing wrong?

Thanks!

[vooon]

a) Do not use gcs_url of mavros! You should connect to gcs_bridge! b) Bad code for filling Mavlink.msg. Should be something like this:

mavlink::common::msg::HEARTBEAT hb{};
...

mavlink::mavlink_message_t msg;
mavlink::MsgMap map(msg);

hb.serialize(map);
mavlink::finalize_message_buffer(&msg, sysid, compid, ...);

mavros_msgs::Mavlink ml;
mavros_msgs::mavlink::convert(msg, ml);

See:

• https://github.com/mavlink/mavros/blob/master/libmavconn/include/mavconn/msgbuffer.h#L62
• https://github.com/mavlink/mavros/blob/master/mavros_msgs/include/mavros_msgs/mavlink_convert.h

[pablogarciaaunon]

Hi!

Thanks! I finally made it work. I used different classes, since I am using Indigo. I leave here the code, in case someone else is wondering how to do it exactly. I send a heartbeat and an altitude message and receive them in the QGroundControl:

1- Launch mavros node with fcu_url=udp://:14540@ and gcs_url=udp://:14556@127.0.0.1:14551. 2- Launch gcs_bridge with gcs_url:=udp://@127.0.0.1:14560. 3- Launch this node:

#include <ros/ros.h>
#include <mavconn/interface.h>
#include <mavros_msgs/Mavlink.h>
#include <mavros_msgs/mavlink_convert.h>

// General parameters
uint8_t system_id = 1;
uint8_t component_id = 1;
uint8_t type = 1;
uint8_t autopilot = 0;
uint8_t base_mode = 128;
uint32_t custom_mode = 0;
uint8_t system_status = 0;

// Parameters of the altitude message
float alt_monotonic = 10.0;
float alt_amsl = 100;
float alt_local = 10.0;
float alt_relative = 10.0;
float alt_terrain = 10;
float bottom_clearance = 0;

int main(int argc, char **argv){

    // Create heartbeat message
    mavlink_message_t msg_heartbeat;
    mavlink_msg_heartbeat_pack(system_id, component_id, &msg_heartbeat, type, autopilot, base_mode, custom_mode, system_status);

    mavros_msgs::Mavlink ml_heartbeat;
    mavros_msgs::mavlink::convert(msg_heartbeat, ml_heartbeat);

    // Create altitude message
    mavlink_message_t msg_altitude;
    mavros_msgs::Mavlink ml_altitude;

    ros::init(argc, argv, "telemetry_publisher");
    ros::NodeHandle n;

    ros::Publisher example_pub = n.advertise<mavros_msgs::Mavlink>("/mavlink/from", 1000);
    ros::Rate loop_rate(1); // 1 Hz

    while (ros::ok())
    {
        uint64_t time_stamp = ros::Time::now().nsec;

        // Finish altitude message
        mavlink_msg_altitude_pack(system_id, component_id, &msg_altitude, time_stamp, alt_monotonic, alt_amsl, alt_local, alt_relative, alt_terrain, bottom_clearance);
        mavros_msgs::mavlink::convert(msg_altitude, ml_altitude);

        // Publish messages
        example_pub.publish(ml_heartbeat);
        example_pub.publish(ml_altitude);

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

4- Launch QGroundControl and configure connection at 127.0.0.1:14560

If you open the MavLink Inspector, you should see the heartbeat and the altitude message at ~1Hz approximately.

[shawinmihail]

i try do this, but get error, i really cant understand why, can anyone help me? mavlink_msg_heartbeat_pack function must included when #include <mavconn/interface.h> but it didn't... May be i was wrong in cmake?

#include "ros/ros.h"
#include <mavros_msgs/mavlink_convert.h>
#include <mavconn/interface.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<mavros_msgs::Mavlink>("/mavlink/from", 1000);
    ros::Rate loop_rate(10);

    uint8_t system_id = 1;
    uint8_t component_id = 1;
    uint8_t type = 1;
    uint8_t autopilot = 0;
    uint8_t base_mode = 128;
    uint32_t custom_mode = 0;
    uint8_t system_status = 0;
    mavlink_message_t msg;
    mavlink_msg_heartbeat_pack(system_id, component_id, &msg, type, autopilot, base_mode, custom_mode, system_status);
  return 0;
}

cmake_minimum_required(VERSION 2.8.3)
project(talker)

find_package(catkin REQUIRED COMPONENTS mavros mavros_msgs mavlink)
catkin_package()
include(EnableCXX11)
include(MavrosMavlink)
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${mavlink_INCLUDE_DIRS}
)

add_executable(talker talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})

error: ‘mavlink_message_t’ was not declared in this scope mavlink_message_t msg; error: ‘mavlink_msg_heartbeat_pack’ was not declared in this scope mavlink_msg_heartbeat_pack(system_id, component_id, &msg, type, autopilot, base_mode, custom_mode, system_status);

[vooon]

@shawinmihail what version of mavros? 0.18+ will only include mavlink_helpers.h and C++11 lib.

[shawinmihail]

I have found a solution of my problem. Probably, mavros v0.18+ uses mavlink v2.0 headers only. To create any message we should use MsgMap.serialize() instead of msg_name_pack()

    mavlink::mavlink_message_t msg;
    mavlink::MsgMap map(msg);
    mavlink::groupcontrol::msg::GROUP_COEFFS_PART1 msgPart1 //my custom msg;

    msgPart1.kc = kc;
    msgPart1.ks = ks;
        //........................... - fill msg

         msgPart1.serialize(map);
    map.reset();
    auto mi = msgPart1.get_message_info();
    mavlink::mavlink_finalize_message(&msg, system_id, component_id, mi.min_length, mi.length, mi.crc_extra);

    mavros_msgs::Mavlink mavrosMsg;
    mavros_msgs::mavlink::convert(msg, mavrosMsg);

Maybe it can help someone. If you know better way, pls write it. Thanks for hint.

[vooon]

Message::serialize() does MsgMap::reset(), do not call it directly! It may break message.

[vooon]

Also perhaps better to write custom gcs_bridge, as there you have MAVConnInterface class, where you may send your message:

auto gcs_link = MAVConnInterface::open_url("udp-b://@");

mavlink::common::msg::HEARTBEAT hb{};
// fill hb

gcs_link->send_message_ignore_drop(hb);

[shawinmihail]

@vooon, thanks for comments. I find that my solution doesn't work with mavlink v1.0 protocol correctly. Now I make and test new one, it works!

#include "ros/ros.h"
#include <mavros_msgs/mavlink_convert.h>
#include <mavconn/interface.h>
#include <iostream>
#include <mavlink/v2.0/mavlink_helpers.h>

float kc = 0.0f; /*< gravity coeff */
float ks = 0.0f; /*< repulsive coeff */
float ka = 0.0f; /*< speed alignment coeff */
float kp = 0.0f; /*< chasing coeff */
float km = 0.0f; /*< waypoint gravity coeff */
float kh = 0.0f; /*< rotate speed coeff */
float vmax = 0.0f; /*< max autopilot velocity */
float rn = 0.0f; /*< search radius of groupmate  */
float rs = 0.0f; /*< repulsiveOn radius */
float rm = 0.0f; /*< getting waypoint radius */
float rmb = 0.0f; /*< change gravity law radius */

void packMavlinkMessage(const mavlink::Message& mavMsg, mavros_msgs::Mavlink &rosMsg)
{
    mavlink::mavlink_message_t msg;
    mavlink::MsgMap map(msg);
    mavMsg.serialize(map);
    auto mi = mavMsg.get_message_info();

    mavlink::mavlink_status_t *status = mavlink::mavlink_get_channel_status(mavlink::MAVLINK_COMM_0);
    status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
    mavlink::mavlink_finalize_message_buffer(&msg, 1, 1, status, mi.min_length, mi.length, mi.crc_extra);

    mavros_msgs::mavlink::convert(msg, rosMsg);
}

void mavlinkCallback(const mavros_msgs::Mavlink& msg)
{
    mavlink::mavlink_message_t mavMsg;
    mavros_msgs::mavlink::convert(msg, mavMsg);

    if(mavMsg.msgid == mavlink::groupcontrol::msg::SET_GROUP_PARAMS::MSG_ID)
    {
        ROS_WARN("SET GROUP_PARAMS");
        mavlink::MsgMap map(mavMsg);
        mavlink::groupcontrol::msg::SET_GROUP_PARAMS setParamsMsg;
        setParamsMsg.deserialize(map);

        std::cout << setParamsMsg.kc;
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<mavros_msgs::Mavlink>("/mavlink/from", 1000);
    ros::Subscriber sub = n.subscribe("mavlink/to", 1000, mavlinkCallback);
    ros::Rate loop_rate(10);

    ROS_INFO("%s", "Talker started");

    mavlink::groupcontrol::msg::GROUP_PARAMS msgPart1;
    msgPart1.kc = kc;
    msgPart1.ks = ks;
    msgPart1.ka = ka;
    msgPart1.kp = kp;
    msgPart1.km = km;
    msgPart1.kh = kh;
    msgPart1.vmax = vmax;
    msgPart1.rn = rn;
    msgPart1.rs = rs;
    msgPart1.rm = rm;
    msgPart1.rmb = rmb;

    while (ros::ok())
    {
        mavros_msgs::Mavlink mavrosMsg;
        packMavlinkMessage(msgPart1, mavrosMsg);
        pub.publish(mavrosMsg);
        ros::spinOnce();
        loop_rate.sleep();
    }

  return 0;
}

[shawinmihail]

may be someone can help me and write how I can do the same from python e.g. create mavlink msg and convert it to mavros msg?

[GaoGeolone]

Also perhaps better to write custom gcs_bridge, as there you have MAVConnInterface class, where you may send your message:

auto gcs_link = MAVConnInterface::open_url("udp-b://@");

mavlink::common::msg::HEARTBEAT hb{};
// fill hb

gcs_link->send_message_ignore_drop(hb);

This method could simply send custom msg, but is there any risk? @vooon