Skip to content

Commit

Permalink
Merge pull request #2649 from microsoft/clovett/mavlink_update
Browse files Browse the repository at this point in the history 
Fix AirSim so it works with PX4 master bits as of 5/1/2020
  • Loading branch information
@lovettchris
lovettchris authored May 28, 2020
2 parents c0bb4f9 + 3fdc5a9 commit 3ec25d5
Show file tree
Hide file tree
Showing 10 changed files with 6,767 additions and 119 deletions.
80 changes: 37 additions & 43 deletions AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,6 @@

namespace msr { namespace airlib {


class MavLinkMultirotorApi : public MultirotorApiBase
{
public: //methods
Expand Down Expand Up @@ -933,11 +932,11 @@ class MavLinkMultirotorApi : public MultirotorApiBase
mavlinkcom::SerialPortInfo info = *iter;
if ((
(info.vid == pixhawkVendorId) &&
(info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId)
) ||
(
(info.displayName.find(L"PX4_") != std::string::npos)
)) {
(info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId)
) ||
(
(info.displayName.find(L"PX4_") != std::string::npos)
)) {
// printf("Auto Selecting COM port: %S\n", info.displayName.c_str());

std::string portName_str;
Expand Down Expand Up @@ -1026,30 +1025,28 @@ class MavLinkMultirotorApi : public MultirotorApiBase
if (connection_info_.control_port == 0) {
throw std::invalid_argument("ControlPort setting has an invalid value.");
}
connectionDelayed_ = true;
}
else {
connectVehicle();
}
}

void connectVehicle()
{
// The PX4 SITL mode app cannot receive commands to control the drone over the same HIL mavlink connection.
// The HIL mavlink connection can only handle HIL_SENSOR messages. This separate channel is needed for
// everything else.
addStatusMessage(Utils::stringf("Connecting to PX4 Control UDP port %d, local IP %s, remote IP...",
connection_info_.control_port, connection_info_.local_host_ip.c_str(), connection_info_.control_ip_address.c_str()));
// The PX4 SITL mode app cannot receive commands to control the drone over the same HIL mavlink connection.
// The HIL mavlink connection can only handle HIL_SENSOR messages. This separate channel is needed for
// everything else.
addStatusMessage(Utils::stringf("Connecting to PX4 Control UDP port %d, local IP %s, remote IP...",
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));
// 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);
auto gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs",
connection_info_.local_host_ip, connection_info_.control_ip_address, connection_info_.control_port);
mav_vehicle_->connect(gcsConnection);

addStatusMessage(std::string("Ground control connected over UDP."));
addStatusMessage(std::string("Ground control connected over UDP."));
}

connectVehicle();
}

void connectVehicle()
{
// listen to this UDP mavlink connection also
auto mavcon = mav_vehicle_->getConnection();
if (mavcon != connection_) {
Expand All @@ -1065,6 +1062,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
connected_ = true;
// now we can start our heartbeats.
mav_vehicle_->startHeartbeat();

// Also request home position messages
mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1);
}

void createMavSerialConnection(const std::string& port_name, int baud_rate)
Expand Down Expand Up @@ -1219,10 +1219,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
is_controls_0_1_ = false;
}

if (connectionDelayed_) {
connectionDelayed_ = false;
connectVehicle();
}
send_params_ = true;
}
else if (is_simulation_mode_ && !is_hil_mode_set_) {
Expand Down Expand Up @@ -1282,11 +1278,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
received_actuator_controls_ = true;
// if the timestamps match then it means we are in lockstep mode.
if (!lock_step_enabled_) {
if (HilActuatorControlsMessage.flags & 0x1) {
addStatusMessage("Lockstep flag enabled");
lock_step_enabled_ = true;
}
else if (hil_sensor_clock_ == HilActuatorControlsMessage.time_usec) {
// && (HilActuatorControlsMessage.flags & 0x1)) // todo: enable this check when this flag is widely available...
if (hil_sensor_clock_ == HilActuatorControlsMessage.time_usec) {
addStatusMessage("Enabling lockstep mode");
lock_step_enabled_ = true;
}
Expand All @@ -1300,7 +1293,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
if (locked && !has_gps_lock_) {
addStatusMessage("Got GPS lock");
has_gps_lock_ = true;
}
}
if (!has_home_ && current_state_.home.is_set) {
addStatusMessage("Got GPS Home Location");
has_home_ = true;
Expand All @@ -1318,6 +1311,10 @@ class MavLinkMultirotorApi : public MultirotorApiBase
// check landed state.
getLandedState();
}
else if (msg.msgid == mavlinkcom::MavLinkHomePosition::kMessageId) {
mavlinkcom::MavLinkHomePosition home;
home.decode(msg);
}
//else ignore message
}

Expand All @@ -1335,7 +1332,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
}

if (!received_actuator_controls_) {
last_hil_sensor_time_ = now;
// drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage.
return;
}
Expand Down Expand Up @@ -1416,7 +1412,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
mavlinkcom::MavLinkHilGps hil_gps;
hil_gps.time_usec = hil_sensor_clock_;
hil_gps.lat = static_cast<int32_t>(geo_point.latitude * 1E7);
hil_gps.lon = static_cast<int32_t>(geo_point.longitude* 1E7);
hil_gps.lon = static_cast<int32_t>(geo_point.longitude * 1E7);
hil_gps.alt = static_cast<int32_t>(geo_point.altitude * 1000);
hil_gps.vn = static_cast<int16_t>(velocity.x() * 100);
hil_gps.ve = static_cast<int16_t>(velocity.y() * 100);
Expand All @@ -1426,7 +1422,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
hil_gps.fix_type = static_cast<uint8_t>(fix_type);
hil_gps.vel = static_cast<uint16_t>(velocity_xy * 100);
hil_gps.cog = static_cast<uint16_t>(cog * 100);
hil_gps.satellites_visible = static_cast<uint8_t>(satellites_visible);
hil_gps.satellites_visible = static_cast<uint8_t>(15);

if (hil_node_ != nullptr) {
hil_node_->sendMessage(hil_gps);
Expand Down Expand Up @@ -1534,7 +1530,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
uint64_t last_gps_time_ = 0;
uint64_t last_hil_sensor_time_ = 0;
uint64_t hil_sensor_clock_ = 0;
bool was_reset_ = false;
bool was_reset_ = false;
bool has_home_ = false;
bool is_ready_ = false;
bool has_gps_lock_ = false;
Expand All @@ -1545,7 +1541,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
std::thread connect_thread_;
bool connecting_ = false;
bool connected_ = false;
bool connectionDelayed_ = false;
common_utils::SmoothingFilter<float> ground_filter_;
double ground_variance_ = 1;
const double GroundTolerance = 0.1;
Expand All @@ -1564,7 +1559,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
mutable int state_version_;
mutable mavlinkcom::VehicleState current_state_;
};


}} //namespace
}
} //namespace
#endif
6 changes: 1 addition & 5 deletions MavLinkCom/MavLinkComGenerator/regen.cmd
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,10 @@
@echo off
set SCRIPT_PATH=%~dp0
cd %~dp0
if "%1" == "" goto :usage
msbuild MavLinkComGenerator.csproj

bin\Debug\MavLinkComGenerator.exe -xml:%1 -out:%SCRIPT_PATH%\..\include
bin\Debug\MavLinkComGenerator.exe -xml:%SCRIPT_PATH%\..\mavlink\message_definitions\common.xml -out:%SCRIPT_PATH%\..\include
copy ..\include\MavLinkMessages.cpp ..\src
del ..\include\MavLinkMessages.cpp

goto :eof

:usage
echo we need the location of the mavlink 2.0 "common.xml" as input argument
Loading

0 comments on commit 3ec25d5

Please sign in to comment.