Arm authorization

examples/CMakeLists.txt

@@ -7,6 +7,7 @@ if (WERROR)
     add_compile_options(-Werror)
 endif()
 
+add_subdirectory(arm_authorizer_server)
 add_subdirectory(autopilot_server)
 add_subdirectory(battery)
 add_subdirectory(calibrate)

examples/arm_authorizer_server/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+project(arm_authorizer_server)
+
+add_executable(arm_authorizer_server
+        arm_authorizer_server.cpp
+)
+
+find_package(MAVSDK REQUIRED)
+
+target_link_libraries(arm_authorizer_server
+        MAVSDK::mavsdk
+)
+
+if (NOT MSVC)
+    add_compile_options(arm_authorizer_server PRIVATE -Wall -Wextra)
+else ()
+    add_compile_options(arm_authorizer_server PRIVATE -WX -W2)
+endif ()

examples/arm_authorizer_server/arm_authorizer_server.cpp

@@ -0,0 +1,66 @@
+#include <iostream>
+#include <thread>
+#include <chrono>
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/arm_authorizer_server/arm_authorizer_server.h>
+#include <chrono>
+
+constexpr uint32_t ARM_AUTHORIZATION_DELAY_S = 5;
+
+using mavsdk::ArmAuthorizerServer;
+
+int main(int argc, char** argv)
+{
+    if (argc < 2) {
+        std::cerr << "Too few input arguments. usage: $ arm_authorizer_server <connection_url>"
+                  << std::endl;
+        return 1;
+    }
+
+    // Create mavlink component with CompanionComputer id
+    mavsdk::Mavsdk mavsdk{
+        mavsdk::Mavsdk::Configuration{mavsdk::Mavsdk::ComponentType::GroundStation}};
+
+    auto result = mavsdk.add_any_connection(argv[1]);
+
+    if (result != mavsdk::ConnectionResult::Success) {
+        std::cerr << "Could not establish connection: " << result << std::endl;
+        return 2;
+    }
+    std::cout << "Created arm authorizer server connection" << std::endl;
+
+    auto arm_authorizer = ArmAuthorizerServer{mavsdk.server_component()};
+
+    // Rememember the time when execution started. Arming will be authorized after specific time
+    // interval elapsed
+    auto start_time = std::chrono::system_clock::now();
+
+    arm_authorizer.subscribe_arm_authorization([&](uint32_t system_id) {
+        std::cout << "Arm authorization request received. Request for system ID: " << system_id
+                  << std::endl;
+        auto elapsed_s = std::chrono::duration_cast<std::chrono::seconds>(
+                             std::chrono::system_clock::now() - start_time)
+                             .count();
+
+        ArmAuthorizerServer::Result result;
+        // Allow arm if sufficient time has passed
+        if (elapsed_s > ARM_AUTHORIZATION_DELAY_S) {
+            result = arm_authorizer.accept_arm_authorization(5);
+        } else {
+            result = arm_authorizer.reject_arm_authorization(
+                true, ArmAuthorizerServer::RejectionReason::Generic, 10);
+        }
+        if (result == ArmAuthorizerServer::Result::Success) {
+            std::cout << "Successfully sent the response" << std::endl;
+        } else {
+            std::cout << "Failed to send the response" << std::endl;
+        }
+    });
+
+    // works as a server and never quit
+    while (true) {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+
+    return 0;
+}

src/mavsdk/plugins/arm_authorizer_server/CMakeLists.txt

@@ -0,0 +1,15 @@
+target_sources(mavsdk
+    PRIVATE
+    arm_authorizer_server.cpp
+    arm_authorizer_server_impl.cpp
+)
+
+target_include_directories(mavsdk PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include/mavsdk>
+    )
+
+install(FILES
+    include/plugins/arm_authorizer_server/arm_authorizer_server.h
+    DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/mavsdk/plugins/arm_authorizer_server
+)

src/mavsdk/plugins/arm_authorizer_server/arm_authorizer_server.cpp

@@ -0,0 +1,78 @@
+// WARNING: THIS FILE IS AUTOGENERATED! As such, it should not be edited.
+// Edits need to be made to the proto files
+// (see
+// https://github.com/mavlink/MAVSDK-Proto/blob/master/protos/arm_authorizer_server/arm_authorizer_server.proto)
+
+#include <iomanip>
+
+#include "arm_authorizer_server_impl.h"
+#include "plugins/arm_authorizer_server/arm_authorizer_server.h"
+
+namespace mavsdk {
+
+ArmAuthorizerServer::ArmAuthorizerServer(std::shared_ptr<ServerComponent> server_component) :
+    ServerPluginBase(),
+    _impl{std::make_unique<ArmAuthorizerServerImpl>(server_component)}
+{}
+
+ArmAuthorizerServer::~ArmAuthorizerServer() {}
+
+ArmAuthorizerServer::ArmAuthorizationHandle
+ArmAuthorizerServer::subscribe_arm_authorization(const ArmAuthorizationCallback& callback)
+{
+    return _impl->subscribe_arm_authorization(callback);
+}
+
+void ArmAuthorizerServer::unsubscribe_arm_authorization(ArmAuthorizationHandle handle)
+{
+    _impl->unsubscribe_arm_authorization(handle);
+}
+
+ArmAuthorizerServer::Result
+ArmAuthorizerServer::accept_arm_authorization(int32_t valid_time_s) const
+{
+    return _impl->accept_arm_authorization(valid_time_s);
+}
+
+ArmAuthorizerServer::Result ArmAuthorizerServer::reject_arm_authorization(
+    bool temporarily, RejectionReason reason, int32_t extra_info) const
+{
+    return _impl->reject_arm_authorization(temporarily, reason, extra_info);
+}
+
+std::ostream& operator<<(std::ostream& str, ArmAuthorizerServer::Result const& result)
+{
+    switch (result) {
+        case ArmAuthorizerServer::Result::Unknown:
+            return str << "Unknown";
+        case ArmAuthorizerServer::Result::Success:
+            return str << "Success";
+        case ArmAuthorizerServer::Result::Failed:
+            return str << "Failed";
+        default:
+            return str << "Unknown";
+    }
+}
+
+std::ostream&
+operator<<(std::ostream& str, ArmAuthorizerServer::RejectionReason const& rejection_reason)
+{
+    switch (rejection_reason) {
+        case ArmAuthorizerServer::RejectionReason::Generic:
+            return str << "Generic";
+        case ArmAuthorizerServer::RejectionReason::None:
+            return str << "None";
+        case ArmAuthorizerServer::RejectionReason::InvalidWaypoint:
+            return str << "Invalid Waypoint";
+        case ArmAuthorizerServer::RejectionReason::Timeout:
+            return str << "Timeout";
+        case ArmAuthorizerServer::RejectionReason::AirspaceInUse:
+            return str << "Airspace In Use";
+        case ArmAuthorizerServer::RejectionReason::BadWeather:
+            return str << "Bad Weather";
+        default:
+            return str << "Unknown";
+    }
+}
+
+} // namespace mavsdk

src/mavsdk/plugins/arm_authorizer_server/arm_authorizer_server_impl.cpp

@@ -0,0 +1,118 @@
+#include "arm_authorizer_server_impl.h"
+
+#include <utility>
+#include "log.h"
+
+namespace mavsdk {
+
+ArmAuthorizerServerImpl::ArmAuthorizerServerImpl(
+    std::shared_ptr<ServerComponent> server_component) :
+    ServerPluginImplBase(std::move(server_component))
+{
+    _server_component_impl->register_plugin(this);
+}
+
+ArmAuthorizerServerImpl::~ArmAuthorizerServerImpl()
+{
+    _server_component_impl->unregister_plugin(this);
+}
+
+void ArmAuthorizerServerImpl::init()
+{
+    _server_component_impl->register_mavlink_command_handler(
+        MAV_CMD_ARM_AUTHORIZATION_REQUEST,
+        [this](const MavlinkCommandReceiver::CommandLong& command) {
+            return process_arm_authorization_request(command);
+        },
+        this);
+}
+
+void ArmAuthorizerServerImpl::deinit() {}
+
+ArmAuthorizerServer::ArmAuthorizationHandle ArmAuthorizerServerImpl::subscribe_arm_authorization(
+    const ArmAuthorizerServer::ArmAuthorizationCallback& callback)
+{
+    return _arm_authorization_callbacks.subscribe(callback);
+}
+
+void ArmAuthorizerServerImpl::unsubscribe_arm_authorization(
+    ArmAuthorizerServer::ArmAuthorizationHandle handle)
+{
+    _arm_authorization_callbacks.unsubscribe(handle);
+}
+
+std::optional<mavlink_command_ack_t> ArmAuthorizerServerImpl::process_arm_authorization_request(
+    const MavlinkCommandReceiver::CommandLong& command)
+{
+    if (_arm_authorization_callbacks.empty()) {
+        LogDebug() << "Set mode requested with no user callback";
+        return _server_component_impl->make_command_ack_message(
+            command, MAV_RESULT::MAV_RESULT_UNSUPPORTED);
+    }
+    auto const system_id = static_cast<uint32_t>(command.params.param1);
+
+    _last_arm_authorization_request_command = command;
+    _arm_authorization_callbacks(system_id);
+
+    return std::nullopt;
+}
+
+ArmAuthorizerServer::Result
+ArmAuthorizerServerImpl::accept_arm_authorization(int32_t valid_time_s) const
+{
+    auto command_ack = _server_component_impl->make_command_ack_message(
+        _last_arm_authorization_request_command, MAV_RESULT_ACCEPTED);
+
+    // If the arm is authorized, param2 is set to the time in seconds through which the
+    // authorization is valid
+    command_ack.result_param2 = valid_time_s;
+
+    if (!_server_component_impl->send_command_ack(command_ack)) {
+        return ArmAuthorizerServer::Result::Failed;
+    }
+    return ArmAuthorizerServer::Result::Success;
+}
+
+ArmAuthorizerServer::Result ArmAuthorizerServerImpl::reject_arm_authorization(
+    bool temporarily, ArmAuthorizerServer::RejectionReason reason, int32_t extra_info) const
+{
+    MAV_RESULT result = temporarily ? MAV_RESULT_TEMPORARILY_REJECTED : MAV_RESULT_DENIED;
+    auto command_ack = _server_component_impl->make_command_ack_message(
+        _last_arm_authorization_request_command, result);
+
+    // Fill in progress, which is the reason for arm rejected or 0 if arm was allowed
+    switch (reason) {
+        case ArmAuthorizerServer::RejectionReason::Generic:
+            command_ack.progress = static_cast<uint8_t>(MAV_ARM_AUTH_DENIED_REASON_GENERIC);
+            break;
+        case ArmAuthorizerServer::RejectionReason::Timeout:
+            command_ack.progress = static_cast<uint8_t>(MAV_ARM_AUTH_DENIED_REASON_TIMEOUT);
+            break;
+        case ArmAuthorizerServer::RejectionReason::BadWeather:
+            command_ack.progress = static_cast<uint8_t>(MAV_ARM_AUTH_DENIED_REASON_BAD_WEATHER);
+            break;
+        case ArmAuthorizerServer::RejectionReason::InvalidWaypoint:
+            command_ack.progress =
+                static_cast<uint8_t>(MAV_ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT);
+            break;
+        case ArmAuthorizerServer::RejectionReason::AirspaceInUse:
+            command_ack.progress = static_cast<uint8_t>(MAV_ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE);
+            break;
+        case ArmAuthorizerServer::RejectionReason::None:
+            [[fallthrough]];
+        default:
+            command_ack.progress = static_cast<uint8_t>(MAV_ARM_AUTH_DENIED_REASON_NONE);
+            break;
+    }
+
+    // Fill in the extra information. It is dependent on the decision, and and reason
+    command_ack.result_param2 = extra_info;
+
+    if (!_server_component_impl->send_command_ack(command_ack)) {
+        return ArmAuthorizerServer::Result::Failed;
+    }
+
+    return ArmAuthorizerServer::Result::Success;
+}
+
+} // namespace mavsdk

src/mavsdk/plugins/arm_authorizer_server/arm_authorizer_server_impl.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include "plugins/arm_authorizer_server/arm_authorizer_server.h"
+#include "server_plugin_impl_base.h"
+#include "callback_list.h"
+
+namespace mavsdk {
+
+class ArmAuthorizerServerImpl : public ServerPluginImplBase {
+public:
+    explicit ArmAuthorizerServerImpl(std::shared_ptr<ServerComponent> server_component);
+
+    ~ArmAuthorizerServerImpl() override;
+
+    void init() override;
+    void deinit() override;
+
+    ArmAuthorizerServer::ArmAuthorizationHandle
+    subscribe_arm_authorization(const ArmAuthorizerServer::ArmAuthorizationCallback& callback);
+
+    void unsubscribe_arm_authorization(ArmAuthorizerServer::ArmAuthorizationHandle handle);
+
+    ArmAuthorizerServer::Result accept_arm_authorization(int32_t valid_time_s) const;
+    ArmAuthorizerServer::Result reject_arm_authorization(
+        bool temporarily, ArmAuthorizerServer::RejectionReason reason, int32_t extra_info) const;
+
+private:
+    CallbackList<uint32_t> _arm_authorization_callbacks{};
+
+    MavlinkCommandReceiver::CommandLong _last_arm_authorization_request_command;
+
+    std::optional<mavlink_command_ack_t>
+    process_arm_authorization_request(const MavlinkCommandReceiver::CommandLong& command);
+};
+
+} // namespace mavsdk