Skip to content

Commit

Permalink
Merge pull request #2423 from mavlink/pr-autopilot-server-fixes
Browse files Browse the repository at this point in the history 
Autopilot server and command server fixes
  • Loading branch information
@julianoes
julianoes authored Oct 13, 2024
2 parents ca0c41e + eb41afd commit ea06648
Show file tree
Hide file tree
Showing 3 changed files with 61 additions and 42 deletions.
56 changes: 34 additions & 22 deletions examples/autopilot_server/autopilot_server.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
#include <atomic>
#include <future>
#include <iostream>
#include <thread>
Expand Down Expand Up @@ -55,13 +56,15 @@ Mission::MissionItem make_mission_item(

int main(int argc, char** argv)
{
std::atomic<bool> _should_exit{false};

// We run the server plugins on a seperate thread so we can use the main
// thread as a ground station.
std::thread autopilotThread([]() {
std::thread autopilot_thread([&_should_exit]() {
mavsdk::Mavsdk mavsdkTester{
mavsdk::Mavsdk::Configuration{mavsdk::ComponentType::Autopilot}};

auto result = mavsdkTester.add_any_connection("udp://127.0.0.1:14551");
auto result = mavsdkTester.add_any_connection("udpout://127.0.0.1:14551");
if (result == mavsdk::ConnectionResult::Success) {
std::cout << "Connected autopilot server side!" << std::endl;
}
Expand All @@ -74,13 +77,9 @@ int main(int argc, char** argv)
auto actionServer = mavsdk::ActionServer{server_component};

// These are needed for MAVSDK at the moment
paramServer.provide_param_int("CAL_ACC0_ID", 1);
paramServer.provide_param_int("CAL_GYRO0_ID", 1);
paramServer.provide_param_int("CAL_MAG0_ID", 1);
paramServer.provide_param_int("SYS_HITL", 0);
paramServer.provide_param_int("MIS_TAKEOFF_ALT", 0);
// Add a custom param
paramServer.provide_param_int("my_param", 1);
paramServer.provide_param_int("MY_PARAM", 1);

// Allow the vehicle to change modes, takeoff and arm
actionServer.set_allowable_flight_modes({true, true, true});
Expand Down Expand Up @@ -117,8 +116,7 @@ int main(int argc, char** argv)
mission_prom.set_value(plan);
});
missionRawServer.subscribe_current_item_changed([](MissionRawServer::MissionItem item) {
std::cout << "Current Mission Item Changed!" << std::endl;
std::cout << "Current Item: " << item << std::endl;
std::cout << "Current item changed: " << item << std::endl;
});
missionRawServer.subscribe_clear_all(
[](uint32_t clear_all) { std::cout << "Clear All Mission!" << std::endl; });
Expand All @@ -135,7 +133,13 @@ int main(int argc, char** argv)
}
});

while (true) {
actionServer.subscribe_land([&position](ActionServer::Result result, bool land) {
if (result == ActionServer::Result::Success) {
position.relative_altitude_m = 0;
}
});

while (!_should_exit.load()) {
std::this_thread::sleep_for(std::chrono::seconds(1));

// Publish the telemetry
Expand All @@ -150,6 +154,12 @@ int main(int argc, char** argv)
}
});

// Before exiting main, we need to make sure we clean up our thread.
auto cleanup_autopilot_thread = [&]() {
_should_exit.store(true);
autopilot_thread.join();
};

// Now this is the main thread, we run client plugins to act as the GCS
// to communicate with the autopilot server plugins.
mavsdk::Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}};
Expand Down Expand Up @@ -226,6 +236,7 @@ int main(int argc, char** argv)
const Mission::Result result = future_result.get();
if (result != Mission::Result::Success) {
std::cout << "Mission upload failed (" << result << "), exiting." << std::endl;
cleanup_autopilot_thread();
return 1;
}
std::cout << "Mission uploaded." << std::endl;
Expand All @@ -239,12 +250,13 @@ int main(int argc, char** argv)
});
}

std::this_thread::sleep_for(std::chrono::seconds(20));
std::this_thread::sleep_for(std::chrono::seconds(3));

// We want to listen to the altitude of the drone at 1 Hz.
auto set_rate_result = telemetry.set_rate_position(1.0);
if (set_rate_result != mavsdk::Telemetry::Result::Success) {
std::cout << "Setting rate failed:" << set_rate_result << std::endl;
cleanup_autopilot_thread();
return 1;
}

Expand All @@ -270,6 +282,7 @@ int main(int argc, char** argv)

if (arm_result != Action::Result::Success) {
std::cout << "Arming failed:" << arm_result << std::endl;
cleanup_autopilot_thread();
return 1;
}

Expand All @@ -278,19 +291,18 @@ int main(int argc, char** argv)
// Take off
std::cout << "Taking off..." << std::endl;
bool takenOff = false;
while (true) {
const Action::Result takeoff_result = action.takeoff();
if (takeoff_result != Action::Result::Success) {
std::cout << "Takeoff failed!:" << takeoff_result << std::endl;
std::this_thread::sleep_for(std::chrono::seconds(1));
continue;
}
break;
const Action::Result takeoff_result = action.takeoff();
if (takeoff_result != Action::Result::Success) {
std::cout << "Takeoff failed!:" << takeoff_result << std::endl;
cleanup_autopilot_thread();
return 1;
}

while (true) {
std::this_thread::sleep_for(std::chrono::seconds(1));
}
std::this_thread::sleep_for(std::chrono::seconds(3));

std::cout << "Land is not currently implemented, so we're leaving it in-air, exiting."
<< std::endl;

cleanup_autopilot_thread();
return 0;
}
26 changes: 14 additions & 12 deletions src/mavsdk/core/mavlink_command_receiver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,14 +40,15 @@ void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& messag
MavlinkCommandReceiver::CommandInt cmd(message);

if (_debugging) {
LogDebug() << "Received command int " << (int)cmd.command;
LogDebug() << "Received command int " << std::to_string(cmd.command);
}

if (cmd.target_component_id != _server_component_impl.get_own_component_id() &&
cmd.target_component_id != MAV_COMP_ID_ALL) {
if (_debugging) {
LogDebug() << "Ignored command int to component " << (int)cmd.target_component_id
<< " instead of " << (int)_server_component_impl.get_own_component_id();
LogDebug() << "Ignored command int to component "
<< std::to_string(cmd.target_component_id) << " instead of "
<< std::to_string(_server_component_impl.get_own_component_id());
}
return;
}
Expand All @@ -57,7 +58,7 @@ void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& messag
for (auto& handler : _mavlink_command_int_handler_table) {
if (handler.cmd_id == cmd.command) {
if (_debugging) {
LogDebug() << "Handling command int " << (int)cmd.command;
LogDebug() << "Handling command int " << std::to_string(cmd.command);
}

// The client side can pack a COMMAND_ACK as a response to receiving the command.
Expand All @@ -76,8 +77,8 @@ void MavlinkCommandReceiver::receive_command_int(const mavlink_message_t& messag
});

if (_debugging) {
LogDebug() << "Acked command int " << (int)cmd.command << " with "
<< maybe_command_ack.value().result;
LogDebug() << "Acked command int " << std::to_string(cmd.command) << " with "
<< std::to_string(maybe_command_ack.value().result);
}
}
}
Expand All @@ -89,14 +90,15 @@ void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& messa
MavlinkCommandReceiver::CommandLong cmd(message);

if (_debugging) {
LogDebug() << "Received command long " << (int)cmd.command;
LogDebug() << "Received command long " << std::to_string(cmd.command);
}

if (cmd.target_component_id != _server_component_impl.get_own_component_id() &&
cmd.target_component_id != MAV_COMP_ID_ALL) {
if (_debugging) {
LogDebug() << "Ignored command long to component " << (int)cmd.target_component_id
<< " instead of " << (int)_server_component_impl.get_own_component_id();
LogDebug() << "Ignored command long to component "
<< std::to_string(cmd.target_component_id) << " instead of "
<< std::to_string(_server_component_impl.get_own_component_id());
}
return;
}
Expand All @@ -106,7 +108,7 @@ void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& messa
for (auto& handler : _mavlink_command_long_handler_table) {
if (handler.cmd_id == cmd.command) {
if (_debugging) {
LogDebug() << "Handling command long " << (int)cmd.command;
LogDebug() << "Handling command long " << std::to_string(cmd.command);
}

// The client side can pack a COMMAND_ACK as a response to receiving the command.
Expand All @@ -124,8 +126,8 @@ void MavlinkCommandReceiver::receive_command_long(const mavlink_message_t& messa
return response_message;
});
if (_debugging) {
LogDebug() << "Acked command long " << (int)cmd.command << " with "
<< maybe_command_ack.value().result;
LogDebug() << "Acked command long " << std::to_string(cmd.command) << " with "
<< std::to_string(maybe_command_ack.value().result);
}
}
}
Expand Down
21 changes: 13 additions & 8 deletions src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,20 +83,25 @@ void TelemetryServerImpl::init()
case MAVLINK_MSG_ID_HOME_POSITION:
if (_maybe_home) {
if (_send_home()) {
return _server_component_impl->make_command_ack_message(
command, MAV_RESULT::MAV_RESULT_ACCEPTED);
return std::optional<mavlink_command_ack_t>{
_server_component_impl->make_command_ack_message(
command, MAV_RESULT::MAV_RESULT_ACCEPTED)};
} else {
return _server_component_impl->make_command_ack_message(
command, MAV_RESULT::MAV_RESULT_FAILED);
return std::optional<mavlink_command_ack_t>{
_server_component_impl->make_command_ack_message(
command, MAV_RESULT::MAV_RESULT_FAILED)};
}
} else {
return _server_component_impl->make_command_ack_message(
command, MAV_RESULT::MAV_RESULT_DENIED);
return std::optional<mavlink_command_ack_t>{
_server_component_impl->make_command_ack_message(
command, MAV_RESULT::MAV_RESULT_DENIED)};
}

default:
return _server_component_impl->make_command_ack_message(
command, MAV_RESULT::MAV_RESULT_DENIED);
// Let's hope someone else answers and keep silent. In an ideal world we would
// explicitly deny all the ones that we ought to answer but haven't implemented
// yet.
return std::optional<mavlink_command_ack_t>{};
}
},
this);
Expand Down

0 comments on commit ea06648

Please sign in to comment.