Commit 08ab92c3 authored by Lorenz Meier's avatar Lorenz Meier

Merge pull request #772 from mavlink/protobuf_cleanup

Remove protobuf hacks
parents 17202331 0ac30734
......@@ -261,36 +261,6 @@ else:exists(user_config.pri):infile(user_config.pri, DEFINES, DISABLE_GOOGLE_EAR
message("Skipping support for Google Earth view (unsupported platform)")
}
#
# [OPTIONAL] Protcol Buffers for PixHawk
#
LinuxBuild : contains(MAVLINK_DIALECT, pixhawk) {
exists(/usr/local/include/google/protobuf) | exists(/usr/include/google/protobuf) {
message("Including support for Protocol Buffers")
DEFINES += QGC_PROTOBUF_ENABLED
LIBS += \
-lprotobuf \
-lprotobuf-lite \
-lprotoc
HEADERS += \
libs/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.pb.h \
src/ui/map3D/ObstacleGroupNode.h \
src/ui/map3D/GLOverlayGeode.h
SOURCES += \
libs/mavlink/share/mavlink/src/v1.0/pixhawk/pixhawk.pb.cc \
src/ui/map3D/ObstacleGroupNode.cc \
src/ui/map3D/GLOverlayGeode.cc
} else {
warning("Skipping support for Protocol Buffers (missing libraries, see README)")
}
} else {
message("Skipping support for Protocol Buffers (unsupported platform)")
}
#
# [REQUIRED] EIGEN matrix library
# NOMINMAX constant required to make internal min/max work.
......
......@@ -33,10 +33,6 @@
#include "QGCMAVLinkUASFactory.h"
#include "QGC.h"
#ifdef QGC_PROTOBUF_ENABLED
#include <google/protobuf/descriptor.h>
#endif
Q_DECLARE_METATYPE(mavlink_message_t)
/**
......@@ -318,87 +314,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
rstatus.txbuf, rstatus.noise, rstatus.remnoise);
}
#if defined(QGC_PROTOBUF_ENABLED)
if (message.msgid == MAVLINK_MSG_ID_EXTENDED_MESSAGE)
{
mavlink_extended_message_t extended_message;
extended_message.base_msg = message;
// read extended header
uint8_t* payload = reinterpret_cast<uint8_t*>(message.payload64);
memcpy(&extended_message.extended_payload_len, payload + 3, 4);
// Check if message is valid
if
(b.size() != MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_EXTENDED_HEADER_LEN+ extended_message.extended_payload_len)
{
//invalid message
qDebug() << "GOT INVALID EXTENDED MESSAGE, ABORTING";
return;
}
const uint8_t* extended_payload = reinterpret_cast<const uint8_t*>(b.constData()) + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_EXTENDED_HEADER_LEN;
// copy extended payload data
memcpy(extended_message.extended_payload, extended_payload, extended_message.extended_payload_len);
#if defined(QGC_USE_PIXHAWK_MESSAGES)
if (protobufManager.cacheFragment(extended_message))
{
std::tr1::shared_ptr<google::protobuf::Message> protobuf_msg;
if (protobufManager.getMessage(protobuf_msg))
{
const google::protobuf::Descriptor* descriptor = protobuf_msg->GetDescriptor();
if (!descriptor)
{
continue;
}
const google::protobuf::FieldDescriptor* headerField = descriptor->FindFieldByName("header");
if (!headerField)
{
continue;
}
const google::protobuf::Descriptor* headerDescriptor = headerField->message_type();
if (!headerDescriptor)
{
continue;
}
const google::protobuf::FieldDescriptor* sourceSysIdField = headerDescriptor->FindFieldByName("source_sysid");
if (!sourceSysIdField)
{
continue;
}
const google::protobuf::Reflection* reflection = protobuf_msg->GetReflection();
const google::protobuf::Message& headerMsg = reflection->GetMessage(*protobuf_msg, headerField);
const google::protobuf::Reflection* headerReflection = headerMsg.GetReflection();
int source_sysid = headerReflection->GetInt32(headerMsg, sourceSysIdField);
UASInterface* uas = UASManager::instance()->getUASForId(source_sysid);
if (uas != NULL)
{
emit extendedMessageReceived(link, protobuf_msg);
}
}
}
#endif
position += extended_message.extended_payload_len;
continue;
}
#endif
// Log data
if (m_loggingEnabled && m_logfile)
{
......
......@@ -42,15 +42,6 @@ This file is part of the QGROUNDCONTROL project
#include "QGCMAVLink.h"
#include "QGC.h"
#if defined(QGC_PROTOBUF_ENABLED)
#include <tr1/memory>
#include <google/protobuf/message.h>
#if defined(QGC_USE_PIXHAWK_MESSAGES)
#include <mavlink_protobuf_manager.hpp>
#endif
#endif
/**
* @brief MAVLink micro air vehicle protocol reference implementation.
*
......@@ -239,17 +230,9 @@ protected:
int systemId;
bool _should_exit;
#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
mavlink::ProtobufManager protobufManager;
#endif
signals:
/** @brief Message received and directly copied via signal */
void messageReceived(LinkInterface* link, mavlink_message_t message);
#if defined(QGC_PROTOBUF_ENABLED)
/** @brief Message received via signal */
void extendedMessageReceived(LinkInterface *link, std::tr1::shared_ptr<google::protobuf::Message> message);
#endif
/** @brief Emitted if heartbeat emission mode is changed */
void heartbeatChanged(bool heartbeats);
/** @brief Emitted if logging is started / stopped */
......
......@@ -97,18 +97,6 @@ public:
virtual double getPitch() const { Q_ASSERT(false); return std::numeric_limits<double>::quiet_NaN(); };
virtual double getYaw() const { Q_ASSERT(false); return std::numeric_limits<double>::quiet_NaN(); };
virtual bool getSelected() const { Q_ASSERT(false); return false; };
#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
virtual px::GLOverlay getOverlay() { Q_ASSERT(false); };
virtual px::GLOverlay getOverlay(qreal& receivedTimestamp) { Q_ASSERT(false); };
virtual px::ObstacleList getObstacleList() { Q_ASSERT(false); };
virtual px::ObstacleList getObstacleList(qreal& receivedTimestamp) { Q_ASSERT(false); };
virtual px::Path getPath() { Q_ASSERT(false); };
virtual px::Path getPath(qreal& receivedTimestamp) { Q_ASSERT(false); };
virtual px::PointCloudXYZRGB getPointCloud() { Q_ASSERT(false); };
virtual px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) { Q_ASSERT(false); };
virtual px::RGBDImage getRGBDImage() { Q_ASSERT(false); };
virtual px::RGBDImage getRGBDImage(qreal& receivedTimestamp) { Q_ASSERT(false); };
#endif
virtual bool isArmed() const { Q_ASSERT(false); return false; };
virtual int getAirframe() const { Q_ASSERT(false); return 0; };
virtual UASWaypointManager* getWaypointManager(void) { Q_ASSERT(false); return NULL; };
......
......@@ -206,14 +206,6 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
#endif
}
#if defined(QGC_PROTOBUF_ENABLED)
void PxQuadMAV::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<google::protobuf::Message> message)
{
UAS::receiveExtendedMessage(link, message);
}
#endif
void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command)
{
#ifdef MAVLINK_ENABLED_PIXHAWK
......
......@@ -35,10 +35,6 @@ public:
public slots:
/** @brief Receive a MAVLink message from this MAV */
void receiveMessage(LinkInterface* link, mavlink_message_t message);
#if defined(QGC_PROTOBUF_ENABLED)
/** @brief Receive a Protobuf message from this MAV */
void receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<google::protobuf::Message> message);
#endif
/** @brief Send a command to an onboard process */
void sendProcessCommand(int watchdogId, int processId, unsigned int command);
signals:
......
......@@ -36,9 +36,6 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
// Connect this robot to the UAS object
connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
connect(mavlink, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), mav->getFileManager(), SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
#ifdef QGC_PROTOBUF_ENABLED
connect(mavlink, SIGNAL(extendedMessageReceived(LinkInterface*, std::tr1::shared_ptr<google::protobuf::Message>)), mav, SLOT(receiveExtendedMessage(LinkInterface*, std::tr1::shared_ptr<google::protobuf::Message>)));
#endif
uas = mav;
}
break;
......@@ -53,9 +50,6 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte
// else the slot of the parent object is called (and thus the special
// packets never reach their goal)
connect(mavlink, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
#ifdef QGC_PROTOBUF_ENABLED
connect(mavlink, SIGNAL(extendedMessageReceived(LinkInterface*, std::tr1::shared_ptr<google::protobuf::Message>)), mav, SLOT(receiveExtendedMessage(LinkInterface*, std::tr1::shared_ptr<google::protobuf::Message>)));
#endif
uas = mav;
}
break;
......
......@@ -30,10 +30,6 @@
#include <Eigen/Geometry>
#include <comm/px4_custom_mode.h>
#ifdef QGC_PROTOBUF_ENABLED
#include <google/protobuf/descriptor.h>
#endif
/**
* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs)
* by calling readSettings. This means the new UAS will have the same settings
......@@ -128,14 +124,6 @@ UAS::UAS(MAVLinkProtocol* protocol, QThread* thread, int id) : UASInterface(),
nedPosGlobalOffset(0,0,0),
nedAttGlobalOffset(0,0,0),
#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
receivedOverlayTimestamp(0.0),
receivedObstacleListTimestamp(0.0),
receivedPathTimestamp(0.0),
receivedPointCloudTimestamp(0.0),
receivedRGBDImageTimestamp(0.0),
#endif
airSpeed(std::numeric_limits<double>::quiet_NaN()),
groundSpeed(std::numeric_limits<double>::quiet_NaN()),
waypointManager(this),
......@@ -1545,105 +1533,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
}
}
#if defined(QGC_PROTOBUF_ENABLED)
/**
* Receive an extended message.
* @param link
* @param message
*/
void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<google::protobuf::Message> message)
{
if (!link)
{
return;
}
if (!links->contains(link))
{
addLink(link);
}
const google::protobuf::Descriptor* descriptor = message->GetDescriptor();
if (!descriptor)
{
return;
}
const google::protobuf::FieldDescriptor* headerField = descriptor->FindFieldByName("header");
if (!headerField)
{
return;
}
const google::protobuf::Descriptor* headerDescriptor = headerField->message_type();
if (!headerDescriptor)
{
return;
}
const google::protobuf::FieldDescriptor* sourceSysIdField = headerDescriptor->FindFieldByName("source_sysid");
if (!sourceSysIdField)
{
return;
}
const google::protobuf::Reflection* reflection = message->GetReflection();
const google::protobuf::Message& headerMsg = reflection->GetMessage(*message, headerField);
const google::protobuf::Reflection* headerReflection = headerMsg.GetReflection();
int source_sysid = headerReflection->GetInt32(headerMsg, sourceSysIdField);
if (source_sysid != uasId)
{
return;
}
#ifdef QGC_USE_PIXHAWK_MESSAGES
if (message->GetTypeName() == overlay.GetTypeName())
{
receivedOverlayTimestamp = QGC::groundTimeSeconds();
overlayMutex.lock();
overlay.CopyFrom(*message);
overlayMutex.unlock();
emit overlayChanged(this);
}
else if (message->GetTypeName() == obstacleList.GetTypeName())
{
receivedObstacleListTimestamp = QGC::groundTimeSeconds();
obstacleListMutex.lock();
obstacleList.CopyFrom(*message);
obstacleListMutex.unlock();
emit obstacleListChanged(this);
}
else if (message->GetTypeName() == path.GetTypeName())
{
receivedPathTimestamp = QGC::groundTimeSeconds();
pathMutex.lock();
path.CopyFrom(*message);
pathMutex.unlock();
emit pathChanged(this);
}
else if (message->GetTypeName() == pointCloud.GetTypeName())
{
receivedPointCloudTimestamp = QGC::groundTimeSeconds();
pointCloudMutex.lock();
pointCloud.CopyFrom(*message);
pointCloudMutex.unlock();
emit pointCloudChanged(this);
}
else if (message->GetTypeName() == rgbdImage.GetTypeName())
{
receivedRGBDImageTimestamp = QGC::groundTimeSeconds();
rgbdImageMutex.lock();
rgbdImage.CopyFrom(*message);
rgbdImageMutex.unlock();
emit rgbdImageChanged(this);
}
#endif
}
#endif
/**
* Set the home position of the UAS.
* @param lat The latitude fo the home position
......
......@@ -45,13 +45,6 @@ This file is part of the QGROUNDCONTROL project
#include "QGCUASParamManagerInterface.h"
#include "RadioCalibration/RadioCalibrationData.h"
#ifdef QGC_PROTOBUF_ENABLED
#include <tr1/memory>
#ifdef QGC_USE_PIXHAWK_MESSAGES
#include <pixhawk/pixhawk.pb.h>
#endif
#endif
class QGCUASFileManager;
enum BatteryType
......@@ -136,19 +129,6 @@ public:
virtual bool getSelected() const = 0;
#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
virtual px::GLOverlay getOverlay() = 0;
virtual px::GLOverlay getOverlay(qreal& receivedTimestamp) = 0;
virtual px::ObstacleList getObstacleList() = 0;
virtual px::ObstacleList getObstacleList(qreal& receivedTimestamp) = 0;
virtual px::Path getPath() = 0;
virtual px::Path getPath(qreal& receivedTimestamp) = 0;
virtual px::PointCloudXYZRGB getPointCloud() = 0;
virtual px::PointCloudXYZRGB getPointCloud(qreal& receivedTimestamp) = 0;
virtual px::RGBDImage getRGBDImage() = 0;
virtual px::RGBDImage getRGBDImage(qreal& receivedTimestamp) = 0;
#endif
virtual bool isArmed() const = 0;
/** @brief Set the airframe of this MAV */
......@@ -603,19 +583,6 @@ signals:
/** @brief Radio Calibration Data has been received from the MAV*/
void radioCalibrationReceived(const QPointer<RadioCalibrationData>&);
#if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
/** @brief Overlay data has been changed */
void overlayChanged(UASInterface* uas);
/** @brief Obstacle list data has been changed */
void obstacleListChanged(UASInterface* uas);
/** @brief Path data has been changed */
void pathChanged(UASInterface* uas);
/** @brief Point cloud data has been changed */
void pointCloudChanged(UASInterface* uas);
/** @brief RGBD image data has been changed */
void rgbdImageChanged(UASInterface* uas);
#endif
/**
* @brief Localization quality changed
* @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment