Skip to content

Commit

Permalink
examples: add example to set SiK NET ID
Browse files Browse the repository at this point in the history
Signed-off-by: Julian Oes <[email protected]>
  • Loading branch information
julianoes committed Feb 8, 2023
1 parent 023250b commit 0165091
Show file tree
Hide file tree
Showing 3 changed files with 127 additions and 0 deletions.
1 change: 1 addition & 0 deletions examples/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@ add_subdirectory(multiple_drones)
add_subdirectory(offboard)
add_subdirectory(parachute)
add_subdirectory(set_actuator)
add_subdirectory(setup_sik_radio)
add_subdirectory(sniffer)
add_subdirectory(system_info)
add_subdirectory(takeoff_and_land)
Expand Down
22 changes: 22 additions & 0 deletions examples/setup_sik_radio/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
cmake_minimum_required(VERSION 3.10.2)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

project(setup_sik_radio)

add_executable(setup_sik_radio
setup_sik_radio.cpp
)

find_package(MAVSDK REQUIRED)

target_link_libraries(setup_sik_radio
MAVSDK::mavsdk
)

if(NOT MSVC)
add_compile_options(setup_sik_radio PRIVATE -Wall -Wextra)
else()
add_compile_options(setup_sik_radio PRIVATE -WX -W2)
endif()
104 changes: 104 additions & 0 deletions examples/setup_sik_radio/setup_sik_radio.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,104 @@
//
// How to set up a SiK radio by sending a MAVLink command.
//

#include <cstdint>
#include <future>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
#include <iostream>
#include <string>

using namespace mavsdk;
using namespace std::this_thread;
using namespace std::chrono;

void usage(const std::string& bin_name)
{
std::cerr << "Usage : " << bin_name << " <connection_url> <radio_id>\n"
<< "Connection URL format should be :\n"
<< " For TCP : tcp://[server_host][:server_port]\n"
<< " For UDP : udp://[bind_host][:bind_port]\n"
<< " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
<< "For example, to connect to the simulator use URL: udp://:14540\n";
}

std::shared_ptr<System> get_system(Mavsdk& mavsdk)
{
std::cout << "Waiting to discover system...\n";
auto prom = std::promise<std::shared_ptr<System>>{};
auto fut = prom.get_future();

// We wait for new systems to be discovered, once we find one that has an
// autopilot, we decide to use it.
Mavsdk::NewSystemHandle handle = mavsdk.subscribe_on_new_system([&mavsdk, &prom, &handle]() {
auto system = mavsdk.systems().back();

if (system->has_autopilot()) {
std::cout << "Discovered autopilot\n";

// Unsubscribe again as we only want to find one system.
mavsdk.unsubscribe_on_new_system(handle);
prom.set_value(system);
}
});

// We usually receive heartbeats at 1Hz, therefore we should find a
// system after around 3 seconds max, surely.
if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
std::cerr << "No autopilot found.\n";
return {};
}

// Get discovered system now.
return fut.get();
}

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

const int new_radio_id = std::stoi(argv[2]);

Mavsdk mavsdk;
ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);

if (connection_result != ConnectionResult::Success) {
std::cerr << "Connection failed: " << connection_result << '\n';
return 1;
}

auto system = get_system(mavsdk);
if (!system) {
return 1;
}

// Instantiate plugin.
MavlinkPassthrough mavlink_passthrough(system);

std::cout << "Setting radio ID to " << new_radio_id << '\n';

const auto result = mavlink_passthrough.send_command_long(MavlinkPassthrough::CommandLong{
mavlink_passthrough.get_target_sysid(),
mavlink_passthrough.get_target_compid(),
550,
0.f, // all IDs
3.f,
static_cast<float>(new_radio_id),
NAN,
NAN,
NAN,
NAN});

if (result != MavlinkPassthrough::Result::Success) {
std::cerr << "Command failed: " << result << '\n';
return 1;
}

std::cerr << "New radio ID " << new_radio_id << " set\n";

return 0;
}

0 comments on commit 0165091

Please sign in to comment.