Problem with sending MAVLink message

[Krrris88]

Hi everyone

I want to send a simple MAVLink message to AP, exactly to SITL. I chose command: MAV-CMD-DO-SET-MODE / MAV-MODE-GUIDED-ARMED. I start from run SITL with this command:

sim_vehicle.py -v ArduPlane -f quadplane --console --mavproxy-args="--master udp:127.0.0.1:14551"

Later turn on MAVLink router: mavlink-routerd

Then I start test:

#include "../../c_library_v2/common/mavlink.h"
#include "../../c_library_v2/ardupilotmega/ardupilotmega.h"
#include "../../c_library_v2/ardupilotmega/mavlink.h"

TEST(MAVLinkRouter, sendMessageToAP)
{
    int sockfd;
    struct sockaddr_in     servaddr;
    const int port = 14551;

    // Creating socket file descriptor
    if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) {
        perror("socket creation failed");
        exit(EXIT_FAILURE);
    }

    memset(&servaddr, 0, sizeof(servaddr));

    // Filling server information
    servaddr.sin_family = AF_INET;
    servaddr.sin_port = htons(port);
    servaddr.sin_addr.s_addr = INADDR_ANY;

    mavlink_message_t msg;

    mavlink_command_long_t command_to_send;

    const uint8_t sysid_thismav = 255;
    const uint8_t cmpid_thismav = 1;
    const uint8_t first_transmission_confirmation = 0;

    const uint8_t AP_id = 1;
    const uint8_t AP_component_id = 1;

    command_to_send.target_system = AP_id;
    command_to_send.target_component = AP_component_id;
    command_to_send.command = MAV_CMD_DO_SET_MODE;
    command_to_send.confirmation = first_transmission_confirmation;
    command_to_send.param1 = MAV_MODE_GUIDED_ARMED;

    mavlink_msg_command_long_encode(sysid_thismav, cmpid_thismav, &msg, &command_to_send);

    uint8_t data[MAVLINK_MAX_PACKET_LEN];

    uint16_t len = mavlink_msg_to_send_buffer(data, &msg);

    /*sendto(sockfd, data, len,
           MSG_CONFIRM, (const struct sockaddr *) &servaddr,
           sizeof(servaddr));*/

    sendto(sockfd, data, MAVLINK_MAX_PACKET_LEN,
           MSG_CONFIRM, (const struct sockaddr *) &servaddr,
           sizeof(servaddr));
}

On MAVProxy console I get some random chars:

So later I tried send just heartbeat:

TEST(MAVLinkRouter, sendHeartbeat)
{
    int sockfd;
    struct sockaddr_in     servaddr;
    const int port = 14551;

    using namespace std::chrono_literals;

    // Creating socket file descriptor
    if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) {
        perror("socket creation failed");
        exit(EXIT_FAILURE);
    }

    memset(&servaddr, 0, sizeof(servaddr));

    // Filling server information
    servaddr.sin_family = AF_INET;
    servaddr.sin_port = htons(port);
    servaddr.sin_addr.s_addr = INADDR_ANY;

    mavlink_message_t msg;

    mavlink_heartbeat_t heartbeat;

    const uint8_t sysid_thismav = 255;
    const uint8_t cmpid_thismav = 1;
    const uint8_t first_transmission_confirmation = 0;

    const uint8_t AP_id = 1;
    const uint8_t AP_component_id = 1;

    heartbeat.type = MAV_TYPE_ONBOARD_CONTROLLER;
    heartbeat.autopilot = MAV_AUTOPILOT_INVALID;
    heartbeat.base_mode = 0; // These flags encode the MAV mode
    heartbeat.custom_mode = PLANE_MODE_FLY_BY_WIRE_A; // A bitfield for use for autopilot-specific flags
    heartbeat.system_status = MAV_STATE_STANDBY;

    mavlink_msg_heartbeat_encode(sysid_thismav, cmpid_thismav, &msg, &heartbeat);

    uint8_t data[MAVLINK_MAX_PACKET_LEN];

    uint16_t len = mavlink_msg_to_send_buffer(data, &msg);

    /*for(auto i = 0; i < 20; i++){
    sendto(sockfd, data, len,
        MSG_CONFIRM, (const struct sockaddr *) &servaddr,
        sizeof(servaddr));
        std::this_thread::sleep_for(500ms);
    }*/

    for(auto i = 0; i < 20; i++){
    sendto(sockfd, data, MAVLINK_MAX_PACKET_LEN,
           MSG_CONFIRM, (const struct sockaddr *) &servaddr,
               sizeof(servaddr));
    std::this_thread::sleep_for(500ms);
    }
}

Also in MAVProxy console I got some random chars:

I read this: MAVLink and Arduino: step by step

Instead xxx_pack I use xxx_encode. So I also tried this xxx_pack function, but result is still the same.
In topic mentioned above for send is used SerialMAV.write(buf, len), I use sendto() with UDP, is it good for communicate with SITL?

I found that all msg has function like this (xxx_send):

static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status)
{...}

But how to use this mavlink_channel_t?

#ifndef HAVE_MAVLINK_CHANNEL_T
typedef enum {
    MAVLINK_COMM_0,
    MAVLINK_COMM_1,
    MAVLINK_COMM_2,
    MAVLINK_COMM_3
} mavlink_channel_t;
#endif

What am I doing wrong? Can anyone help?

[Krrris88]

Some update. I used WireShark for check which port SITL use for communicate, so where I can send my message/command. Port is random for each usage. So how I can send my message, if I don’t know port no?

Anyway I read port no from WireShark and sent message to this port. In response I got this:

result: 2 == MAV_RESULT_DENIED == Command is invalid (is supported but has invalid parameters). Retrying same command and parameters will not work.

My message:

    mavlink_message_t msg_to_send;
    mavlink_command_long_t command_to_send;

    const uint8_t first_transmission_confirmation = 0;
    const uint8_t AP_id = 1;
    const uint8_t AP_component_id = 1;

    command_to_send.target_system = AP_id;
    command_to_send.target_component = AP_component_id;
    command_to_send.command = MAV_CMD_DO_SET_MODE;
    command_to_send.confirmation = first_transmission_confirmation;
    command_to_send.param1 = MAV_MODE_GUIDED_ARMED;

    mavlink_msg_command_long_encode(mavlink_system.sysid, mavlink_system.compid, &msg_to_send, &command_to_send);

    uint8_t msg_data[MAVLINK_MAX_PACKET_LEN];

    uint16_t msg_len = mavlink_msg_to_send_buffer(msg_data, &msg_to_send);

What is wrong with my message? Which parameter is invalid?