Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Autopilot server and command server fixes #2423

Merged
merged 3 commits into from
Oct 13, 2024
Merged

Autopilot server and command server fixes #2423

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f86e84d
examples: cleanup autopilot server
julianoes Oct 13, 2024
64f8038
core: fix/improve debug output
julianoes Oct 13, 2024
eb41afd
core: prevent invalid duplicate acks
julianoes Oct 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
56 changes: 34 additions & 22 deletions examples/autopilot_server/autopilot_server.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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>{};
Comment on lines +101 to +104
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is the issue that multiple components (e.g. autopilot_server or telemetry_server) may process the request, and they can't send MAV_RESULT_DENIED because they don't know if another component will answer? Or do I misunderstand it?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Exactly!

}
},
this);
Expand Down
Loading