Commit bba55d01 authored by James Goppert's avatar James Goppert
parents 4db3f78d 87d277ff
......@@ -44,3 +44,7 @@ user_config.pri
*.cproject
*.sln
*.suo
thirdParty/qserialport-build-desktop/
thirdParty/qserialport/bin/
thirdParty/qserialport/lib/
......@@ -58,36 +58,47 @@ exists(user_config.pri) {
}
INCLUDEPATH += $$BASEDIR/../mavlink/include/common
INCLUDEPATH += $$BASEDIR/../mavlink/include
INCLUDEPATH += $$BASEDIR/thirdParty/mavlink/include/common
INCLUDEPATH += $$BASEDIR/thirdParty/mavlink/include
contains(MAVLINK_CONF, pixhawk) {
# Remove the default set - it is included anyway
INCLUDEPATH -= $$BASEDIR/../mavlink/include/common
INCLUDEPATH -= $$BASEDIR/thirdParty/mavlink/include/common
# PIXHAWK SPECIAL MESSAGES
INCLUDEPATH += $$BASEDIR/../mavlink/include/pixhawk
INCLUDEPATH += $$BASEDIR/thirdParty/mavlink/include/pixhawk
DEFINES += QGC_USE_PIXHAWK_MESSAGES
}
contains(MAVLINK_CONF, slugs) {
# Remove the default set - it is included anyway
INCLUDEPATH -= $$BASEDIR/../mavlink/include/common
INCLUDEPATH -= $$BASEDIR/thirdParty/mavlink/include/common
# SLUGS SPECIAL MESSAGES
INCLUDEPATH += $$BASEDIR/../mavlink/include/slugs
INCLUDEPATH += $$BASEDIR/thirdParty/mavlink/include/slugs
DEFINES += QGC_USE_SLUGS_MESSAGES
}
contains(MAVLINK_CONF, ualberta) {
# Remove the default set - it is included anyway
INCLUDEPATH -= $$BASEDIR/../mavlink/include/common
INCLUDEPATH -= $$BASEDIR/thirdParty/mavlink/include/common
# UALBERTA SPECIAL MESSAGES
INCLUDEPATH += $$BASEDIR/../mavlink/include/ualberta
INCLUDEPATH += $$BASEDIR/thirdParty/mavlink/include/ualberta
DEFINES += QGC_USE_UALBERTA_MESSAGES
}
contains(MAVLINK_CONF, ardupilotmega) {
# Remove the default set - it is included anyway
INCLUDEPATH -= $$BASEDIR/../mavlink/include/common
INCLUDEPATH -= $$BASEDIR/thirdParty/mavlink/include/common
# UALBERTA SPECIAL MESSAGES
INCLUDEPATH += $$BASEDIR/../mavlink/include/ardupilotmega
INCLUDEPATH += $$BASEDIR/thirdParty/mavlink/include/ardupilotmega
DEFINES += QGC_USE_ARDUPILOTMEGA_MESSAGES
}
......@@ -97,13 +108,6 @@ contains(MAVLINK_CONF, ardupilotmega) {
# done by the plugins above
include(qgroundcontrol.pri)
# QWT plot and QExtSerial depend on paths set by qgroundcontrol.pri
# Include serial port library
include(src/lib/qextserialport/qextserialport.pri)
# include qserial library
include(thirdParty/qserial/qserialport.prf)
# Include QWT plotting library
include(src/lib/qwt/qwt.pri)
DEPENDPATH += . \
......@@ -111,11 +115,22 @@ DEPENDPATH += . \
lib/QMapControl/src \
lib/opmapcontrol \
lib/opmapcontrol/src \
plugins
plugins \
thirdParty/qserialport/include \
thirdParty/qserialport/include/QtSerialPort \
thirdParty/qserialport
INCLUDEPATH += . \
lib/QMapControl \
lib/opmapcontrol \
$$BASEDIR/../mavlink/include
thirdParty/qserialport/include \
thirdParty/qserialport/include/QtSerialPort \
thirdParty/qserialport/src
# Include serial port library
include(src/lib/qextserialport/qextserialport.pri)
# include qserial library
include(thirdParty/qserialport/qgroundcontrol-qserialport.pri)
# ../mavlink/include \
# MAVLink/include \
......
......@@ -182,6 +182,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status);
if (decodeState == 1) {
#ifdef MAVLINK_MESSAGE_LENGTHS
const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
if (message.msgid >= sizeof(message_lengths) ||
message.len != message_lengths[message.msgid]) {
qDebug() << "MAVLink message " << message.msgid << " length incorrect (was " << message.len << " expected " << message_lengths[message.msgid] << ")";
continue;
}
#endif
// Log data
if (m_loggingEnabled && m_logfile) {
const int len = MAVLINK_MAX_PACKET_LEN+sizeof(quint64);
......
......@@ -90,6 +90,16 @@ bool MAVLinkXMLParser::generate()
int mavlinkVersion = 0;
// we need to gather the message lengths across multiple includes,
// which we can do via detecting recursion
static unsigned message_lengths[256];
static int highest_message_id;
static int recursion_level;
if (recursion_level == 0) {
highest_message_id = 0;
memset(message_lengths, 0, sizeof(message_lengths));
}
// Start main header
......@@ -138,7 +148,9 @@ bool MAVLinkXMLParser::generate()
MAVLinkXMLParser includeParser(incFilePath, topLevelOutputDirName, this);
connect(&includeParser, SIGNAL(parseState(QString)), this, SIGNAL(parseState(QString)));
// Generate and write
recursion_level++;
includeParser.generate();
recursion_level--;
mainHeader += "\n#include \"../" + pureIncludeFileName + "/" + pureIncludeFileName + ".h\"\n";
......@@ -340,7 +352,7 @@ bool MAVLinkXMLParser::generate()
QString decodeLines;
QString sendArguments;
QString commentLines;
unsigned message_length = 0;
// Get the message fields
......@@ -437,6 +449,40 @@ bool MAVLinkXMLParser::generate()
}
// message length calculation
unsigned element_multiplier = 1;
unsigned element_length = 0;
const struct {
const char *prefix;
unsigned length;
} length_map[] = {
{ "array", 1 },
{ "char", 1 },
{ "uint8", 1 },
{ "int8", 1 },
{ "uint16", 2 },
{ "int16", 2 },
{ "uint32", 4 },
{ "int32", 4 },
{ "uint64", 8 },
{ "int64", 8 },
{ "float", 4 },
{ "double", 8 },
};
if (fieldType.contains("[")) {
element_multiplier = fieldType.split("[").at(1).split("]").first().toInt();
}
for (unsigned i=0; i<sizeof(length_map)/sizeof(length_map[0]); i++) {
if (fieldType.startsWith(length_map[i].prefix)) {
element_length = length_map[i].length * element_multiplier;
break;
}
}
if (element_length == 0) {
emit parseState(tr("<font color=\"red\">ERROR: Unable to calculate length for %2 near line %1\nAbort.</font>").arg(QString::number(e.lineNumber()), fieldType));
}
message_length += element_length;
//
// QString unpackingCode;
......@@ -489,6 +535,11 @@ bool MAVLinkXMLParser::generate()
f = f.nextSibling();
}
if (messageId > highest_message_id) {
highest_message_id = messageId;
}
message_lengths[messageId] = message_length;
cStruct = cStruct.arg(cStructName, cStructLines);
lcmStructDefs.append("\n").append(cStruct).append("\n");
pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines);
......@@ -540,6 +591,15 @@ bool MAVLinkXMLParser::generate()
mainHeader += includeLine.arg(messagesDirName + "/" + cFiles.at(i).first);
}
mainHeader += "\n\n// MESSAGE LENGTHS\n\n";
mainHeader += "#undef MAVLINK_MESSAGE_LENGTHS\n";
mainHeader += "#define MAVLINK_MESSAGE_LENGTHS { ";
for (int i=0; i<highest_message_id; i++) {
mainHeader += QString::number(message_lengths[i]);
if (i < highest_message_id-1) mainHeader += ", ";
}
mainHeader += " }\n\n";
mainHeader += "#ifdef __cplusplus\n}\n#endif\n";
mainHeader += "#endif";
// Newline to make compiler happy
......
......@@ -49,7 +49,8 @@ signals:
public:
enum baudRateType {
enum baudRateType
{
BAUD50, //POSIX ONLY
BAUD75, //POSIX ONLY
BAUD110,
......@@ -77,14 +78,16 @@ public:
BAUD921600 // WINDOWS ONLY
};
enum dataBitsType {
enum dataBitsType
{
DATA_5,
DATA_6,
DATA_7,
DATA_8
};
enum parityType {
enum parityType
{
PAR_NONE,
PAR_ODD,
PAR_EVEN,
......@@ -92,13 +95,15 @@ public:
PAR_SPACE
};
enum stopBitsType {
enum stopBitsType
{
STOP_1,
STOP_1_5, //WINDOWS ONLY
STOP_2
};
enum flowType {
enum flowType
{
FLOW_OFF,
FLOW_HARDWARE,
FLOW_XONXOFF
......@@ -107,7 +112,8 @@ public:
/**
* structure to contain port settings
*/
struct portSettings {
struct portSettings
{
baudRateType BaudRate;
dataBitsType DataBits;
parityType Parity;
......@@ -186,10 +192,11 @@ public:
}
virtual void setTimeout(qint64 timeout) {
_port->setTimeout(timeout);
};
}
virtual void setFlow(SerialInterface::flowType flow) {
// TODO implement
};
_port->setFlowControl((FlowType)flow);
}
};
using namespace TNX;
......@@ -262,11 +269,11 @@ public:
virtual void setTimeout(qint64 timeout) {
// TODO implement
//_port->setTimeout(timeout);
};
}
virtual void setFlow(SerialInterface::flowType flow) {
// TODO map
settings.setFlowControl(QPortSettings::FLOW_OFF);
};
}
};
#endif // SERIALINTERFACE_H
......
......@@ -48,32 +48,14 @@ SerialLink::SerialLink(QString portname, SerialInterface::baudRateType baudrate,
this->timeout = 1; ///< The timeout controls how long the program flow should wait for new serial bytes. As we're polling, we don't want to wait at all.
// Set the port name
if (porthandle == "") {
// name = tr("serial link ") + QString::number(getId()) + tr(" (unconfigured)");
if (porthandle == "")
{
name = tr("Serial Link ") + QString::number(getId());
} else {
name = portname.trimmed();
}
#ifdef _WIN3232
// Windows 32bit & 64bit serial connection
winPort = CreateFile(porthandle,
GENERIC_READ | GENERIC_WRITE,
0,
0,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL,
0);
if(winPort==INVALID_HANDLE_VALUE) {
if(GetLastError()==ERROR_FILE_NOT_FOUND) {
//serial port does not exist. Inform user.
}
//some other error occurred. Inform user.
else
{
name = portname.trimmed();
}
#else
#endif
loadSettings();
}
......
......@@ -85,29 +85,32 @@ void UDPLink::setPort(int port)
*/
void UDPLink::addHost(const QString& host)
{
qDebug() << "UDP:" << "ADDING HOST:" << host;
if (host.contains(":")) {
qDebug() << "HOST: " << host.split(":").first();
QHostInfo info = QHostInfo::fromName(host.split(":").first());
// Add host
QList<QHostAddress> hostAddresses = info.addresses();
QHostAddress address;
for (int i = 0; i < hostAddresses.size(); i++) {
// Exclude loopback IPv4 and all IPv6 addresses
if (!hostAddresses.at(i).toString().contains(":")) {
address = hostAddresses.at(i);
if (host != "")
{
qDebug() << "UDP:" << "ADDING HOST:" << host;
if (host.contains(":")) {
qDebug() << "HOST: " << host.split(":").first();
QHostInfo info = QHostInfo::fromName(host.split(":").first());
// Add host
QList<QHostAddress> hostAddresses = info.addresses();
QHostAddress address;
for (int i = 0; i < hostAddresses.size(); i++) {
// Exclude loopback IPv4 and all IPv6 addresses
if (!hostAddresses.at(i).toString().contains(":")) {
address = hostAddresses.at(i);
}
}
hosts.append(address);
qDebug() << "Address:" << address.toString();
// Set port according to user input
ports.append(host.split(":").last().toInt());
} else {
QHostInfo info = QHostInfo::fromName(host);
// Add host
hosts.append(info.addresses().first());
// Set port according to default (this port)
ports.append(port);
}
hosts.append(address);
qDebug() << "Address:" << address.toString();
// Set port according to user input
ports.append(host.split(":").last().toInt());
} else {
QHostInfo info = QHostInfo::fromName(host);
// Add host
hosts.append(info.addresses().first());
// Set port according to default (this port)
ports.append(port);
}
}
......@@ -137,20 +140,21 @@ void UDPLink::removeHost(const QString& hostname)
void UDPLink::writeBytes(const char* data, qint64 size)
{
// Broadcast to all connected systems
//QList<QHostAddress>::iterator h;
// for (h = hosts->begin(); h != hosts->end(); ++h)
for (int h = 0; h < hosts.size(); h++) {
for (int h = 0; h < hosts.size(); h++)
{
QHostAddress currentHost = hosts.at(h);
quint16 currentPort = ports.at(h);
qDebug() << "WRITING TO" << currentHost.toIPv4Address() << currentPort;
QString bytes;
QString ascii;
//qDebug() << "WRITING DATA TO" << currentHost.toString() << currentPort;
for (int i=0; i<size; i++) {
unsigned char v =data[i];
qDebug("%02x ", v);
unsigned char v = data[i];
bytes.append(QString().sprintf("%02x ", v));
ascii.append(data[i]);
}
qDebug() <<"Sent to " << currentHost.toString() << ":" << currentPort;
qDebug() << "Sent" << size << "bytes to" << currentHost.toString() << ":" << currentPort << "data:";
qDebug() << bytes;
qDebug() << "ASCII:" << ascii;
socket->writeDatagram(data, size, currentHost, currentPort);
}
......
######################################################################
# Automatically generated by qmake (2.01a) Sa. Apr 2 10:42:30 2011
######################################################################
DEPENDPATH += . \
include/QtSerialPort \
src/common \
src/posix \
src/win32
INCLUDEPATH += include \
include/QtSerialPort \
src/posix \
src/win32
# Input
HEADERS += include/QtSerialPort/qportsettings.h \
include/QtSerialPort/qserialport.h \
include/QtSerialPort/qserialport_export.h \
include/QtSerialPort/qserialportnative.h
macx|linux-g++|linux-g++-64 {
HEADERS += src/posix/termioshelper.h
}
win32-msvc2008|win32-g++ {
HEADERS += src/win32/commdcbhelper.h \
src/win32/qwincommevtnotifier.h \
src/win32/wincommevtbreaker.h \
src/win32/commdcbhelper.h \
src/win32/qwincommevtnotifier.h \
src/win32/wincommevtbreaker.h
}
SOURCES += src/common/qportsettings.cpp \
src/common/qserialport.cpp
macx|linux-g++|linux-g++-64 {
SOURCES += src/posix/qserialportnative_posix.cpp \
src/posix/termioshelper.cpp
}
win32-msvc2008|win32-g++ {
SOURCES += src/win32/commdcbhelper.cpp \
src/win32/qserialportnative_win32.cpp \
src/win32/qwincommevtnotifier.cpp \
src/win32/wincommevtbreaker.cpp
}
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