sender/receiver based on MavlinkDirect working too slow

[pedramsafi]

I have created a sender/receiver pair to exchange custom mavlink messages over SiK Radio.

ideally, I would like to minimize the amount of data being sent over the link and keep it as minimal as possible to conserve bandwidth.

Below is an example of each of the sender and receiver modules:

Sender

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

    uint8_t drone_system_id = 2;
    uint8_t drone_component_id = 1;
    bool always_send_heartbeat = true;

    Mavsdk::Configuration config = Mavsdk::Configuration(drone_system_id, drone_component_id, always_send_heartbeat);
    Mavsdk mavsdk_drone{config};
    std::shared_ptr<mavsdk::System> peer_system_;

    //connect to the local serial radio and obtain the peer_system_
    auto mavlink_direct = MavlinkDirect{peer_system_};

    //define and load custom_xml

    //create an instance of the message
    MavlinkDirect::MavlinkMessage msg_name{};

    while(1){
        msg_name.fields_json = R"({...});
        auto result = mavlink_direct.send_message(msg_name);
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // 1Hz
    return 0;

}

Receiver

int main(argc, char* argv[])
{
    uint8_t gcs_system_id = 250;
    uint8_t gcs_component_id = 1;
    bool always_send_heartbeat = true;

    Mavsdk::Configuration config = Mavsdk::Configuration(gcs_system_id, gcs_component_id, always_send_heartbeat);
    Mavsdk mavsdk_drone{config};
    std::shared_ptr<mavsdk::System> peer_system_;

    //connect to the local serial radio and obtain the peer_system_
    auto mavlink_direct = MavlinkDirect{peer_system_};

    //define and load custom_xml

    //create an instance of the message
    MavlinkDirect::MavlinkMessage msg_name{};

    auto result = mavlink_direct.subscribe_message("msg_name", [](const mavsdk::MavlinkDirect::MavlinkMessage &message){
        std::cout << message.message_name << " was received!" <<  std::endl;
    });

    while(1){
         std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // 1Hz
    }
return 0;

}

The problem is the receiver is not receiving all the messages. It is dropping some. Here is a sample of the output log. the lat/lon alt values are dummy values and show "the number of seconds since start".

Listening for GPS messages. Press Ctrl+C to exit...
[12:42:07|Info ] heartbeats timed out (system_impl.cpp:370)
{"message_id":42000,"message_name":"END_TELEMETRY","altitude":13,"latitude":13,"longitude":13}
{"message_id":42000,"message_name":"END_TELEMETRY","altitude":18,"latitude":18,"longitude":18}
{"message_id":42000,"message_name":"END_TELEMETRY","altitude":19,"latitude":19,"longitude":19}
{"message_id":42000,"message_name":"END_TELEMETRY","altitude":24,"latitude":24,"longitude":24}
{"message_id":42000,"message_name":"END_TELEMETRY","altitude":25,"latitude":25,"longitude":25}
[12:42:23|Debug] Discovered 1 component(s) (system_impl.cpp:607)
[12:42:27|Info ] heartbeats timed out (system_impl.cpp:370)
{"message_id":42000,"message_name":"END_TELEMETRY","altitude":39,"latitude":39,"longitude":39}
[12:42:40|Debug] Discovered 1 component(s) (system_impl.cpp:607)
{"message_id":42000,"message_name":"END_TELEMETRY","altitude":47,"latitude":47,"longitude":47}
[12:42:43|Info ] heartbeats timed out (system_impl.cpp:370)
[12:42:44|Debug] Discovered 1 component(s) (system_impl.cpp:607)
[12:42:47|Info ] heartbeats timed out (system_impl.cpp:370)

[julianoes]

In this example you're sending messages as fast as you can, that's not going to work...

Any chance you could create a full working example for me to test?

[pedramsafi]

Sorry for the delay. I will do this next week. I have seen significant improvement after I switched radio ( I realized one of the modems was faulty ) but I am still not receiving %100 of the messages.

[julianoes]

Ok, please let me know how I can reproduce it, otherwise I'll have to close the issue.

[julianoes]

Related: #2694

[pedramsafi]

sample code for the sender (drone)

#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/mavlink_direct/mavlink_direct.h>
#include <mavsdk/mavlink_address.h>

#include <iostream>
#include <chrono>
#include <thread>
#include <memory>

using namespace std::chrono_literals;
using namespace mavsdk;

void usage(const std::string &bin_name)
{
    std::cerr << "Usage : " << bin_name << " <connection_url>\n"
              << "Connection URL format should be :\n"
              << " For TCP server: tcpin://<our_ip>:<port>\n"
              << " For TCP client: tcpout://<remote_ip>:<port>\n"
              << " For UDP server: udp://<our_ip>:<port>\n"
              << " For UDP client: udp://<remote_ip>:<port>\n"
              << " For Serial : serial://</path/to/serial/dev>:<baudrate>]\n"
              << "For example, to connect to the simulator use URL: udpin://0.0.0.0:14540\n";
}

int main(int argc, char *argv[])
{
    if (argc != 2)
    {
        usage(argv[0]);
        return 1;
    }

    float_t msg_counter = 0.0f;

    uint8_t drone_system_id = 10;
    uint8_t drone_component_id = 2;
    bool always_send_heartbeat = true;

    Mavsdk::Configuration config = Mavsdk::Configuration(drone_system_id, drone_component_id, always_send_heartbeat);

    // create mavsdk instance
    Mavsdk mavsdk_drone{config};

    // create a system
    std::shared_ptr<mavsdk::System> peer_system_;

    // Connect using the provided connection URL
    std::string conn_url = argv[1];
    auto connection_result = mavsdk_drone.add_any_connection(conn_url);

    while (connection_result != ConnectionResult::Success)
    {
        std::this_thread::sleep_for(std::chrono::seconds(1));
        connection_result = mavsdk_drone.add_any_connection(conn_url);
    }

    std::cout << "connected to the local serial radio at " << conn_url << std::endl;

    int counter = 0;
    while (mavsdk_drone.systems().size() == 0)
    {
        std::cout << counter++ << "-trying to detect a peer system..." << std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    std::cout << "peer system detected... " << std::endl;

    // Get the first connected system
    peer_system_ = mavsdk_drone.systems()[0];
    if (!peer_system_->is_connected())
    {
        std::cerr << "peer system not connected!" << std::endl;
        return 1;
    }
    else
    {
        std::cout << "peer system connected!" << std::endl;
    }

    auto mavlink_direct = MavlinkDirect{peer_system_};

    std::string custom_xml = R"(
    <mavlink>
        <messages>
            <message id="42000" name="MY_DRONE_GLOBAL_POSITION">
                <field type="float" name="latitude">latitude</field>
                <field type="float" name="longitude">longitude</field>
                <field type="float" name="altitude">altitude</field>
            </message>
        </messages>
    </mavlink>)";

    // Load custom XML
    MavlinkDirect::Result xml_result = mavlink_direct.load_custom_xml(custom_xml);

    if (xml_result != MavlinkDirect::Result::Success)
    {
        std::cerr << "Failed to load custom XML: " << xml_result << std::endl;
        return 1;
    }

    MavlinkDirect::MavlinkMessage my_drone_global_position{};
    my_drone_global_position.message_name = "END_TELEMETRY";
    my_drone_global_position.system_id = drone_system_id;
    my_drone_global_position.component_id = drone_component_id;
    my_drone_global_position.target_system_id = 250;
    my_drone_global_position.target_component_id = 1;

    while (true)
    {

        // fill out the message
        my_drone_global_position.fields_json =
            R"({"latitude": )" + std::to_string(msg_counter) +
            R"(,"longitude": )" + std::to_string(msg_counter) +
            R"(,"altitude": )" + std::to_string(msg_counter) +
            R"(})";

        auto result_hb = mavlink_direct.send_message(end_telemetry);

        if (result_hb == MavlinkDirect::Result::Success)
        {
            std::cout << "telemetry message " << (msg_counter + 1) << " sent successfully" << std::endl;
        }
        else
        {
            std::cerr << " telemetry message could not be sent: " << result_hb << std::endl;
        }

        msg_counter++;

        // 1Hz
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    return 0;
}

[julianoes]

Thanks. Have you re-tested after #2694?

[pedramsafi]

sample code for the receiver (gcs)

#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/mavlink_direct/mavlink_direct.h>

#include <iostream>
#include <chrono>
#include <thread>
#include <memory>

using namespace std::chrono_literals;
using namespace mavsdk;

void usage(const std::string &bin_name)
{
    std::cerr << "Usage : " << bin_name << " <connection_url>\n"
              << "Connection URL format should be :\n"
              << " For TCP server: tcpin://<our_ip>:<port>\n"
              << " For TCP client: tcpout://<remote_ip>:<port>\n"
              << " For UDP server: udp://<our_ip>:<port>\n"
              << " For UDP client: udp://<remote_ip>:<port>\n"
              << " For Serial : serial://</path/to/serial/dev>:<baudrate>]\n"
              << "For example, to connect to the simulator use URL: udpin://0.0.0.0:14540\n";
}

int main(int argc, char *argv[])
{
    if (argc != 2)
    {
        usage(argv[0]);
        return 1;
    }

    uint8_t gcs_system_id = 250;
    uint8_t gcs_component_id = 1;
    bool always_send_heartbeat = true;

    Mavsdk::Configuration config = Mavsdk::Configuration(gcs_system_id, gcs_component_id, always_send_heartbeat);


    // create mavsdk instance
    Mavsdk mavsdk_drone{config};

    // create a system
    std::shared_ptr<mavsdk::System> peer_system_;

    // Connect using the provided connection URL
    auto connection_result = mavsdk_drone.add_any_connection(argv[1]);
    if (connection_result != ConnectionResult::Success)
    {
        std::cerr << "Connection failed: " << connection_result << std::endl;
        return 1;
    }
    else
    {
        std::cout << "connected to the local serial radio!" << std::endl;
    }

    int counter = 0;
    while (mavsdk_drone.systems().size() == 0)
    {
        std::cout << counter++ << "-trying to detect a peer system..." << std::endl;
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

    std::cout << "peer system detected... " << std::endl;

    // Get the connected system
    peer_system_ = mavsdk_drone.systems()[0];
    if (!peer_system_->is_connected())
    {
        std::cerr << "peer system not connected!" << std::endl;
        return 1;
    }
    else
    {
        std::cout << "peer system connected!" << std::endl;
    }

    auto mavlink_direct = MavlinkDirect{peer_system_};

    std::string custom_xml = R"(
    <mavlink>
        <messages>
            <message id="42000" name="MY_DRONE_GLOBAL_POSITION">
                <field type="float" name="latitude">latitude</field>
                <field type="float" name="longitude">longitude</field>
                <field type="float" name="altitude">altitude</field>
            </message>
        </messages>
    </mavlink>)";

    // Load custom XML
    MavlinkDirect::Result xml_result = mavlink_direct.load_custom_xml(custom_xml);

    if (xml_result != MavlinkDirect::Result::Success)
    {
        std::cerr << "Failed to load custom XML: " << xml_result << std::endl;
        throw std::runtime_error("Failed to load custom XML");
    }

    MavlinkDirect::MavlinkMessage my_drone_global_position{};


    auto tm_msg_handle = mavlink_direct.subscribe_message("MY_DRONE_GLOBAL_POSITION",
                                                          [](const mavsdk::MavlinkDirect::MavlinkMessage &message)
                                                          {
                                                              std::cout << "Telemetry received from " << message.system_id << std::endl;
                                                              std::cout << message.fields_json << std::endl;
                                                          });

    std::cout << "Listening for GPS messages. Press Ctrl+C to exit..." << std::endl;
    while (true)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }

    return 0;
}

[pedramsafi]

Thanks. Have you re-tested after #2694?

I have not. I will test again!