Mavros: Senden Sie MAVLink-Nachrichten an QGroundControl

sys_status : Wie Sie sehen, ist Mavros _publisher_, nicht Abonnent.

Hallo Vooon,

Vielen Dank für Ihre Antwort. Wenn ich es gut verstanden habe, kann diese Nachricht nicht gesendet werden und wird nur in ROS empfangen und angezeigt, oder?

Könnte ich dann _mavlink/to_ verwenden, um diese Nachrichten zu senden?
Wie könnte ich es tun?

Danke noch einmal!

mavlink/to sendet eine Nachricht an die FCU, nicht an GCS.

Sie können jedoch den Knoten mavlink/from .

Hallo nochmal,

ok, ich habe den Code so geändert, dass er eine einfache Heartbeat-Nachricht sendet:

#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 - Ich führe den mavros_node mit fcu_url=udp://:14540@ und gcs_url = udp://:[email protected] :14551 aus und erhalte:

[ INFO] [1468659493.280504313]: FCU URL: udp://:14540@
[ INFO] [1468659493.289025098]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1468659493.289457240]: GCS URL: udp://:[email protected]: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 - Ich führe die gcs_bridge mit gcs_url:='udp://@127.0.0.1' aus und erhalte:

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

3 - Ich betreibe den obigen Knoten und veröffentliche in /mavlink/from bei 1 Hz.
4 - Ich verbinde QGroundControl mit UDP 127.0.0.1:14550 und verwende die System-ID 1

... und wieder ist der MAVLink-Inspektor leer.

Am Anfang passiert nichts, aber irgendwann wirft QGroundControl diesen Fehler:

Es liegt ein MAVLink-Versions- oder Baudraten-Mismatch vor. Bitte überprüfen Sie, ob die Baudraten von QGroundControl und Ihrem Autopiloten gleich sind.

Ich denke, QGC ändert den UDP-Port irgendwann vor diesem Fehler.

Was mache ich falsch?

Vielen Dank!

a) Verwenden Sie keine gcs_url von Mavros! Sie sollten sich mit gcs_bridge !
b) Fehlerhafter Code zum Ausfüllen von Mavlink.msg. Sollte etwa so sein:

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);

Sehen:

• 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

Hallo!

Vielen Dank! Endlich habe ich es geschafft. Ich habe verschiedene Klassen verwendet, da ich Indigo verwende. Ich lasse hier den Code, falls sich jemand anders fragt, wie das genau geht. Ich sende eine Heartbeat- und eine Höhennachricht und empfange diese in der QGroundControl:

1- Starten Sie den mavros-Knoten mit fcu_url=udp://:14540@ und gcs_url= udp://:[email protected] :14551.
2- Starten Sie gcs_bridge mit gcs_url:=udp://@127.0.0.1:14560.
3- Starten Sie diesen Knoten:

#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- Starten Sie QGroundControl und konfigurieren Sie die Verbindung unter 127.0.0.1:14560

Wenn Sie den MavLink Inspector öffnen, sollten Sie den Herzschlag und die Höhenmeldung bei ungefähr 1 Hz sehen.

Ich versuche dies zu tun, bekomme aber eine Fehlermeldung, ich kann wirklich nicht verstehen warum, kann mir jemand helfen?
mavlink_msg_heartbeat_pack-Funktion muss enthalten sein, wenn #includeaber es ging nicht...
Kann es sein, dass ich mich in cmake geirrt habe?

#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})

Fehler: 'mavlink_message_t' wurde in diesem Bereich nicht deklariert
mavlink_message_t msg;
Fehler: 'mavlink_msg_heartbeat_pack' wurde in diesem Bereich nicht deklariert
mavlink_msg_heartbeat_pack(system_id, component_id, &msg, type, autopilot, base_mode, custom_mode, system_status);

@shawinmihail welche Version von Mavros? 0.18+ enthält nur mavlink_helpers.h und C++11 lib.

Ich habe eine Lösung für mein Problem gefunden. Wahrscheinlich verwendet mavros v0.18+ nur mavlink v2.0-Header. Um eine Nachricht zu erstellen, sollten wir MsgMap.serialize() anstelle von msg_name_pack() verwenden

    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);

Vielleicht kann es jemandem helfen. Wenn Sie einen besseren Weg kennen, schreiben Sie ihn bitte.
Danke für Hinweis.

Message::serialize() tut MsgMap::reset() , rufen Sie es nicht direkt an! Es kann die Nachricht unterbrechen.

Vielleicht ist es auch besser, benutzerdefinierte gcs_bridge zu schreiben, da Sie dort eine MAVConnInterface Klasse haben, in die Sie Ihre Nachricht senden können:

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

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

gcs_link->send_message_ignore_drop(hb);

@vooon , danke für die Kommentare. Ich finde, dass meine Lösung mit dem Protokoll mavlink v1.0 nicht richtig funktioniert. Jetzt mache und teste ich ein neues, es funktioniert!

#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;
}

vielleicht kann mir jemand helfen und schreiben
wie kann ich das gleiche von python zB machen
mavlink msg erstellen und in mavros msg konvertieren?

Vielleicht ist es auch besser, benutzerdefinierte gcs_bridge zu schreiben, da Sie dort eine MAVConnInterface Klasse haben, in die Sie Ihre Nachricht senden können:

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

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

gcs_link->send_message_ignore_drop(hb);

Diese Methode könnte einfach eine benutzerdefinierte Nachricht senden, aber besteht ein Risiko? @vooon