MavLinkCom is a cross-platform C++ library that helps connect to and communicate with MavLink based vehicles. Specifically this library is designed to work well with PX4 based drones. To build the library you need Boost. See Installing Boost below details on installing Boost.
You can view the Design.dgml diagram in Visual Studio. The following are the most important classes in this library.
This is the base class for all MavLinkNodes (subclasses include MavLinkVehicle, MavLinkVideoClient and MavLinkVideoServer). The node connects to your mavlink enabled vehicle via a MavLinkConnection and provides methods for sending MavLinkMessages and MavLinkCommands and for subscribing to receive messages. This base class also stores the local system id and component id your app wants to use to identify itself to your remove vehicle. You can also call startHeartbeat to send regular heartbeat messages to keep the connection alive.
This is the encoded MavLinkMessage. For those who have used the mavlink.h C API, this is similar to mavlink_message_t. You do not create these manually, they are encoded from a strongly typed MavLinkMessageBase subclass.
The MavLinkComGenerator parses the mavlink common.xml message definitions and generates all the MavLink* MavLinkMessageBase subclasses as well as a bunch of handy mavlink enums and a bunch of strongly typed MavLinkCommand subclasses.
This is the base class for a set of strongly typed message classes that are code generated by the MavLinkComGenerator project. This replaces the C messages defined in the mavlink C API and provides a slightly more object oriented way to send and receive messages via sendMessage on MavLinkNode. These classes have encode/decode methods that convert to and from the MavLinkMessage class.
This is the base class for a set of strongly typed command classes that are code generated by the MavLinkComGenerator project. This replaces the C definitions defined in the mavlink C API and provides a more object oriented way to send commands via the sendCommand method on MavLinkNode. The MavLinkNode takes care of turning these into the underlying mavlink COMMAND_LONG message.
This class provides static helper methods for creating connections to remote MavLink nodes, over serial ports, as well as UDP, or TCP sockets. This class provides a way to subscribe to receive messages from that node in a pub/sub way so you can have multiple subscribers on the same connection. MavLinkVehicle uses this to track various messages that define the overall vehicle state.
MavLinkVehicle is a MavLinkNode that tracks various messages that define the overall vehicle state and provides a VehicleState struct containing a snapshot of that state, including home position, current orientation, local position, global position, and so on. This class also provides a bunch of helper methods that wrap commonly used commands providing simle method calls to do things like arm, disarm, takeoff, land, go to a local coordinate, and fly under offbaord control either by position or velocity control.
This helper class provides a way to setup a "server" that accepts MavLinkConnections from remote nodes. You can use this class to get a connection that you can then give to MavLinkVideoServer to serve images over MavLink.
This helper class takes a given MavLinkConnection and provides FTP client support for the MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL for vehicles that support the FTP capability. This class provides simple methods to list directory contents, and the get and put files.
This helper class takes a given MavLinkConnection and provides helper methods for requesting video from remote node and packaging up the MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE and MAVLINK_MSG_ID_ENCAPSULATED_DATA messages into simple to use MavLinkVideoFrames.
This helper class takes a given MavLinkConnection and provides the server side of the MavLinkVideoClient protocol, including helper methods for notifying when there is a video request to process (hasVideoRequest) and a method to send video frames (sendFrame) which will generate the right MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE and MAVLINK_MSG_ID_ENCAPSULATED_DATA sequence.
The following code from the UnitTest project shows how to connect to a Pixhawk flight controller over USB serial port, then wait for the first heartbeat message to be received:
auto connection = MavLinkConnection::connectSerial("drone", "/dev/ttyACM0", 115200, "sh /etc/init.d/rc.usb\n");
MavLinkHeartbeat heartbeat;
if (!waitForHeartbeat(10000, heartbeat)) {
throw std::runtime_error("Received no heartbeat from PX4 after 10 seconds");
}
The following code connects to serial port, and then forwards all messages to and from QGroundControl to that drone using another connection that is joined to the drone stream.
auto droneConnection = MavLinkConnection::connectSerial("drone", "/dev/ttyACM0", 115200, "sh /etc/init.d/rc.usb\n");
auto proxyConnection = MavLinkConnection::connectRemoteUdp("qgc", "127.0.0.1", "127.0.0.1", 14550);
droneConnection->join(proxyConnection);
The following code then takes that connection and turns on heartBeats and starts tracking vehicle information using local system id 166 and component id 1.
auto vehicle = std::make_shared<MavLinkVehicle>(166, 1);
vehicle->connect(connection);
vehicle->startHeartbeat();
std::this_thread::sleep_for(std::chrono::seconds(5));
VehicleState state = vehicle->getVehicleState();
printf("Home position is %s, %f,%f,%f\n", state.home.is_set ? "set" : "not set",
state.home.global_pos.lat, state.home.global_pos.lon, state.home.global_pos.alt);
The following code uses the vehicle object to arm the drone and take off and wait for the takeoff altitude to be reached:
bool rc = false;
if (!vehicle->armDisarm(true).wait(3000, &rc) || !rc) {
printf("arm command failed\n");
return;
}
if (!vehicle->takeoff(targetAlt).wait(3000, &rc) || !rc) {
printf("takeoff command failed\n");
return;
}
int version = vehicle->getVehicleStateVersion();
while (true) {
int newVersion = vehicle->getVehicleStateVersion();
if (version != newVersion) {
VehicleState state = vehicle->getVehicleState();
float alt = state.local_est.pos.z;
if (alt >= targetAlt - delta && alt <= targetAlt + delta)
{
reached = true;
printf("Target altitude reached\n");
break;
}
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
The following code uses offboard control to make the drone fly in a circle with camera pointed at the center. Here we use the subscribe method to check each new local position message to indicate so we can compue the new velocity vector as soon as that new position is received. We request a high rate for those messages using setMessageInterval to ensure smooth circular orbit.
vehicle->setMessageInterval((int)MavLinkMessageIds::MAVLINK_MSG_ID_LOCAL_POSITION_NED, 30);
vehicle->requestControl();
int subscription = vehicle->getConnection()->subscribe(
[&](std::shared_ptr<MavLinkConnection> connection, const MavLinkMessage& m) {
if (m.msgid == (int)MavLinkMessageIds::MAVLINK_MSG_ID_LOCAL_POSITION_NED)
{
float x = localPos.x;
float y = localPos.y;
float dx = x - cx;
float dy = y - cy;
float angle = atan2(dy, dx);
if (angle < 0) angle += M_PI * 2;
float tangent = angle + M_PI_2;
double newvx = orbitSpeed * cos(tangent);
double newvy = orbitSpeed * sin(tangent);
float heading = angle + M_PI;
vehicle->moveByLocalVelocityWithAltHold(newvx, newvy, altitude, true, heading);
}
});
The following code stops flying the drone in offboard mode and tells the drone to loiter at its current location. This version of the code shows how to use the AsyncResult without blocking on a wait call.
vehicle->releaseControl();
if (vehicle->loiter().then([=](bool rc) {
printf("loiter command %s\n", rc ? "suceeded" : "failed");
}
The following code gets all configurable parameters from the drone and prints them:
auto list = vehicle->getParamList();
auto end = list.end();
int count = 0;
for (auto iter = list.begin(); iter < end; iter++)
{
count++;
MavLinkParameter p = *iter;
if (p.type == MAV_PARAM_TYPE_REAL32 || p.type == MAV_PARAM_TYPE_REAL64) {
printf("%s=%f\n", p.name.c_str(), p.value);
}
else {
printf("%s=%d\n", p.name.c_str(), static_cast<int>(p.value));
}
}
The following code sets a parameter on the Pixhawk to disable the USB safety check (this is handy if you are controlling the Pixhawk over USB using another onboard computer that is part of the drone itself). You should NOT do this if you are connecting your PC or laptop to the drone over USB.
MavLinkParameter p;
p.name = "CBRK_USB_CHK";
p.value = 197848;
if (!vehicle->setParameter(p).wait(3000,&rc) || !rc) {
printf("Setting the CBRK_USB_CHK failed");
}
MavLinkVehicle actually has a helper method for this called allowFlightControlOverUsb, so now you know how it is implemented :-)
- download boost 1.63 zip file
- unzip boost someplace and then set an environment variable named BOOST_ROOT that points to the folder you just unzippsed. It should point to the root folder that contains 'bootstrap.bat'.
- open developer command prompt for VS2015 and cd over to the boost folder and run' bootstrapbat'
- from that same location run b2 variant=debug,release link=static runtime-link=shared threading=multi address-model=64 and wait about 20 minutes (coffee time :-)
- (on linux you can drop the address-model=64).
Now start a new instance of Visual Studio 2015 (so it finds your new BOOST_ROOT variable), and load "~\MavLinkTest\MavLinkTest.sln" and select "x64" build target. select MavLinkTest as the startup project, then hit F5.