-
-
Notifications
You must be signed in to change notification settings - Fork 507
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Added arm authorization server functionality
Fixed spelling for arm_authorizer_server Changed component type in arm_authorizer_server example Changes to comply with updates in proto repository Deleted forgotten file proto: update submodule now that PR is merged Signed-off-by: Julian Oes <julian@oes.ch> update submodule again Signed-off-by: Julian Oes <julian@oes.ch>
- Loading branch information
Showing
22 changed files
with
7,488 additions
and
1 deletion.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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(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 () |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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::ReasonGeneric, 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; | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
) |
76 changes: 76 additions & 0 deletions
76
src/mavsdk/plugins/arm_authorizer_server/arm_authorizer_server.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,76 @@ | ||
// 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::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 |
118 changes: 118 additions & 0 deletions
118
src/mavsdk/plugins/arm_authorizer_server/arm_authorizer_server_impl.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
36 changes: 36 additions & 0 deletions
36
src/mavsdk/plugins/arm_authorizer_server/arm_authorizer_server_impl.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
Oops, something went wrong.