Commit 18d74deb authored by James Goppert's avatar James Goppert

K/R formatting, debugging serialport.

parent 48f4d3ba
......@@ -155,6 +155,7 @@ if(QSERIAL_BUILD_FROM_SOURCE)
${PROJECT_SOURCE_DIR}/thirdParty/qserial/include/QtSerialPort
${PROJECT_SOURCE_DIR}/thirdParty/qserial/src
)
set(QSERIAL_LIBRARIES qserial)
add_custom_command(OUTPUT QSERIAL_BUILD.stamp
COMMAND touch QSERIAL_BUILD.stamp)
endif()
......@@ -216,6 +217,7 @@ endif()
message(STATUS "\t\tQSERIAL\t\t${QSERIAL_FOUND}")
if (QSERIAL_FOUND)
list(APPEND qgroundcontrolIncludes ${QSERIAL_INCLUDE_DIRS})
list(APPEND qgroundcontrolLibs ${QSERIAL_LIBRARIES})
endif()
message(STATUS "\t\tOpenSceneGraph\t${OPENSCENEGRAPH_FOUND}")
......@@ -792,13 +794,13 @@ set (qserialHdrs
thirdParty/qserial/include/QtSerialPort/qserialport_export.h
thirdParty/qserial/include/QtSerialPort/QSerialPort
thirdParty/qserial/include/QtSerialPort/qportsettings.h
thirdParty/qserial/include/QtSerialPort/qserialport.h
thirdParty/qserial/include/QtSerialPort/qserialportnative.h
)
# qserial headers with Q_OBJECT
# r !grep -Rl Q_OBJECT thirdParty/qserial
set (qserialMocSrc
thirdParty/qserial/include/QtSerialPort/qserialport.h
thirdParty/qserial/include/QtSerialPort/qserialportnative.h
)
)
# qserial src
set (qserialSrc
thirdParty/qserial/src/common/qserialport.cpp
......@@ -812,28 +814,32 @@ set(qserialRscSrc
)
# qserial native code
if (WIN32)
list(APPEND qSerialMocSrc
thirdParty/qserial/src/win32/qwincommevtnotifier.h
thirdParty/qserial/src/win32/wincommevtbreaker.h
)
list(APPEND qSerialSrc
thirdParty/qserial/src/win32/qserialportnative_win32.cpp
thirdParty/qserial/src/win32/commdcbhelper.h
thirdParty/qserial/src/win32/commdcbhelper.cpp
thirdParty/qserial/src/win32/qserialportnative_wince.cpp
thirdParty/qserial/src/win32/wincommevtbreaker.cpp
thirdParty/qserial/src/win32/qwincommevtnotifier.cpp
)
elseif(UNIX OR APPLE)
list(APPEND qSerialSrc
#if (WIN32)
#list(APPEND qserialHdrs
#thirdParty/qserial/src/win32/commdcbhelper.h
#)
#list(APPEND qserialMocSrc
#thirdParty/qserial/src/win32/qwincommevtnotifier.h
#thirdParty/qserial/src/win32/wincommevtbreaker.h
#)
#list(APPEND qserialSrc
#thirdParty/qserial/src/win32/qserialportnative_win32.cpp
#thirdParty/qserial/src/win32/commdcbhelper.cpp
#thirdParty/qserial/src/win32/qwincommevtnotifier.cpp
##thirdParty/qserial/src/win32/qserialportnative_wince.cpp
##thirdParty/qserial/src/win32/wincommevtbreaker.cpp
#)
#elseif(UNIX OR APPLE)
list(APPEND qserialHdrs
thirdParty/qserial/src/posix/termioshelper.h
)
list(APPEND qserialSrc
thirdParty/qserial/src/posix/termioshelper.cpp
thirdParty/qserial/src/posix/qserialportnative_posix.cpp
)
else()
message(FATAL_ERROR "unknown OS")
endif()
#else()
#message(FATAL_ERROR "unknown OS")
#endif()
# qserial linking
qt4_wrap_cpp(qserialMoc ${qserialMocSrc})
......
# - Try to find QSERIAL
# Once done, this will define
#
# QSERIAL_FOUND - system has scicoslab
# QSERIAL_INCLUDE_DIRS - the scicoslab include directories
# QSERIAL_LIBRARIES - libraries to link to
include(LibFindMacros)
# Include dir
find_path(QSERIAL_INCLUDE_DIR
NAMES QSerialPort
PATHS
/usr/include/QtSerialPort
/usr/local/include/QtSerialPort
/usr/local/qserialport/include/QtSerialPort
)
# Finally the library itself
find_library(QSERIAL_LIBRARY
NAMES
QtSerialPort
PATHS
/usr/lib
/usr/local/lib
/usr/local/qserialport/lib
)
# Set the include dir variables and the libraries and let libfind_process do the rest.
# NOTE: Singular variables for this library, plural for libraries this this lib depends on.
set(QSERIAL_PROCESS_INCLUDES QSERIAL_INCLUDE_DIR)
set(QSERIAL_PROCESS_LIBS QSERIAL_LIBRARY QSERIAL_LIBRARIES)
libfind_process(QSERIAL)
#!/bin/bash
astyle --style=k/r $(find src -regex ".*\.\(cpp\|cc\|h\|hpp\)")
rm -f $(find . -regex '.*orig$')
git commit
#include "AudioOutput.h"
#ifdef Q_OS_MAC
#include <ApplicationServices/ApplicationServices.h>
#include <ApplicationServices/ApplicationServices.h>
#else
#include <flite.h>
#include <phonon/mediaobject.h>
#include <QTemporaryFile>
#include <flite.h>
#include <phonon/mediaobject.h>
#include <QTemporaryFile>
#endif
#include <QDebug>
......@@ -34,8 +34,7 @@ AudioOutput::AudioOutput(QString voice, QObject* parent)
flite_init();
#endif
switch (voiceIndex)
{
switch (voiceIndex) {
case 0:
selectFemaleVoice();
break;
......@@ -79,8 +78,7 @@ bool AudioOutput::say(QString text, int severity)
#else
QTemporaryFile file;
file.setFileTemplate("XXXXXX.wav");
if (file.open())
{
if (file.open()) {
cst_wave* wav = flite_text_to_wave(text.toStdString().c_str(), this->voice);
// file.fileName() returns the unique file name
cst_wave_save(wav, file.fileName().toStdString().c_str(), "riff");
......@@ -155,7 +153,7 @@ extern "C" {
l.append(s);
}
printf("\n");
*/
*/
#endif
return l;
}
......
......@@ -87,20 +87,16 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv)
// Show user an upgrade message if QGC got upgraded (see code below, after splash screen)
bool upgraded = false;
QString lastApplicationVersion("");
if (settings.contains("QGC_APPLICATION_VERSION"))
{
if (settings.contains("QGC_APPLICATION_VERSION")) {
QString qgcVersion = settings.value("QGC_APPLICATION_VERSION").toString();
if (qgcVersion != QGC_APPLICATION_VERSION)
{
if (qgcVersion != QGC_APPLICATION_VERSION) {
lastApplicationVersion = qgcVersion;
settings.clear();
// Write current application version
settings.setValue("QGC_APPLICATION_VERSION", QGC_APPLICATION_VERSION);
upgraded = true;
}
}
else
{
} else {
// If application version is not set, clear settings anyway
settings.clear();
// Write current application version
......@@ -172,8 +168,7 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv)
if (upgraded) mainWindow->showInfoMessage(tr("Default Settings Loaded"), tr("QGroundControl has been upgraded from version %1 to version %2. Some of your user preferences have been reset to defaults for safety reasons. Please adjust them where needed.").arg(lastApplicationVersion).arg(QGC_APPLICATION_VERSION));
// Check if link could be connected
if (!udpLink->connect())
{
if (!udpLink->connect()) {
QMessageBox msgBox;
msgBox.setIcon(QMessageBox::Critical);
msgBox.setText("Could not connect UDP port. Is an instance of " + qAppName() + "already running?");
......@@ -186,8 +181,7 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv)
QTimer::singleShot(5000, &msgBox, SLOT(reject()));
// Exit application
if (ret == QMessageBox::Yes)
{
if (ret == QMessageBox::Yes) {
//mainWindow->close();
QTimer::singleShot(200, mainWindow, SLOT(close()));
}
......@@ -244,8 +238,7 @@ void Core::startUASManager()
if (pluginsDir.dirName().toLower() == "debug" || pluginsDir.dirName().toLower() == "release")
pluginsDir.cdUp();
#elif defined(Q_OS_MAC)
if (pluginsDir.dirName() == "MacOS")
{
if (pluginsDir.dirName() == "MacOS") {
pluginsDir.cdUp();
pluginsDir.cdUp();
pluginsDir.cdUp();
......@@ -259,12 +252,10 @@ void Core::startUASManager()
QStringList pluginFileNames;
foreach (QString fileName, pluginsDir.entryList(QDir::Files))
{
foreach (QString fileName, pluginsDir.entryList(QDir::Files)) {
QPluginLoader loader(pluginsDir.absoluteFilePath(fileName));
QObject *plugin = loader.instance();
if (plugin)
{
if (plugin) {
//populateMenus(plugin);
pluginFileNames += fileName;
//printf(QString("Loaded plugin from " + fileName + "\n").toStdString().c_str());
......
......@@ -89,9 +89,9 @@ GAudioOutput* GAudioOutput::instance()
#define QGC_GAUDIOOUTPUT_KEY QString("QGC_AUDIOOUTPUT_")
GAudioOutput::GAudioOutput(QObject* parent) : QObject(parent),
voiceIndex(0),
emergency(false),
muted(false)
voiceIndex(0),
emergency(false),
muted(false)
{
// Load settings
QSettings settings;
......@@ -103,19 +103,15 @@ muted(false)
flite_init();
#endif
#if _MSC_VER2
#if _MSC_VER2
ISpVoice * pVoice = NULL;
if (FAILED(::CoInitialize(NULL)))
{
if (FAILED(::CoInitialize(NULL))) {
qDebug("Creating COM object for audio output failed!");
}
else
{
} else {
HRESULT hr = CoCreateInstance(CLSID_SpVoice, NULL, CLSCTX_ALL, IID_ISpVoice, (void **)&pVoice;);
if( SUCCEEDED( hr ) )
{
if( SUCCEEDED( hr ) ) {
hr = pVoice->Speak(L"Hello world", 0, NULL);
pVoice->Release();
pVoice = NULL;
......@@ -131,8 +127,7 @@ muted(false)
emergencyTimer = new QTimer();
connect(emergencyTimer, SIGNAL(timeout()), this, SLOT(beep()));
switch (voiceIndex)
{
switch (voiceIndex) {
case 0:
selectFemaleVoice();
break;
......@@ -151,8 +146,7 @@ GAudioOutput::~GAudioOutput()
void GAudioOutput::mute(bool mute)
{
if (mute != muted)
{
if (mute != muted) {
this->muted = mute;
QSettings settings;
settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", this->muted);
......@@ -168,13 +162,11 @@ bool GAudioOutput::isMuted()
bool GAudioOutput::say(QString text, int severity)
{
if (!muted)
{
if (!muted) {
// TODO Add severity filter
Q_UNUSED(severity);
bool res = false;
if (!emergency)
{
if (!emergency) {
// Speech synthesis is only supported with MSVC compiler
#ifdef _MSC_VER2
......@@ -187,8 +179,7 @@ bool GAudioOutput::say(QString text, int severity)
#ifdef Q_OS_LINUX
QTemporaryFile file;
file.setFileTemplate("XXXXXX.wav");
if (file.open())
{
if (file.open()) {
cst_voice* v = register_cmu_us_kal(NULL);
cst_wave* wav = flite_text_to_wave(text.toStdString().c_str(), v);
// file.fileName() returns the unique file name
......@@ -211,9 +202,7 @@ bool GAudioOutput::say(QString text, int severity)
#endif
}
return res;
}
else
{
} else {
return false;
}
}
......@@ -223,40 +212,35 @@ bool GAudioOutput::say(QString text, int severity)
*/
bool GAudioOutput::alert(QString text)
{
if (!emergency || !muted)
{
if (!emergency || !muted) {
// Play alert sound
beep();
// Say alert message
say(text, 2);
return true;
}
else
{
} else {
return false;
}
}
void GAudioOutput::notifyPositive()
{
if (!muted)
{
if (!muted) {
// Use QFile to transform path for all OS
QFile f(QCoreApplication::applicationDirPath()+QString("/audio/double_notify.wav"));
m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str()));
m_media->play();
}
}
}
void GAudioOutput::notifyNegative()
{
if (!muted)
{
if (!muted) {
// Use QFile to transform path for all OS
QFile f(QCoreApplication::applicationDirPath()+QString("/audio/flat_notify.wav"));
m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str()));
m_media->play();
}
}
}
/**
......@@ -268,8 +252,7 @@ void GAudioOutput::notifyNegative()
*/
bool GAudioOutput::startEmergency()
{
if (!emergency)
{
if (!emergency) {
emergency = true;
// Beep immediately and then start timer
if (!muted) beep();
......@@ -287,8 +270,7 @@ bool GAudioOutput::startEmergency()
*/
bool GAudioOutput::stopEmergency()
{
if (emergency)
{
if (emergency) {
emergency = false;
emergencyTimer->stop();
}
......@@ -297,8 +279,7 @@ bool GAudioOutput::stopEmergency()
void GAudioOutput::beep()
{
if (!muted)
{
if (!muted) {
// Use QFile to transform path for all OS
QFile f(QCoreApplication::applicationDirPath()+QString("/audio/alert.wav"));
qDebug() << "FILE:" << f.fileName();
......@@ -339,8 +320,7 @@ QStringList GAudioOutput::listVoices(void)
printf("Voices available: ");
for (v=flite_voice_list; v; v=val_cdr(v))
{
for (v=flite_voice_list; v; v=val_cdr(v)) {
voice = val_voice(val_car(v));
QString s;
s.sprintf("%s",voice->name);
......
......@@ -59,7 +59,7 @@ This file is part of the PIXHAWK project
extern "C" {
cst_voice *REGISTER_VOX(const char *voxdir);
void UNREGISTER_VOX(cst_voice *vox);
cst_voice* register_cmu_us_kal16(const char *voxdir);
cst_voice* register_cmu_us_kal16(const char *voxdir);
}
#endif
......
......@@ -61,16 +61,14 @@ void LogCompressor::run()
qDebug() << "LOG COMPRESSOR: Starting" << fileName;
if (!file.exists() || !file.open(QIODevice::ReadOnly | QIODevice::Text))
{
if (!file.exists() || !file.open(QIODevice::ReadOnly | QIODevice::Text)) {
//qDebug() << "LOG COMPRESSOR: INPUT FILE DOES NOT EXIST";
emit logProcessingStatusChanged(tr("Log Compressor: Cannot start/compress log file, since input file %1 is not readable").arg(QFileInfo(fileName).absoluteFilePath()));
return;
}
// Check if file is writeable
if (outFileName == ""/* || !QFileInfo(outfile).isWritable()*/)
{
if (outFileName == ""/* || !QFileInfo(outfile).isWritable()*/) {
//qDebug() << "LOG COMPRESSOR: OUTPUT FILE DOES NOT EXIST" << outFileName;
emit logProcessingStatusChanged(tr("Log Compressor: Cannot start/compress log file, since output file %1 is not writable").arg(QFileInfo(outFileName).absoluteFilePath()));
return;
......@@ -98,8 +96,7 @@ void LogCompressor::run()
QString header = "";
QString spacer = "";
for (int i = 0; i < keys->length(); i++)
{
for (int i = 0; i < keys->length(); i++) {
header += keys->at(i) + separator;
spacer += " " + separator;
}
......@@ -116,14 +113,12 @@ void LogCompressor::run()
in.reset();
in.resetStatus();
bool ok;
while (!in.atEnd())
{
while (!in.atEnd()) {
QString line = in.readLine();
// Accumulate map of keys
// Data field name is at position 2b
quint64 time = static_cast<QString>(line.split(separator).at(0)).toLongLong(&ok);
if (ok)
{
if (ok) {
times.append(time);
}
}
......@@ -134,11 +129,9 @@ void LogCompressor::run()
// Create lines
QStringList* outLines = new QStringList();
for (int i = 0; i < times.length(); i++)
{
for (int i = 0; i < times.length(); i++) {
// Cast to signed on purpose, 64 bit timestamp still long enough
if (static_cast<qint64>(times.at(i)) != lastTime)
{
if (static_cast<qint64>(times.at(i)) != lastTime) {
outLines->append(QString("%1").arg(times.at(i)) + separator + spacer);
lastTime = static_cast<qint64>(times.at(i));
finalTimes.append(times.at(i));
......@@ -157,8 +150,7 @@ void LogCompressor::run()
quint64 lastTimeIndex = 0;
bool failed = false;
while (!data.atEnd())
{
while (!data.atEnd()) {
linecounter++;
currentDataLine = linecounter;
QString line = data.readLine();
......@@ -168,8 +160,7 @@ void LogCompressor::run()
QString field = parts.at(2);
QString value = parts.at(3);
// Enforce NaN if no value is present
if (value.length() == 0 || value == "" || value == " " || value == "\t" || value == "\n")
{
if (value.length() == 0 || value == "" || value == " " || value == "\t" || value == "\n") {
value = "NaN";
}
// Get matching output line
......@@ -185,29 +176,21 @@ void LogCompressor::run()
// Search the index until it is valid (!= -1)
// or the start of the list has been reached (failed)
while (index == -1 && !failed)
{
if (lastTimeIndex > offsetLimit)
{
while (index == -1 && !failed) {
if (lastTimeIndex > offsetLimit) {
offset = lastTimeIndex - offsetLimit;
}
else
{
} else {
offset = 0;
}
index = finalTimes.indexOf(time, offset);
if (index == -1)
{
if (offset == 0)
{
if (index == -1) {
if (offset == 0) {
emit logProcessingStatusChanged(tr("Log compressor: Timestamp %1 not found in dataset, ignoring log line %2").arg(time).arg(linecounter));
qDebug() << "Completely failed finding value";
//continue;
failed = true;
}
else
{
} else {
emit logProcessingStatusChanged(tr("Log compressor: Timestamp %1 not found in dataset, restarting search.").arg(time));
offsetLimit*=2;
}
......@@ -216,8 +199,7 @@ void LogCompressor::run()
if (dataLines > 100) if (index % (dataLines/100) == 0) emit logProcessingStatusChanged(tr("Log compressor: Processed %1% of %2 lines").arg(index/(float)dataLines*100, 0, 'f', 2).arg(dataLines));
if (!failed)
{
if (!failed) {
// When the algorithm reaches here the correct index was found
lastTimeIndex = index;
QString outLine = outLines->at(index);
......@@ -233,8 +215,7 @@ void LogCompressor::run()
// Add header, write out file
file.close();
if (outFileName == logFileName)
{
if (outFileName == logFileName) {
QFile::remove(file.fileName());
outfile.setFileName(file.fileName());
......@@ -246,8 +227,7 @@ void LogCompressor::run()
//QString fileHeader = QString("unix_timestamp") + header.replace(" ", "_") + QString("\n");
// File output
for (int i = 0; i < outLines->length(); i++)
{
for (int i = 0; i < outLines->length(); i++) {
//qDebug() << outLines->at(i);
outfile.write(QString(outLines->at(i) + "\n").toLatin1());
......
......@@ -41,62 +41,58 @@ This file is part of the PIXHAWK project
namespace MG
{
const static int MAX_FLIGHT_TIME = 60 * 60 * 24 * 21;
const static int MAX_FLIGHT_TIME = 60 * 60 * 24 * 21;
class VERSION
{
public:
class VERSION
{
public:
static const int MAJOR = 1;
static const int MINOR = 01;
};
};
class SYSTEM
{
public:
class SYSTEM
{
public:
static const int ID = 127;
static const int COMPID = 0;
static int getID()
{
static int getID() {
return SYSTEM::ID;
}
};
};
class SLEEP : public QThread
{
public:
class SLEEP : public QThread
{
public:
/**
* @brief Set a thread to sleep for seconds
* @param s time in seconds to sleep
**/
static void sleep(unsigned long s)
{
static void sleep(unsigned long s) {
QThread::sleep(s);
}
/**
* @brief Set a thread to sleep for milliseconds
* @param ms time in milliseconds to sleep
**/
static void msleep(unsigned long ms)
{
static void msleep(unsigned long ms) {
QThread::msleep(ms);
}
/**
* @brief Set a thread to sleep for microseconds
* @param us time in microseconds to sleep
**/
static void usleep(unsigned long us)
{
static void usleep(unsigned long us) {
QThread::usleep(us);
}
};
};
class UNITS
{
public:
class UNITS
{
public:
/** The distance units supported for display by the groundstation **/
enum DistanceUnit {
......@@ -116,8 +112,7 @@ namespace MG
*
* @return The converted distance
*/
static double convertFromMeter(double in, DistanceUnit newUnit)
{
static double convertFromMeter(double in, DistanceUnit newUnit) {
double result = in;
// Conversion table in meter
......@@ -162,8 +157,7 @@ namespace MG
*
* @return The converted distance
*/
static double convert(double in, DistanceUnit inputUnit, DistanceUnit outputUnit)
{
static double convert(double in, DistanceUnit inputUnit, DistanceUnit outputUnit) {
double meters = convertToMeter(in, inputUnit);
return convertFromMeter(meters, outputUnit);
}
......@@ -214,11 +208,12 @@ namespace MG
return result;
}
};
};
class DISPLAY {
class DISPLAY
{
public:
public:
DISPLAY() {
// Initialize static class display with notebook display default value
......@@ -267,21 +262,23 @@ namespace MG
pixelSize = screenDiameter / sqrt(static_cast<float>(horizontalResolution*horizontalResolution + verticalResolution*verticalResolution));
}
private:
private:
/** The size of a single pixel **/
static double pixelSize;
};
};
class STAT {
class STAT
{
/** The time interval for the last few moments in milliseconds **/
static const int SHORT_TERM_INTERVAL = 300;
/** The time interval for the last moment in milliseconds **/
static const int CURRENT_INTERVAL = 50;
};
};
class TIME {
class TIME
{
public:
public:
//static const QString ICONDIR = "./icons";
......@@ -343,11 +340,12 @@ namespace MG
return time.addMSecs(msecs % 1000);
}
};
};
class DIR {
class DIR
{
public:
public:
/**
* @brief Get the current support file directory.
*
......@@ -362,32 +360,23 @@ namespace MG
QString path = QDir::current().absolutePath();
QDir currentDir = QDir::current();
if (QDir::current().dirName().toLower() == "debug")
{
if (QDir::current().dirName().toLower() == "debug") {
// Debug folder of development environment
path.append("/..");
}
else if (QDir::current().dirName().toLower() == "release")
{
} else if (QDir::current().dirName().toLower() == "release") {
// Release folder of development environment
path.append("/..");
}
else if (QDir::current().dirName().toLower() == "bin")
{
} else if (QDir::current().dirName().toLower() == "bin") {
// Release folder of development environment
path.append("/..");
}
else if (QDir::current().dirName().toLower() == "macos")
{
} else if (QDir::current().dirName().toLower() == "macos") {
// Mac application bundle in development environment
path.append("/../../../../..");
}
// Check if we are still in a development folder
if(currentDir.cdUp())
{
if(currentDir.dirName().toLower() == "build")
{
if(currentDir.cdUp()) {
if(currentDir.dirName().toLower() == "build") {
path.append("/..");
}
}
......@@ -408,7 +397,7 @@ namespace MG
return MG::DIR::getSupportFilesDirectory() + "/images/";
}
};
};
}
......
......@@ -26,7 +26,8 @@ This file is part of the QGROUNDCONTROL project
#include <qmath.h>
#include <float.h>
namespace QGC {
namespace QGC
{
quint64 groundTimeUsecs()
{
......@@ -48,13 +49,11 @@ quint64 groundTimeMilliseconds()
float limitAngleToPMPIf(float angle)
{
while (angle > ((float)M_PI+FLT_EPSILON))
{
while (angle > ((float)M_PI+FLT_EPSILON)) {
angle -= 2.0f * (float)M_PI;
}
while (angle <= -((float)M_PI+FLT_EPSILON))
{
while (angle <= -((float)M_PI+FLT_EPSILON)) {
angle += 2.0f * (float)M_PI;
}
......@@ -63,17 +62,12 @@ float limitAngleToPMPIf(float angle)
double limitAngleToPMPId(double angle)
{
if (angle < -M_PI)
{
while (angle < -M_PI)
{
if (angle < -M_PI) {
while (angle < -M_PI) {
angle += M_PI;
}
}
else if (angle > M_PI)
{
while (angle > M_PI)
{
} else if (angle > M_PI) {
while (angle > M_PI) {
angle -= M_PI;
}
}
......
......@@ -9,58 +9,55 @@
namespace QGC
{
const static int defaultSystemId = 255;
const static int defaultComponentId = 0;
const static int defaultSystemId = 255;
const static int defaultComponentId = 0;
const QColor colorCyan(55, 154, 195);
const QColor colorRed(154, 20, 20);
const QColor colorGreen(20, 200, 20);
const QColor colorYellow(255, 255, 0);
const QColor colorOrange(255, 140, 0);
const QColor colorDarkYellow(180, 180, 0);
const QColor colorBackground("#050508");
const QColor colorBlack(0, 0, 0);
const QColor colorCyan(55, 154, 195);
const QColor colorRed(154, 20, 20);
const QColor colorGreen(20, 200, 20);
const QColor colorYellow(255, 255, 0);
const QColor colorOrange(255, 140, 0);
const QColor colorDarkYellow(180, 180, 0);
const QColor colorBackground("#050508");
const QColor colorBlack(0, 0, 0);
/** @brief Get the current ground time in microseconds */
quint64 groundTimeUsecs();
/** @brief Get the current ground time in milliseconds */
quint64 groundTimeMilliseconds();
/** @brief Returns the angle limited to -pi - pi */
float limitAngleToPMPIf(float angle);
/** @brief Returns the angle limited to -pi - pi */
double limitAngleToPMPId(double angle);
int applicationVersion();
/** @brief Get the current ground time in microseconds */
quint64 groundTimeUsecs();
/** @brief Get the current ground time in milliseconds */
quint64 groundTimeMilliseconds();
/** @brief Returns the angle limited to -pi - pi */
float limitAngleToPMPIf(float angle);
/** @brief Returns the angle limited to -pi - pi */
double limitAngleToPMPId(double angle);
int applicationVersion();
const static int MAX_FLIGHT_TIME = 60 * 60 * 24 * 21;
const static int MAX_FLIGHT_TIME = 60 * 60 * 24 * 21;
class SLEEP : public QThread
{
public:
class SLEEP : public QThread
{
public:
/**
* @brief Set a thread to sleep for seconds
* @param s time in seconds to sleep
**/
static void sleep(unsigned long s)
{
static void sleep(unsigned long s) {
QThread::sleep(s);
}
/**
* @brief Set a thread to sleep for milliseconds
* @param ms time in milliseconds to sleep
**/
static void msleep(unsigned long ms)
{
static void msleep(unsigned long ms) {
QThread::msleep(ms);
}
/**
* @brief Set a thread to sleep for microseconds
* @param us time in microseconds to sleep
**/
static void usleep(unsigned long us)
{
static void usleep(unsigned long us) {
QThread::usleep(us);
}
};
};
}
......
......@@ -78,8 +78,7 @@ void Waypoint::save(QTextStream &saveStream)
bool Waypoint::load(QTextStream &loadStream)
{
const QStringList &wpParams = loadStream.readLine().split("\t");
if (wpParams.size() == 12)
{
if (wpParams.size() == 12) {
this->id = wpParams[0].toInt();
this->current = (wpParams[1].toInt() == 1 ? true : false);
this->frame = (MAV_FRAME) wpParams[2].toInt();
......@@ -107,8 +106,7 @@ void Waypoint::setId(quint16 id)
void Waypoint::setX(double x)
{
if (this->x != x)
{
if (this->x != x) {
this->x = x;
emit changed(this);
}
......@@ -116,8 +114,7 @@ void Waypoint::setX(double x)
void Waypoint::setY(double y)
{
if (this->y != y)
{
if (this->y != y) {
this->y = y;
emit changed(this);
}
......@@ -125,8 +122,7 @@ void Waypoint::setY(double y)
void Waypoint::setZ(double z)
{
if (this->z != z)
{
if (this->z != z) {
this->z = z;
emit changed(this);
}
......@@ -134,8 +130,7 @@ void Waypoint::setZ(double z)
void Waypoint::setLatitude(double lat)
{
if (this->x != lat)
{
if (this->x != lat) {
this->x = lat;
this->frame = MAV_FRAME_GLOBAL;
emit changed(this);
......@@ -144,8 +139,7 @@ void Waypoint::setLatitude(double lat)
void Waypoint::setLongitude(double lon)
{
if (this->y != lon)
{
if (this->y != lon) {
this->y = lon;
this->frame = MAV_FRAME_GLOBAL;
emit changed(this);
......@@ -154,8 +148,7 @@ void Waypoint::setLongitude(double lon)
void Waypoint::setAltitude(double altitude)
{
if (this->z != altitude)
{
if (this->z != altitude) {
this->z = altitude;
this->frame = MAV_FRAME_GLOBAL;
emit changed(this);
......@@ -164,8 +157,7 @@ void Waypoint::setAltitude(double altitude)
void Waypoint::setYaw(int yaw)
{
if (this->yaw != yaw)
{
if (this->yaw != yaw) {
this->yaw = yaw;
emit changed(this);
}
......@@ -173,8 +165,7 @@ void Waypoint::setYaw(int yaw)
void Waypoint::setYaw(double yaw)
{
if (this->yaw != yaw)
{
if (this->yaw != yaw) {
this->yaw = yaw;
emit changed(this);
}
......@@ -182,8 +173,7 @@ void Waypoint::setYaw(double yaw)
void Waypoint::setAction(int action)
{
if (this->action != (MAV_CMD)action)
{
if (this->action != (MAV_CMD)action) {
this->action = (MAV_CMD)action;
emit changed(this);
}
......@@ -191,8 +181,7 @@ void Waypoint::setAction(int action)
void Waypoint::setAction(MAV_CMD action)
{
if (this->action != action)
{
if (this->action != action) {
this->action = action;
emit changed(this);
}
......@@ -200,8 +189,7 @@ void Waypoint::setAction(MAV_CMD action)
void Waypoint::setFrame(MAV_FRAME frame)
{
if (this->frame != frame)
{
if (this->frame != frame) {
this->frame = frame;
emit changed(this);
}
......@@ -209,8 +197,7 @@ void Waypoint::setFrame(MAV_FRAME frame)
void Waypoint::setAutocontinue(bool autoContinue)
{
if (this->autocontinue != autocontinue)
{
if (this->autocontinue != autocontinue) {
this->autocontinue = autoContinue;
emit changed(this);
}
......@@ -218,8 +205,7 @@ void Waypoint::setAutocontinue(bool autoContinue)
void Waypoint::setCurrent(bool current)
{
if (this->current != current)
{
if (this->current != current) {
this->current = current;
emit changed(this);
}
......@@ -227,8 +213,7 @@ void Waypoint::setCurrent(bool current)
void Waypoint::setAcceptanceRadius(double radius)
{
if (this->param2 != radius)
{
if (this->param2 != radius) {
this->param2 = radius;
emit changed(this);
}
......@@ -238,8 +223,7 @@ void Waypoint::setParam1(double param1)
{
qDebug() << "SENDER:" << QObject::sender();
qDebug() << "PARAM1 SET REQ:" << param1;
if (this->param1 != param1)
{
if (this->param1 != param1) {
this->param1 = param1;
emit changed(this);
}
......@@ -247,8 +231,7 @@ void Waypoint::setParam1(double param1)
void Waypoint::setParam2(double param2)
{
if (this->param2 != param2)
{
if (this->param2 != param2) {
this->param2 = param2;
emit changed(this);
}
......@@ -256,8 +239,7 @@ void Waypoint::setParam2(double param2)
void Waypoint::setParam3(double param3)
{
if (this->orbit != param3)
{
if (this->orbit != param3) {
this->orbit = param3;
emit changed(this);
}
......@@ -265,8 +247,7 @@ void Waypoint::setParam3(double param3)
void Waypoint::setParam4(double param4)
{
if (this->yaw != param4)
{
if (this->yaw != param4) {
this->yaw = param4;
emit changed(this);
}
......@@ -274,8 +255,7 @@ void Waypoint::setParam4(double param4)
void Waypoint::setParam5(double param5)
{
if (this->x != param5)
{
if (this->x != param5) {
this->x = param5;
emit changed(this);
}
......@@ -283,8 +263,7 @@ void Waypoint::setParam5(double param5)
void Waypoint::setParam6(double param6)
{
if (this->y != param6)
{
if (this->y != param6) {
this->y = param6;
emit changed(this);
}
......@@ -292,8 +271,7 @@ void Waypoint::setParam6(double param6)
void Waypoint::setParam7(double param7)
{
if (this->z != param7)
{
if (this->z != param7) {
this->z = param7;
emit changed(this);
}
......@@ -301,8 +279,7 @@ void Waypoint::setParam7(double param7)
void Waypoint::setLoiterOrbit(double orbit)
{
if (this->orbit != orbit)
{
if (this->orbit != orbit) {
this->orbit = orbit;
emit changed(this);
}
......@@ -310,8 +287,7 @@ void Waypoint::setLoiterOrbit(double orbit)
void Waypoint::setHoldTime(int holdTime)
{
if (this->param1 != holdTime)
{
if (this->param1 != holdTime) {
this->param1 = holdTime;
emit changed(this);
}
......@@ -319,8 +295,7 @@ void Waypoint::setHoldTime(int holdTime)
void Waypoint::setHoldTime(double holdTime)
{
if (this->param1 != holdTime)
{
if (this->param1 != holdTime) {
this->param1 = holdTime;
emit changed(this);
}
......@@ -328,8 +303,7 @@ void Waypoint::setHoldTime(double holdTime)
void Waypoint::setTurns(int turns)
{
if (this->param1 != turns)
{
if (this->param1 != turns) {
this->param1 = turns;
emit changed(this);
}
......
......@@ -47,30 +47,78 @@ public:
bool autocontinue = true, bool current = false, MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_CMD action=MAV_CMD_NAV_WAYPOINT);
~Waypoint();
quint16 getId() const { return id; }
double getX() const { return x; }
double getY() const { return y; }
double getZ() const { return z; }
double getLatitude() const { return x; }
double getLongitude() const { return y; }
double getAltitude() const { return z; }
double getYaw() const { return yaw; }
bool getAutoContinue() const { return autocontinue; }
bool getCurrent() const { return current; }
double getLoiterOrbit() const { return orbit; }
double getAcceptanceRadius() const { return param2; }
double getHoldTime() const { return param1; }
double getParam1() const { return param1; }
double getParam2() const { return param2; }
double getParam3() const { return orbit; }
double getParam4() const { return yaw; }
double getParam5() const { return x; }
double getParam6() const { return y; }
double getParam7() const { return z; }
int getTurns() const { return param1; }
MAV_FRAME getFrame() const { return frame; }
MAV_CMD getAction() const { return action; }
const QString& getName() const { return name; }
quint16 getId() const {
return id;
}
double getX() const {
return x;
}
double getY() const {
return y;
}
double getZ() const {
return z;
}
double getLatitude() const {
return x;
}
double getLongitude() const {
return y;
}
double getAltitude() const {
return z;
}
double getYaw() const {
return yaw;
}
bool getAutoContinue() const {
return autocontinue;
}
bool getCurrent() const {
return current;
}
double getLoiterOrbit() const {
return orbit;
}
double getAcceptanceRadius() const {
return param2;
}
double getHoldTime() const {
return param1;
}
double getParam1() const {
return param1;
}
double getParam2() const {
return param2;
}
double getParam3() const {
return orbit;
}
double getParam4() const {
return yaw;
}
double getParam5() const {
return x;
}
double getParam6() const {
return y;
}
double getParam7() const {
return z;
}
int getTurns() const {
return param1;
}
MAV_FRAME getFrame() const {
return frame;
}
MAV_CMD getAction() const {
return action;
}
const QString& getName() const {
return name;
}
/** @brief Returns true if x, y, z contain reasonable navigation data */
bool isNavigationType();
......
......@@ -54,7 +54,7 @@ AS4Protocol::AS4Protocol()
// heartbeatTimer = new QTimer(this);
// connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT());
// heartbeatTimer->start(1000/heartbeatRate);
/*
/*
// Start the node manager
configData = new FileLoader("nodeManager.conf");
handler = new MyHandler();
......@@ -86,8 +86,7 @@ AS4Protocol::~AS4Protocol()
void AS4Protocol::run()
{
forever
{
forever {
QGC::SLEEP::msleep(5000);
}
}
......@@ -126,8 +125,7 @@ void AS4Protocol::receiveBytes(LinkInterface* link)
// qDebug() << __FILE__ << __LINE__ << ": buffer size:" << maxlen << "bytes:" << bytesToRead;
//
for (int position = 0; position < bytesToRead; position++)
{
for (int position = 0; position < bytesToRead; position++) {
}
// receiveMutex.unlock();
......
......@@ -109,7 +109,8 @@ public:
* SAE AS-4 Nodemanager
*
**/
class AS4Protocol : public ProtocolInterface {
class AS4Protocol : public ProtocolInterface
{
Q_OBJECT
public:
......
......@@ -39,7 +39,8 @@ along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
* with the groundstation application.
*
**/
class LinkInterface : public QThread {
class LinkInterface : public QThread
{
Q_OBJECT
public:
LinkInterface(QObject* parent = 0) : QThread(parent) {}
......@@ -232,21 +233,20 @@ signals:
void communicationError(const QString& linkname, const QString& error);
protected:
static int getNextLinkId()
{
static int getNextLinkId() {
static int nextId = 0;
return nextId++;
}
protected slots:
/**
/**
* @brief Read a number of bytes from the interface.
*
* @param bytes The pointer to write the bytes to
* @param maxLength The maximum length which can be written
**/
virtual void readBytes() = 0;
virtual void readBytes() = 0;
};
......
......@@ -36,7 +36,8 @@ This file is part of the QGROUNDCONTROL project
#include <QDebug>
LinkManager* LinkManager::instance() {
LinkManager* LinkManager::instance()
{
static LinkManager* _instance = 0;
if(_instance == 0) {
_instance = new LinkManager();
......@@ -66,8 +67,7 @@ LinkManager::~LinkManager()
void LinkManager::add(LinkInterface* link)
{
if (!links.contains(link))
{
if (!links.contains(link)) {
if(!link) return;
connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
links.append(link);
......@@ -85,8 +85,7 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol)
// If protocol has not been added before (list length == 0)
// OR if link has not been added to protocol, add
if ((linkList.length() > 0 && !linkList.contains(link)) || linkList.length() == 0)
{
if ((linkList.length() > 0 && !linkList.contains(link)) || linkList.length() == 0) {
// Protocol is new, add
connect(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)), protocol, SLOT(receiveBytes(LinkInterface*, QByteArray)));
// Store the connection information in the protocol links map
......@@ -105,8 +104,7 @@ bool LinkManager::connectAll()
{
bool allConnected = true;
foreach (LinkInterface* link, links)
{
foreach (LinkInterface* link, links) {
if(!link) {}
else if(!link->connect()) allConnected = false;
}
......@@ -118,10 +116,9 @@ bool LinkManager::disconnectAll()
{
bool allDisconnected = true;
foreach (LinkInterface* link, links)
{
foreach (LinkInterface* link, links) {
//static int i=0;
if(!link){}
if(!link) {}
else if(!link->disconnect()) allDisconnected = false;
}
......@@ -143,27 +140,22 @@ bool LinkManager::disconnectLink(LinkInterface* link)
void LinkManager::removeLink(QObject* link)
{
LinkInterface* linkInterface = dynamic_cast<LinkInterface*>(link);
if (linkInterface)
{
if (linkInterface) {
removeLink(linkInterface);
}
}
bool LinkManager::removeLink(LinkInterface* link)
{
if(link)
{
for (int i=0; i < QList<LinkInterface*>(links).size(); i++)
{
if(link==links.at(i))
{
if(link) {
for (int i=0; i < QList<LinkInterface*>(links).size(); i++) {
if(link==links.at(i)) {
links.removeAt(i); //remove from link list
}
}
// Remove link from protocol map
QList<ProtocolInterface* > protocols = protocolLinks.keys(link);
foreach (ProtocolInterface* proto, protocols)
{
foreach (ProtocolInterface* proto, protocols) {
protocolLinks.remove(proto, link);
}
return true;
......@@ -179,8 +171,7 @@ bool LinkManager::removeLink(LinkInterface* link)
*/
LinkInterface* LinkManager::getLinkForId(int id)
{
foreach (LinkInterface* link, links)
{
foreach (LinkInterface* link, links) {
if (link->getId() == id) return link;
}
return NULL;
......
......@@ -63,7 +63,9 @@ public:
const QList<LinkInterface*> getLinks();
/** @brief Get a list of all protocols */
const QList<ProtocolInterface*> getProtocols() { return protocolLinks.uniqueKeys(); }
const QList<ProtocolInterface*> getProtocols() {
return protocolLinks.uniqueKeys();
}
public slots:
......
This diff is collapsed.
......@@ -49,7 +49,8 @@ This file is part of the QGROUNDCONTROL project
* for more information, please see the official website.
* @ref http://pixhawk.ethz.ch/software/mavlink/
**/
class MAVLinkProtocol : public ProtocolInterface {
class MAVLinkProtocol : public ProtocolInterface
{
Q_OBJECT
public:
......@@ -66,31 +67,55 @@ public:
/** @brief The auto heartbeat emission rate in Hertz */
int getHeartbeatRate();
/** @brief Get heartbeat state */
bool heartbeatsEnabled() const { return m_heartbeatsEnabled; }
bool heartbeatsEnabled() const {
return m_heartbeatsEnabled;
}
/** @brief Get logging state */
bool loggingEnabled() const { return m_loggingEnabled; }
bool loggingEnabled() const {
return m_loggingEnabled;
}
/** @brief Get protocol version check state */
bool versionCheckEnabled() const { return m_enable_version_check; }
bool versionCheckEnabled() const {
return m_enable_version_check;
}
/** @brief Get the multiplexing state */
bool multiplexingEnabled() const { return m_multiplexingEnabled; }
bool multiplexingEnabled() const {
return m_multiplexingEnabled;
}
/** @brief Get the authentication state */
bool getAuthEnabled() { return m_authEnabled; }
bool getAuthEnabled() {
return m_authEnabled;
}
/** @brief Get the protocol version */
int getVersion() { return MAVLINK_VERSION; }
int getVersion() {
return MAVLINK_VERSION;
}
/** @brief Get the auth key */
QString getAuthKey() { return m_authKey; }
QString getAuthKey() {
return m_authKey;
}
/** @brief Get the name of the packet log file */
QString getLogfileName();
/** @brief Get state of parameter retransmission */
bool paramGuardEnabled() { return m_paramGuardEnabled; }
bool paramGuardEnabled() {
return m_paramGuardEnabled;
}
/** @brief Get parameter read timeout */
int getParamRetransmissionTimeout() { return m_paramRetransmissionTimeout; }
int getParamRetransmissionTimeout() {
return m_paramRetransmissionTimeout;
}
/** @brief Get parameter write timeout */
int getParamRewriteTimeout() { return m_paramRewriteTimeout; }
int getParamRewriteTimeout() {
return m_paramRewriteTimeout;
}
/** @brief Get state of action retransmission */
bool actionGuardEnabled() { return m_actionGuardEnabled; }
bool actionGuardEnabled() {
return m_actionGuardEnabled;
}
/** @brief Get parameter read timeout */
int getActionRetransmissionTimeout() { return m_actionRetransmissionTimeout; }
int getActionRetransmissionTimeout() {
return m_actionRetransmissionTimeout;
}
public slots:
/** @brief Receive bytes from a communication interface */
......@@ -138,7 +163,9 @@ public slots:
void enableAuth(bool enable);
/** @brief Set authentication token */
void setAuthKey(QString key) { m_authKey = key;}
void setAuthKey(QString key) {
m_authKey = key;
}
/** @brief Send an extra heartbeat to all connected units */
void sendHeartbeat();
......
This diff is collapsed.
......@@ -52,16 +52,14 @@ void MAVLinkSimulationMAV::mainloop()
// double xNew = // (nextSPX - previousSPX)
if (flying)
{
if (flying) {
sys_state = MAV_STATE_ACTIVE;
sys_mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
}
// 1 Hz execution
if (timer1Hz <= 0)
{
if (timer1Hz <= 0) {
mavlink_message_t msg;
mavlink_msg_heartbeat_pack_version_free(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK, mavlink_version);
link->sendMAVLinkMessage(&msg);
......@@ -83,8 +81,7 @@ void MAVLinkSimulationMAV::mainloop()
}
// 10 Hz execution
if (timer10Hz <= 0)
{
if (timer10Hz <= 0) {
double radPer100ms = 0.00006;
double altPer100ms = 0.4;
double xm = (nextSPX - x);
......@@ -93,35 +90,28 @@ void MAVLinkSimulationMAV::mainloop()
float zsign = (zm < 0) ? -1.0f : 1.0f;
if (!firstWP)
{
if (!firstWP) {
//float trueyaw = atan2f(xm, ym);
float newYaw = atan2f(ym, xm);
if (fabs(yaw - newYaw) < 90)
{
if (fabs(yaw - newYaw) < 90) {
yaw = yaw*0.7 + 0.3*newYaw;
}
else
{
} else {
yaw = newYaw;
}
//qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw;
//if (sqrt(xm*xm+ym*ym) > 0.0000001)
if (flying)
{
if (flying) {
x += cos(yaw)*radPer100ms;
y += sin(yaw)*radPer100ms;
z += altPer100ms*zsign;
}
//if (xm < 0.001) xm
}
else
{
} else {
x = nextSPX;
y = nextSPY;
z = nextSPZ;
......@@ -205,12 +195,11 @@ void MAVLinkSimulationMAV::mainloop()
}
// 25 Hz execution
if (timer25Hz <= 0)
{
if (timer25Hz <= 0) {
// The message container to be used for sending
mavlink_message_t ret;
#ifdef MAVLINK_ENABLED_PIXHAWK
#ifdef MAVLINK_ENABLED_PIXHAWK
// Send which controllers are active
mavlink_control_status_t control_status;
control_status.control_att = 1;
......@@ -223,7 +212,7 @@ void MAVLinkSimulationMAV::mainloop()
control_status.ahrs_health = 230;
mavlink_msg_control_status_encode(systemid, MAV_COMP_ID_IMU, &ret, &control_status);
link->sendMAVLinkMessage(&ret);
#endif //MAVLINK_ENABLED_PIXHAWK
#endif //MAVLINK_ENABLED_PIXHAWK
// Send actual controller outputs
// This message just shows the direction
......@@ -301,34 +290,28 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
{
//qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid;
switch(msg.msgid)
{
switch(msg.msgid) {
case MAVLINK_MSG_ID_ATTITUDE:
break;
case MAVLINK_MSG_ID_SET_MODE:
{
case MAVLINK_MSG_ID_SET_MODE: {
mavlink_set_mode_t mode;
mavlink_msg_set_mode_decode(&msg, &mode);
if (systemid == mode.target) sys_mode = mode.mode;
}
break;
case MAVLINK_MSG_ID_ACTION:
{
case MAVLINK_MSG_ID_ACTION: {
mavlink_action_t action;
mavlink_msg_action_decode(&msg, &action);
if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU))
{
if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) {
mavlink_action_ack_t ack;
ack.action = action.action;
switch (action.action)
{
switch (action.action) {
case MAV_ACTION_TAKEOFF:
flying = true;
nav_mode = MAV_NAV_LIFTOFF;
ack.result = 1;
break;
default:
{
default: {
ack.result = 0;
}
break;
......@@ -341,12 +324,10 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
}
}
break;
case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
{
case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: {
mavlink_local_position_setpoint_set_t sp;
mavlink_msg_local_position_setpoint_set_decode(&msg, &sp);
if (sp.target_system == this->systemid)
{
if (sp.target_system == this->systemid) {
nav_mode = MAV_NAV_WAYPOINT;
previousSPX = nextSPX;
previousSPY = nextSPY;
......
......@@ -55,8 +55,7 @@ protected:
bool flying;
int mavlink_version;
static inline uint16_t mavlink_msg_heartbeat_pack_version_free(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot, uint8_t version)
{
static inline uint16_t mavlink_msg_heartbeat_pack_version_free(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot, uint8_t version) {
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
......
......@@ -7,8 +7,7 @@
#include "MAVLinkSimulationLink.h"
#include "QGCMAVLink.h"
enum PX_WAYPOINTPLANNER_STATES
{
enum PX_WAYPOINTPLANNER_STATES {
PX_WPP_IDLE = 0,
PX_WPP_SENDLIST,
PX_WPP_SENDLIST_SENDWPS,
......
......@@ -30,7 +30,7 @@ MAVLinkSyntaxHighlighter::MAVLinkSyntaxHighlighter(QObject *parent) :
void MAVLinkSyntaxHighlighter::highlightBlock(const QString &text)
{
{
QTextCharFormat myClassFormat;
myClassFormat.setFontWeight(QFont::Bold);
myClassFormat.setForeground(Qt::darkMagenta);
......@@ -43,4 +43,4 @@ void MAVLinkSyntaxHighlighter::highlightBlock(const QString &text)
setFormat(index, length, myClassFormat);
index = text.indexOf(expression, index + length);
}
}
}
......@@ -5,7 +5,7 @@
class MAVLinkSyntaxHighlighter : public QSyntaxHighlighter
{
Q_OBJECT
Q_OBJECT
public:
explicit MAVLinkSyntaxHighlighter(QObject *parent = 0);
......
This diff is collapsed.
......@@ -79,20 +79,16 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
for (int i=0; (!(decodeSuccess=mavlink_parse_char(this->getId(), bytes[i], &msg, &status))&& i<length); ++i);
/* perform the appropriate action */
if (decodeSuccess)
{
switch(msg.msgid)
{
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
if (decodeSuccess) {
switch(msg.msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
qDebug() << "OpalLink::writeBytes(): request params";
mavlink_message_t param;
OpalRT::ParameterList::const_iterator paramIter;
for (paramIter = params->begin(); paramIter != params->end(); ++paramIter)
{
for (paramIter = params->begin(); paramIter != params->end(); ++paramIter) {
mavlink_msg_param_value_pack(systemID,
(*paramIter).getComponentID(),
&param,
......@@ -105,8 +101,7 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
}
case MAVLINK_MSG_ID_PARAM_SET:
{
case MAVLINK_MSG_ID_PARAM_SET: {
// qDebug() << "OpalLink::writeBytes(): Attempt to set a parameter";
......@@ -116,8 +111,7 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
// qDebug() << "OpalLink::writeBytes():paramName: " << paramName;
if ((*params).contains(param.target_component, paramName))
{
if ((*params).contains(param.target_component, paramName)) {
OpalRT::Parameter p = (*params)(param.target_component, paramName);
// qDebug() << __FILE__ << ":" << __LINE__ << ": " << p;
// Set the param value in Opal-RT
......@@ -144,8 +138,7 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
// }
// break;
#ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES
case MAVLINK_MSG_ID_RADIO_CALIBRATION:
{
case MAVLINK_MSG_ID_RADIO_CALIBRATION: {
mavlink_radio_calibration_t radio;
mavlink_msg_radio_calibration_decode(&msg, &radio);
// qDebug() << "RADIO CALIBRATION RECEIVED";
......@@ -220,12 +213,10 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
break;
#endif
#ifdef MAVLINK_ENABLED_PIXHAWK
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
{
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
mavlink_request_data_stream_t stream;
mavlink_msg_request_data_stream_decode(&msg, &stream);
switch (stream.req_stream_id)
{
switch (stream.req_stream_id) {
case 0: // All data types
break;
case 1: // Raw Sensor Data
......@@ -257,8 +248,7 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
}
}
break;
default:
{
default: {
qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet";
}
}
......@@ -283,8 +273,7 @@ void OpalLink::receiveMessage(mavlink_message_t message)
// Write message into buffer, prepending start sign
int len = mavlink_msg_to_send_buffer((uint8_t*)(buffer), &message);
// If link is connected
if (isConnected())
{
if (isConnected()) {
receiveDataMutex.lock();
receiveBuffer->enqueue(QByteArray(buffer, len));
receiveDataMutex.unlock();
......@@ -296,8 +285,7 @@ void OpalLink::receiveMessage(mavlink_message_t message)
void OpalLink::heartbeat()
{
if (m_heartbeatsEnabled)
{
if (m_heartbeatsEnabled) {
mavlink_message_t beat;
mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC);
receiveMessage(beat);
......@@ -312,8 +300,7 @@ void OpalLink::setSignals(double *values)
int returnValue;
returnValue = OpalSetSignals( numSignals, logicalId, signalIndex, values);
if (returnValue != EOK)
{
if (returnValue != EOK) {
OpalRT::OpalErrorMsg::displayLastErrorMsg();
}
}
......@@ -327,16 +314,13 @@ void OpalLink::getSignals()
unsigned short *lastValues = new unsigned short(false);
unsigned short *decimation = new unsigned short(0);
while (!(*lastValues))
{
while (!(*lastValues)) {
int returnVal = OpalGetSignals(timeout, acqGroup, OpalRT::NUM_OUTPUT_SIGNALS, numSignals, timestep,
values, lastValues, decimation);
if (returnVal == EOK )
{
if (returnVal == EOK ) {
/* Send position info to qgroundcontrol */
if (sendPosition)
{
if (sendPosition) {
mavlink_message_t local_position;
mavlink_msg_local_position_pack(systemID, componentID, &local_position,
(*timestep)*1000000,
......@@ -375,8 +359,7 @@ void OpalLink::getSignals()
receiveMessage(bias);
/* send radio outputs */
if (sendRCValues)
{
if (sendRCValues) {
mavlink_message_t rc;
mavlink_msg_rc_channels_raw_pack(systemID, componentID, &rc,
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_1]),
......@@ -391,8 +374,7 @@ void OpalLink::getSignals()
);
receiveMessage(rc);
}
if (sendRawController)
{
if (sendRawController) {
mavlink_message_t rawController;
mavlink_msg_attitude_controller_output_pack(systemID, componentID, &rawController,
1,
......@@ -403,9 +385,7 @@ void OpalLink::getSignals()
);
receiveMessage(rawController);
}
}
else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready
{
} else if (returnVal != EAGAIN) { // if returnVal == EAGAIN => data just wasn't ready
getSignalsTimer->stop();
OpalRT::OpalErrorMsg::displayLastErrorMsg();
}
......@@ -447,7 +427,8 @@ void OpalLink::setName(QString name)
emit nameChanged(this->name);
}
bool OpalLink::isConnected() {
bool OpalLink::isConnected()
{
return connectState;
}
......@@ -459,8 +440,7 @@ uint16_t OpalLink::duty2PulseMicros(double duty)
uint8_t OpalLink::rescaleNorm(double norm, int ch)
{
switch(ch)
{
switch(ch) {
case OpalRT::NORM_CHANNEL_1:
case OpalRT::NORM_CHANNEL_2:
case OpalRT::NORM_CHANNEL_4:
......@@ -488,8 +468,7 @@ bool OpalLink::connect()
if ((OpalConnect(opalInstID, false, &modelState) == EOK)
&& (OpalGetSignalControl(0, true) == EOK)
&& (OpalGetParameterControl(true) == EOK))
{
&& (OpalGetParameterControl(true) == EOK)) {
connectState = true;
if (params)
delete params;
......@@ -497,9 +476,7 @@ bool OpalLink::connect()
emit connected();
heartbeatTimer->start(1000/heartbeatRate);
getSignalsTimer->start(getSignalsPeriod);
}
else
{
} else {
connectState = false;
OpalRT::OpalErrorMsg::displayLastErrorMsg();
}
......@@ -546,7 +523,8 @@ qint64 OpalLink::getTotalUpstream()
return totalUpstream;
}
qint64 OpalLink::getTotalDownstream() {
qint64 OpalLink::getTotalDownstream()
{
statisticsMutex.lock();
qint64 totalDownstream = bitsReceivedTotal / ((MG::TIME::getGroundTimeNow() - connectionStartTime) / 1000);
statisticsMutex.unlock();
......@@ -563,11 +541,13 @@ qint64 OpalLink::getMaxUpstream()
return 0; //unknown
}
qint64 OpalLink::getBitsSent() {
qint64 OpalLink::getBitsSent()
{
return bitsSentTotal;
}
qint64 OpalLink::getBitsReceived() {
qint64 OpalLink::getBitsReceived()
{
return bitsReceivedTotal;
}
......
......@@ -96,7 +96,9 @@ public:
void run();
int getOpalInstID() {return static_cast<int>(opalInstID);}
int getOpalInstID() {
return static_cast<int>(opalInstID);
}
public slots:
......@@ -108,7 +110,9 @@ public slots:
void getSignals();
void setOpalInstID(int instID) {opalInstID = static_cast<unsigned short>(instID);}
void setOpalInstID(int instID) {
opalInstID = static_cast<unsigned short>(instID);
}
protected slots:
......
......@@ -2,19 +2,19 @@
namespace OpalRT
{
// lastErrorMsg = QString();
void OpalErrorMsg::displayLastErrorMsg()
{
// lastErrorMsg = QString();
void OpalErrorMsg::displayLastErrorMsg()
{
static QString lastErrorMsg;
setLastErrorMsg();
QMessageBox msgBox;
msgBox.setIcon(QMessageBox::Critical);
msgBox.setText(lastErrorMsg);
msgBox.exec();
}
}
void OpalErrorMsg::setLastErrorMsg()
{
void OpalErrorMsg::setLastErrorMsg()
{
char* buf = new char[512];
unsigned short len;
static QString lastErrorMsg;
......@@ -22,5 +22,5 @@ namespace OpalRT
lastErrorMsg.clear();
lastErrorMsg.append(buf);
delete buf;
}
}
}
......@@ -37,36 +37,35 @@ This file is part of the QGROUNDCONTROL project
namespace OpalRT
{
/**
/**
Configuration info for the model
*/
const unsigned short NUM_OUTPUT_SIGNALS=48;
/* ------------------------------ Outputs ------------------------------
*
* Port 1: Navigation state estimates
* 1 t [s] time elapsed since INS mode started
* 2-4 p^n [m] navigation frame position (N,E,D)
* 5-7 v^n [m/s] navigation frame velocity (N,E,D)
* 8-10 Euler angles [rad] (roll, pitch, yaw)
* 11-13 Angular rates
* 14-16 b_f [m/s^2] accelerometer biases
* 17-19 b_w [rad/s] gyro biases
*
* Port 2: Navigation system status
* 1 mode (0: initialization, 1: INS)
* 2 t_GPS time elapsed since last valid GPS measurement
* 3 solution status (0: SOL_COMPUTED, 1: INSUFFICIENT_OBS)
* 4 position solution type ( 0: NONE, 34: NARROW_FLOAT,
* 49: WIDE_INT, 50: NARROW_INT)
* 5 # obs (number of visible satellites)
*
* Port 3: Covariance matrix diagonal
* 1-15 diagonal elements of estimation error covariance matrix P
*/
enum SignalPort
{
const unsigned short NUM_OUTPUT_SIGNALS=48;
/* ------------------------------ Outputs ------------------------------
*
* Port 1: Navigation state estimates
* 1 t [s] time elapsed since INS mode started
* 2-4 p^n [m] navigation frame position (N,E,D)
* 5-7 v^n [m/s] navigation frame velocity (N,E,D)
* 8-10 Euler angles [rad] (roll, pitch, yaw)
* 11-13 Angular rates
* 14-16 b_f [m/s^2] accelerometer biases
* 17-19 b_w [rad/s] gyro biases
*
* Port 2: Navigation system status
* 1 mode (0: initialization, 1: INS)
* 2 t_GPS time elapsed since last valid GPS measurement
* 3 solution status (0: SOL_COMPUTED, 1: INSUFFICIENT_OBS)
* 4 position solution type ( 0: NONE, 34: NARROW_FLOAT,
* 49: WIDE_INT, 50: NARROW_INT)
* 5 # obs (number of visible satellites)
*
* Port 3: Covariance matrix diagonal
* 1-15 diagonal elements of estimation error covariance matrix P
*/
enum SignalPort {
T_ELAPSED,
X_POS,
Y_POS,
......@@ -110,27 +109,26 @@ namespace OpalRT
ELE_POUT,
ELE_IOUT,
ELE_DOUT
};
};
/** Component IDs of the parameters. Currently they are all 1 becuase there is no advantage
/** Component IDs of the parameters. Currently they are all 1 becuase there is no advantage
to dividing them between component ids. However this syntax is used so that this can
easily be changed in the future.
*/
enum SubsystemIds
{
enum SubsystemIds {
NAV = 1,
LOG,
CONTROLLER,
SERVO_OUTPUTS,
SERVO_INPUTS
};
};
class OpalErrorMsg
{
class OpalErrorMsg
{
static QString lastErrorMsg;
public:
public:
static void displayLastErrorMsg();
static void setLastErrorMsg();
};
};
}
#endif // OPALRT_H
......@@ -87,8 +87,7 @@ float Parameter::getValue()
int returnVal = OpalGetParameters(allocatedParams, &numParams, &opalID,
numValues, &returnedNumValues, &value);
if (returnVal != EOK)
{
if (returnVal != EOK) {
OpalRT::OpalErrorMsg::displayLastErrorMsg();
return FLT_MAX;
}
......@@ -106,8 +105,7 @@ void Parameter::setValue(float val)
int returnVal = OpalSetParameters(allocatedParams, &numParams, &opalID,
numValues, &returnedNumValues, &value);
if (returnVal != EOK)
{
if (returnVal != EOK) {
//qDebug() << __FILE__ << ":" << __LINE__ << ": Error numer: " << QString::number(returnVal);
OpalErrorMsg::displayLastErrorMsg();
}
......
......@@ -43,9 +43,9 @@ This file is part of the QGROUNDCONTROL project
namespace OpalRT
{
class Parameter
{
public:
class Parameter
{
public:
// Parameter(char *simulinkPath = "",
// char *simulinkName = "",
// uint8_t componentID = 0,
......@@ -59,24 +59,34 @@ namespace OpalRT
Parameter(const Parameter& other);
~Parameter();
const QGCParamID& getParamID() const {return *paramID;}
void setOpalID(unsigned short opalID) {this->opalID = opalID;}
const QString& getSimulinkPath() const {return *simulinkPath;}
const QString& getSimulinkName() const {return *simulinkName;}
uint8_t getComponentID() const {return componentID;}
const QGCParamID& getParamID() const {
return *paramID;
}
void setOpalID(unsigned short opalID) {
this->opalID = opalID;
}
const QString& getSimulinkPath() const {
return *simulinkPath;
}
const QString& getSimulinkName() const {
return *simulinkName;
}
uint8_t getComponentID() const {
return componentID;
}
float getValue();
void setValue(float value);
bool operator==(const Parameter& other) const;
operator QString() const;
protected:
protected:
QString *simulinkPath;
QString *simulinkName;
uint8_t componentID;
QGCParamID *paramID;
unsigned short opalID;
};
};
}
#endif // PARAMETER_H
......@@ -59,8 +59,7 @@ ParameterList::ParameterList()
reqdServoParams->append("RUD_RIGHT_IN");
QString filename(settingsDir.path() + "/ParameterList.xml");
if ((QFile::exists(filename)) && open(filename))
{
if ((QFile::exists(filename)) && open(filename)) {
/* Get a list of the available parameters from opal-rt */
QMap<QString, unsigned short> *opalParams = new QMap<QString, unsigned short>;
......@@ -70,20 +69,15 @@ ParameterList::ParameterList()
QMap<int, QMap<QGCParamID, Parameter> >::iterator componentIter;
QMap<QGCParamID, Parameter>::iterator paramIter;
QString s;
for (componentIter = params->begin(); componentIter != params->end(); ++componentIter)
{
for (componentIter = params->begin(); componentIter != params->end(); ++componentIter) {
paramList->append(QList<Parameter*>());
for (paramIter = (*componentIter).begin(); paramIter != (*componentIter).end(); ++paramIter)
{
for (paramIter = (*componentIter).begin(); paramIter != (*componentIter).end(); ++paramIter) {
paramList->last().append(paramIter.operator ->());
s = (*paramIter).getSimulinkPath() + (*paramIter).getSimulinkName();
if (opalParams->contains(s))
{
if (opalParams->contains(s)) {
(*paramIter).setOpalID(opalParams->value(s));
// qDebug() << __FILE__ << " Line:" << __LINE__ << ": Successfully added " << s;
}
else
{
} else {
qWarning() << __FILE__ << " Line:" << __LINE__ << ": " << s << " was not found in param list";
}
}
......@@ -128,8 +122,7 @@ void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
allocatedPathLen, &maxPathLen, paths,
allocatedNameLen, &maxNameLen, names,
allocatedVarLen, &maxVarLen, var);
if (returnValue!=E2BIG)
{
if (returnValue!=E2BIG) {
// OpalRT::setLastErrorMsg();
OpalRT::OpalErrorMsg::displayLastErrorMsg();
return;
......@@ -160,16 +153,14 @@ void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
allocatedNameLen, &maxNameLen, names,
allocatedVarLen, &maxVarLen, var);
if (returnValue != EOK)
{
if (returnValue != EOK) {
// OpalRT::setLastErrorMsg();
OpalRT::OpalErrorMsg::displayLastErrorMsg();
return;
}
QString path, name;
for (int i=0; i<numParams; ++i)
{
for (int i=0; i<numParams; ++i) {
path.clear();
path.append(paths[i]);
name.clear();
......@@ -194,8 +185,7 @@ int ParameterList::indexOf(const Parameter &p)
QList<QList<Parameter*> >::const_iterator iter;
int index = -1;
for (iter = paramList->begin(); iter != paramList->end(); ++iter)
{
for (iter = paramList->begin(); iter != paramList->end(); ++iter) {
if ((index = (*iter).indexOf(pPtr)) != -1)
return index;
}
......@@ -245,14 +235,12 @@ int ParameterList::count()
bool ParameterList::open(QString filename)
{
QFile paramFile(filename);
if (!paramFile.exists())
{
if (!paramFile.exists()) {
/// \todo open dialog box (maybe: that could also go in comm config window)
return false;
}
if (!paramFile.open(QIODevice::ReadOnly))
{
if (!paramFile.open(QIODevice::ReadOnly)) {
return false;
}
......@@ -272,8 +260,7 @@ bool ParameterList::read(QIODevice *device)
int errorColumn;
if (!paramConfig->setContent(device, true, &errorStr, &errorLine,
&errorColumn))
{
&errorColumn)) {
qDebug() << "Error reading XML Parameter File on line: " << errorLine << errorStr;
return false;
}
......@@ -285,17 +272,14 @@ bool ParameterList::read(QIODevice *device)
}
QDomElement child = root.firstChildElement("Block");
while (!child.isNull())
{
while (!child.isNull()) {
parseBlock(child);
child = child.nextSiblingElement("Block");
}
if (!reqdServoParams->empty())
{
if (!reqdServoParams->empty()) {
qDebug() << __FILE__ << __LINE__ << "Missing the following required servo parameters";
foreach(QString s, *reqdServoParams)
{
foreach(QString s, *reqdServoParams) {
qDebug() << s;
}
}
......@@ -321,13 +305,11 @@ void ParameterList::parseBlock(const QDomElement &block)
id = OpalRT::SERVO_INPUTS;
paramList = block.elementsByTagName("Parameter");
for (int i=0; i < paramList.size(); ++i)
{
for (int i=0; i < paramList.size(); ++i) {
e = paramList.item(i).toElement();
if (e.hasAttribute("SimulinkPath") &&
e.hasAttribute("SimulinkParameterName") &&
e.hasAttribute("QGCParamID"))
{
e.hasAttribute("QGCParamID")) {
p = new Parameter(e.attribute("SimulinkPath"),
e.attribute("SimulinkParameterName"),
......@@ -340,9 +322,7 @@ void ParameterList::parseBlock(const QDomElement &block)
delete p;
}
else
{
} else {
qDebug() << __FILE__ << ":" << __LINE__ << ": error in xml doc in block" << block.attribute("name");
}
}
......
......@@ -40,9 +40,9 @@ This file is part of the QGROUNDCONTROL project
namespace OpalRT
{
class ParameterList
{
public:
class ParameterList
{
public:
class const_iterator
{
......@@ -52,16 +52,32 @@ namespace OpalRT
inline const_iterator() {}
const_iterator(const const_iterator& other);
const_iterator& operator+=(int i) {index += i; return *this;}
bool operator<(const const_iterator& other) const {return (this->paramList == other.paramList)
&&(this->index<other.index);}
bool operator==(const const_iterator& other) const {return (this->paramList == other.paramList)
&&(this->index==other.index);}
bool operator!=(const const_iterator& other) const {return !((*this) == other);}
const Parameter& operator*() const {return paramList[index];}
const Parameter* operator->() const {return &paramList[index];}
const_iterator& operator++() {++index; return *this;}
const_iterator& operator+=(int i) {
index += i;
return *this;
}
bool operator<(const const_iterator& other) const {
return (this->paramList == other.paramList)
&&(this->index<other.index);
}
bool operator==(const const_iterator& other) const {
return (this->paramList == other.paramList)
&&(this->index==other.index);
}
bool operator!=(const const_iterator& other) const {
return !((*this) == other);
}
const Parameter& operator*() const {
return paramList[index];
}
const Parameter* operator->() const {
return &paramList[index];
}
const_iterator& operator++() {
++index;
return *this;
}
private:
const_iterator(QList<Parameter>);
QList<Parameter> paramList;
......@@ -90,21 +106,33 @@ namespace OpalRT
\endcode
*/
int indexOf(const Parameter& p);
bool contains(int compid, QGCParamID paramid) const {return (*params)[compid].contains(paramid);}
bool contains(int compid, QGCParamID paramid) const {
return (*params)[compid].contains(paramid);
}
/// Get a parameter from the list
const Parameter getParameter(int compid, QGCParamID paramid) const {return (*params)[compid][paramid];}
Parameter& getParameter(int compid, QGCParamID paramid) {return (*params)[compid][paramid];}
const Parameter getParameter(int compid, int index) const {return *((*paramList)[compid][index]);}
const Parameter getParameter(int compid, QGCParamID paramid) const {
return (*params)[compid][paramid];
}
Parameter& getParameter(int compid, QGCParamID paramid) {
return (*params)[compid][paramid];
}
const Parameter getParameter(int compid, int index) const {
return *((*paramList)[compid][index]);
}
/** Convenient syntax for calling OpalRT::Parameter::getParameter() */
Parameter& operator()(int compid, QGCParamID paramid) {return getParameter(compid, paramid);}
Parameter& operator()(uint8_t compid, QGCParamID paramid) {return getParameter(static_cast<int>(compid), paramid);}
Parameter& operator()(int compid, QGCParamID paramid) {
return getParameter(compid, paramid);
}
Parameter& operator()(uint8_t compid, QGCParamID paramid) {
return getParameter(static_cast<int>(compid), paramid);
}
const_iterator begin() const;
const_iterator end() const;
protected:
protected:
/** Store the parameters mapped by componentid, and paramid.
\code
// Look up a parameter
......@@ -142,6 +170,6 @@ namespace OpalRT
bool read(QIODevice *device);
void parseBlock(const QDomElement &block);
};
};
}
#endif // PARAMETERLIST_H
......@@ -46,7 +46,8 @@ This file is part of the QGROUNDCONTROL project
* for more information, please see the official website.
* @ref http://pixhawk.ethz.ch/software/mavlink/
**/
class QGCNMEAProtocol : public ProtocolInterface {
class QGCNMEAProtocol : public ProtocolInterface
{
Q_OBJECT
public:
......@@ -55,7 +56,9 @@ public:
void run();
/** @brief Get the human-friendly name of this protocol */
QString getName() { return QString("NMEA (GPS)"); }
QString getName() {
return QString("NMEA (GPS)");
}
public slots:
/** @brief Receive bytes from a communication interface */
......
......@@ -43,30 +43,40 @@ This file is part of the QGROUNDCONTROL project
namespace OpalRT
{
/** Stores a param_id for the mavlink parameter packets. This class adds the convenience
/** Stores a param_id for the mavlink parameter packets. This class adds the convenience
of storing the id as a string (e.g., easy comparison).
\todo Fix: warning: deprecated conversion from string constant to 'char*'
*/
class QGCParamID
{
class QGCParamID
{
// friend QDataStream& operator<<(QDataStream& stream, const QGCParamID& paramid);
public:
public:
QGCParamID(const char[]);
QGCParamID(const QString);
QGCParamID() {}
QGCParamID(const QGCParamID& other);
bool operator<(const QGCParamID& other) const {return data<other.data;}
bool operator==(const QGCParamID& other) const {return data == other.data;}
operator QString() const {return data;}
const QString getParamString() const {return static_cast<const QString>(data);}
int8_t* toInt8_t() const {return (int8_t*)data.toAscii().data();}
protected:
bool operator<(const QGCParamID& other) const {
return data<other.data;
}
bool operator==(const QGCParamID& other) const {
return data == other.data;
}
operator QString() const {
return data;
}
const QString getParamString() const {
return static_cast<const QString>(data);
}
int8_t* toInt8_t() const {
return (int8_t*)data.toAscii().data();
}
protected:
QString data;
};
};
}
#endif // QGCPARAMID_H
This diff is collapsed.
This diff is collapsed.
......@@ -52,7 +52,8 @@ This file is part of the QGROUNDCONTROL project
* safe.
*
*/
class SerialLink : public SerialLinkInterface {
class SerialLink : public SerialLinkInterface
{
Q_OBJECT
//Q_INTERFACES(SerialLinkInterface:LinkInterface)
......
......@@ -36,7 +36,8 @@ This file is part of the QGROUNDCONTROL project
#include <QString>
#include <LinkInterface.h>
class SerialLinkInterface : public LinkInterface {
class SerialLinkInterface : public LinkInterface
{
Q_OBJECT
public:
......
......@@ -93,8 +93,7 @@ void SerialSimulationLink::run()
msleep(rate);
}*/
forever
{
forever {
QGC::SLEEP::msleep(5000);
}
}
......@@ -107,8 +106,7 @@ void SerialSimulationLink::enableLoopBackMode(SerialLinkInterface* loop)
disconnect();
// Delete previous loopback link if exists
if(loopBack != NULL)
{
if(loopBack != NULL) {
delete loopBack;
loopBack = NULL;
}
......@@ -129,12 +127,9 @@ qint64 SerialSimulationLink::bytesAvailable()
{
readyBufferMutex.lock();
qint64 size = 0;
if(loopBack == 0)
{
if(loopBack == 0) {
size = readyBuffer.size();
}
else
{
} else {
size = loopBack->bytesAvailable();
}
readyBufferMutex.unlock();
......@@ -145,8 +140,7 @@ qint64 SerialSimulationLink::bytesAvailable()
void SerialSimulationLink::writeBytes(char* data, qint64 length)
{
/* Write bytes to one line */
for(qint64 i = 0; i < length; i++)
{
for(qint64 i = 0; i < length; i++) {
outStream->operator <<(data[i]);
outStream->flush();
}
......@@ -160,15 +154,12 @@ void SerialSimulationLink::readBytes()
char data[maxLength];
/* Lock concurrent resource readyBuffer */
readyBufferMutex.lock();
if(loopBack == NULL)
{
if(loopBack == NULL) {
// FIXME Maxlength has no meaning here
/* copy leftmost maxLength bytes and remove them from buffer */
qstrncpy(data, readyBuffer.left(maxLength).data(), maxLength);
readyBuffer.remove(0, maxLength);
}
else
{
} else {
//loopBack->readBytes(data, maxLength);
}
readyBufferMutex.unlock();
......@@ -188,8 +179,7 @@ void SerialSimulationLink::readBytes()
void SerialSimulationLink::readLine()
{
if(_isConnected)
{
if(_isConnected) {
/* The order of operations in this method is arranged to
* minimize the impact of slow file read operations on the
* message emit timing. The functions should be kept in this order
......@@ -200,19 +190,15 @@ void SerialSimulationLink::readLine()
/* (2) Save content of line buffer in readyBuffer (has to be lock for thread safety)*/
readyBufferMutex.lock();
if(loopBack == NULL)
{
if(loopBack == NULL) {
readyBuffer.append(lineBuffer);
//qDebug() << "readLine readyBuffer: " << readyBuffer;
}
else
{
} else {
loopBack->writeBytes(lineBuffer.data(), lineBuffer.size());
}
readyBufferMutex.unlock();
if(loopBack == NULL)
{
if(loopBack == NULL) {
readBytes();
}
......@@ -290,7 +276,8 @@ void SerialSimulationLink::addTimeNoise()
* @return True if connection has been disconnected, false if connection
* couldn't be disconnected.
**/
bool SerialSimulationLink::disconnect() {
bool SerialSimulationLink::disconnect()
{
if(isConnected()) {
timer->stop();
......
......@@ -46,7 +46,8 @@ This file is part of the QGROUNDCONTROL project
* by communicating with a simulation.
*
**/
class SerialSimulationLink : public SerialLinkInterface {
class SerialSimulationLink : public SerialLinkInterface
{
Q_OBJECT
public:
......
This diff is collapsed.
......@@ -51,7 +51,9 @@ public:
bool isConnected();
qint64 bytesAvailable();
int getPort() const { return port; }
int getPort() const {
return port;
}
/**
* @brief The human readable port name
......@@ -63,7 +65,9 @@ public:
int getParityType();
int getDataBitsType();
int getStopBitsType();
QList<QHostAddress> getHosts() { return hosts; }
QList<QHostAddress> getHosts() {
return hosts;
}
/* Extensive statistics for scientific purposes */
qint64 getNominalDataRate();
......
This diff is collapsed.
This diff is collapsed.
......@@ -56,8 +56,7 @@ public:
QSharedPointer<QByteArray> getColoredDepthData(void);
QSharedPointer< QVector<QVector3D> > get3DPointCloudData(void);
typedef struct
{
typedef struct {
double x;
double y;
double z;
......@@ -71,8 +70,7 @@ public:
void setTiltAngle(int angle);
private:
typedef struct
{
typedef struct {
// coordinates of principal point
double cx;
double cy;
......
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
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