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

Jump to bottom

Add retry loop when connecting PX4 SITL control channel. #2986

Merged
merged 2 commits into from
Aug 31, 2020
Merged

Add retry loop when connecting PX4 SITL control channel. #2986

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4d2084d
Add retry loop when connecting PX4 SITL control channel.
lovettchris Aug 27, 2020
a47e698
fixes to shutdown of simulator while still connecting to PX4
lovettchris Aug 28, 2020
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
34 changes: 26 additions & 8 deletions AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -612,6 +612,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
virtual void disconnect() {
addStatusMessage("Disconnecting mavlink vehicle");
connected_ = false;
connecting_ = false;
if (connection_ != nullptr) {
if (is_hil_mode_set_ && mav_vehicle_ != nullptr) {
setNormalMode();
Expand All @@ -625,7 +626,10 @@ class MavLinkMultirotorApi : public MultirotorApiBase
}

if (mav_vehicle_ != nullptr) {
mav_vehicle_->getConnection()->stopLoggingSendMessage();
auto c = mav_vehicle_->getConnection();
if (c != nullptr) {
c->stopLoggingSendMessage();
}
mav_vehicle_->close();
mav_vehicle_ = nullptr;
}
Expand All @@ -652,6 +656,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
void connect_thread()
{
addStatusMessage("Waiting for mavlink vehicle...");
connecting_ = true;
createMavConnection(connection_info_);
if (mav_vehicle_ != nullptr) {
connectToLogViewer();
Expand Down Expand Up @@ -966,6 +971,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
{
close();

connecting_ = true;
got_first_heartbeat_ = false;
is_hil_mode_set_ = false;
is_armed_ = false;
Expand Down Expand Up @@ -1031,13 +1037,24 @@ class MavLinkMultirotorApi : public MultirotorApiBase
connection_info_.control_port, connection_info_.local_host_ip.c_str(), connection_info_.control_ip_address.c_str()));

// if we try and connect the UDP port too quickly it doesn't work, bug in PX4 ?
std::this_thread::sleep_for(std::chrono::seconds(2));

auto gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs",
connection_info_.local_host_ip, connection_info_.control_ip_address, connection_info_.control_port);
mav_vehicle_->connect(gcsConnection);
for (int retries = 60; retries >= 0 && connecting_; retries--) {
try {
auto gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs",
connection_info_.local_host_ip, connection_info_.control_ip_address, connection_info_.control_port);
mav_vehicle_->connect(gcsConnection);
}
catch (std::exception&) {
std::this_thread::sleep_for(std::chrono::seconds(1));
}
}

addStatusMessage(std::string("Ground control connected over UDP."));
if (mav_vehicle_->getConnection() != nullptr) {
addStatusMessage(std::string("Ground control connected over UDP."));
}
else {
addStatusMessage(std::string("Timeout trying to connect ground control over UDP."));
return;
}
}

connectVehicle();
Expand Down Expand Up @@ -1069,6 +1086,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
{
close();

connecting_ = true;
bool reported = false;
std::string port_name_auto = port_name;
while (port_name_auto == "" || port_name_auto == "*") {
Expand Down Expand Up @@ -1096,7 +1114,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
addStatusMessage(Utils::stringf("Connecting to PX4 over serial port: %s, baud rate %d ....", port_name_auto.c_str(), baud_rate));
reported = false;

while (true) {
while (connecting_) {
try {
connection_ = mavlinkcom::MavLinkConnection::connectSerial("hil", port_name_auto, baud_rate);
connection_->ignoreMessage(mavlinkcom::MavLinkAttPosMocap::kMessageId); //TODO: find better way to communicate debug pose instead of using fake Mo-cap messages
Expand Down