From f86e84de530624f82815319cac37fe985e9291ec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 13 Oct 2024 14:16:08 +1300 Subject: [PATCH 1/3] examples: cleanup autopilot server - Remove parameters that are now unused. - Clean up thread properly. - Use udpout syntax. --- .../autopilot_server/autopilot_server.cpp | 56 +++++++++++-------- 1 file changed, 34 insertions(+), 22 deletions(-) diff --git a/examples/autopilot_server/autopilot_server.cpp b/examples/autopilot_server/autopilot_server.cpp index 0366b62cf..ce2f57445 100644 --- a/examples/autopilot_server/autopilot_server.cpp +++ b/examples/autopilot_server/autopilot_server.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -55,13 +56,15 @@ Mission::MissionItem make_mission_item( int main(int argc, char** argv) { + std::atomic _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; } @@ -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}); @@ -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; }); @@ -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 @@ -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}}; @@ -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; @@ -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; } @@ -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; } @@ -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; } From 64f803882f0389ce4c80e10f9a2888d3627204b6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 13 Oct 2024 14:45:22 +1300 Subject: [PATCH 2/3] core: fix/improve debug output --- src/mavsdk/core/mavlink_command_receiver.cpp | 26 +++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/src/mavsdk/core/mavlink_command_receiver.cpp b/src/mavsdk/core/mavlink_command_receiver.cpp index b16913513..ccbe61574 100644 --- a/src/mavsdk/core/mavlink_command_receiver.cpp +++ b/src/mavsdk/core/mavlink_command_receiver.cpp @@ -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; } @@ -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. @@ -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); } } } @@ -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; } @@ -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. @@ -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); } } } From eb41afdc90514c44a94b130cb7592a82e3463fed Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 13 Oct 2024 14:45:37 +1300 Subject: [PATCH 3/3] core: prevent invalid duplicate acks It turns out we respond with two acks to a MAV_REQUEST_MESSAGE when we should only respond once with the successful one. Alternatively, we could first grab all the results and then do some magic to determine which one is the best ack but that's a bit too magic and complex, so I'm not doing that yet. --- .../telemetry_server_impl.cpp | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp b/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp index 1f59dc710..f68d04772 100644 --- a/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp +++ b/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp @@ -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{ + _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{ + _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{ + _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{}; } }, this);