Commit 45d5c5e6 authored by Don Gagne's avatar Don Gagne

Remove unused code

parent c1e6f573
......@@ -272,7 +272,6 @@ FORMS += \
src/ui/AudioOutputWidget.ui \
src/ui/QGCSensorSettingsWidget.ui \
src/ui/QGCDataPlot2D.ui \
src/ui/QGCRemoteControlView.ui \
src/ui/QMap3D.ui \
src/ui/QGCWebView.ui \
src/ui/map3D/QGCGoogleEarthView.ui \
......@@ -290,7 +289,6 @@ FORMS += \
src/ui/QGCUDPLinkConfiguration.ui \
src/ui/QGCTCPLinkConfiguration.ui \
src/ui/SettingsDialog.ui \
src/ui/UASControlParameters.ui \
src/ui/map/QGCMapTool.ui \
src/ui/map/QGCMapToolBar.ui \
src/ui/QGCMAVLinkInspector.ui \
......@@ -317,7 +315,6 @@ FORMS += \
src/ui/QGCHilXPlaneConfiguration.ui \
src/ui/uas/UASQuickView.ui \
src/ui/uas/UASQuickViewItemSelect.ui \
src/ui/uas/UASActionsWidget.ui \
src/ui/QGCTabbedInfoView.ui \
src/ui/UASRawStatusView.ui \
src/ui/uas/QGCMessageView.ui \
......@@ -384,13 +381,6 @@ HEADERS += \
src/QGC.h \
src/ui/QGCDataPlot2D.h \
src/ui/linechart/IncrementalPlot.h \
src/ui/QGCRemoteControlView.h \
src/ui/RadioCalibration/RadioCalibrationData.h \
src/ui/RadioCalibration/RadioCalibrationWindow.h \
src/ui/RadioCalibration/AirfoilServoCalibrator.h \
src/ui/RadioCalibration/SwitchCalibrator.h \
src/ui/RadioCalibration/CurveCalibrator.h \
src/ui/RadioCalibration/AbstractCalibrator.h \
src/comm/QGCMAVLink.h \
src/ui/QGCWebView.h \
src/ui/map3D/QGCWebPage.h \
......@@ -415,7 +405,6 @@ HEADERS += \
src/ui/QGCUDPLinkConfiguration.h \
src/ui/QGCTCPLinkConfiguration.h \
src/ui/SettingsDialog.h \
src/ui/uas/UASControlParameters.h \
src/uas/QGCUASParamManager.h \
src/ui/map/QGCMapWidget.h \
src/ui/map/MAV2DIcon.h \
......@@ -457,7 +446,6 @@ HEADERS += \
src/ui/uas/UASQuickViewItemSelect.h \
src/ui/uas/UASQuickViewTextItem.h \
src/ui/uas/UASQuickViewGaugeItem.h \
src/ui/uas/UASActionsWidget.h \
src/ui/QGCTabbedInfoView.h \
src/ui/UASRawStatusView.h \
src/ui/PrimaryFlightDisplay.h \
......@@ -538,13 +526,6 @@ SOURCES += \
src/QGC.cc \
src/ui/QGCDataPlot2D.cc \
src/ui/linechart/IncrementalPlot.cc \
src/ui/QGCRemoteControlView.cc \
src/ui/RadioCalibration/RadioCalibrationWindow.cc \
src/ui/RadioCalibration/AirfoilServoCalibrator.cc \
src/ui/RadioCalibration/SwitchCalibrator.cc \
src/ui/RadioCalibration/CurveCalibrator.cc \
src/ui/RadioCalibration/AbstractCalibrator.cc \
src/ui/RadioCalibration/RadioCalibrationData.cc \
src/ui/QGCWebView.cc \
src/ui/map3D/QGCWebPage.cc \
src/ui/QGCMainWindowAPConfigurator.cc \
......@@ -568,7 +549,6 @@ SOURCES += \
src/ui/QGCUDPLinkConfiguration.cc \
src/ui/QGCTCPLinkConfiguration.cc \
src/ui/SettingsDialog.cc \
src/ui/uas/UASControlParameters.cpp \
src/uas/QGCUASParamManager.cc \
src/ui/map/QGCMapWidget.cc \
src/ui/map/MAV2DIcon.cc \
......@@ -608,7 +588,6 @@ SOURCES += \
src/ui/uas/UASQuickViewTextItem.cc \
src/ui/uas/UASQuickViewGaugeItem.cc \
src/ui/uas/UASQuickViewItemSelect.cc \
src/ui/uas/UASActionsWidget.cpp \
src/ui/QGCTabbedInfoView.cpp \
src/ui/UASRawStatusView.cpp \
src/ui/PrimaryFlightDisplay.cc \
......
......@@ -43,7 +43,6 @@ This file is part of the QGROUNDCONTROL project
#include "UASParameterDataModel.h"
#include "UASWaypointManager.h"
#include "QGCUASParamManagerInterface.h"
#include "RadioCalibration/RadioCalibrationData.h"
class QGCUASFileManager;
......@@ -584,8 +583,6 @@ signals:
void remoteControlChannelScaledChanged(int channelId, float normalized);
/** @brief Remote control RSSI changed */
void remoteControlRSSIChanged(float rssi);
/** @brief Radio Calibration Data has been received from the MAV*/
void radioCalibrationReceived(const QPointer<RadioCalibrationData>&);
/**
* @brief Localization quality changed
......
#include "UASParameterCommsMgr.h"
#include <QSettings>
#include <QDebug>
#include "QGCUASParamManagerInterface.h"
#include "UASInterface.h"
......
#include "MAVLinkDecoder.h"
#include "UASManager.h"
#include <QDebug>
MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
QThread()
{
......
......@@ -61,7 +61,6 @@ This file is part of the QGROUNDCONTROL project
#include "UASQuickView.h"
#include "QGCDataPlot2D.h"
#include "Linecharts.h"
#include "UASActionsWidget.h"
#include "QGCTabbedInfoView.h"
#include "UASRawStatusView.h"
#include "PrimaryFlightDisplay.h"
......
......@@ -57,7 +57,6 @@ This file is part of the QGROUNDCONTROL project
#include "ParameterInterface.h"
#include "HDDisplay.h"
#include "HSIDisplay.h"
#include "QGCRemoteControlView.h"
#include "opmapcontrol.h"
#ifdef QGC_GOOGLE_EARTH_ENABLED
#include "QGCGoogleEarthView.h"
......@@ -65,11 +64,11 @@ This file is part of the QGROUNDCONTROL project
#include "QGCToolBar.h"
#include "LogCompressor.h"
#include "UASControlParameters.h"
#include "QGCMAVLinkInspector.h"
#include "QGCMAVLinkLogPlayer.h"
#include "MAVLinkDecoder.h"
#include "QGCUASFileViewMulti.h"
#include "QGCFlightGearLink.h"
class QGCMapTool;
class QGCMAVLinkMessageSender;
......
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Implementation of QGCRemoteControlView
* @author Lorenz Meier <mail@qgroundcontrol.org>
* @author Bryan Godbolt <godbolt@ece.ualberta.ca>
*/
#include <QGridLayout>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QPushButton>
#include <QLabel>
#include <QProgressBar>
#include "QGCRemoteControlView.h"
#include "ui_QGCRemoteControlView.h"
#include "UASManager.h"
QGCRemoteControlView::QGCRemoteControlView(QWidget *parent) :
QWidget(parent),
uasId(-1),
rssi(0.0f),
updated(false),
channelLayout(new QVBoxLayout()),
ui(NULL)
{
ui->setupUi(this);
QGridLayout* layout = new QGridLayout(this);
layout->addLayout(channelLayout, 1, 0, 1, 2);
nameLabel = new QLabel(this);
layout->addWidget(nameLabel, 0, 0, 1, 2);
this->setVisible(false);
//setVisible(false);
// calibrate = new QPushButton(tr("Calibrate"), this);
// QHBoxLayout *calibrateButtonLayout = new QHBoxLayout();
// calibrateButtonLayout->addWidget(calibrate, 0, Qt::AlignHCenter);
// layout->addItem(calibrateButtonLayout, 3, 0, 1, 2);
// calibrationWindow = new RadioCalibrationWindow(this);
// connect(calibrate, SIGNAL(clicked()), calibrationWindow, SLOT(show()));
connect(UASManager::instance(), SIGNAL(activeUASSet(int)), this, SLOT(setUASId(int)));
connect(&updateTimer, SIGNAL(timeout()), this, SLOT(redraw()));
updateTimer.start(1500);
}
QGCRemoteControlView::~QGCRemoteControlView()
{
if(this->ui)
{
delete ui;
}
if(this->channelLayout)
{
delete channelLayout;
}
}
void QGCRemoteControlView::setUASId(int id)
{
if (uasId != -1)
{
UASInterface* uas = UASManager::instance()->getUASForId(uasId);
if (uas)
{
// The UAS exists, disconnect any existing connections
disconnect(uas, SIGNAL(remoteControlChannelRawChanged(int,float,float)), this, SLOT(setChannel(int,float,float)));
disconnect(uas, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float)));
//disconnect(uas, SIGNAL(radioCalibrationRawReceived(const QPointer<RadioCalibrationData>&)), calibrationWindow, SLOT(receive(const QPointer<RadioCalibrationData>&)));
//disconnect(uas, SIGNAL(remoteControlChannelRawChanged(int,float)), calibrationWindow, SLOT(setChannel(int,float)));
disconnect(uas, SIGNAL(remoteControlChannelScaledChanged(int,float)), this, SLOT(setChannelScaled(int,float)));
}
}
// Clear channel count
raw.clear();
raw.resize(0);
normalized.clear();
normalized.resize(0);
foreach (QLabel* label, rawLabels)
{
label->deleteLater();
}
foreach(QProgressBar* bar, progressBars)
{
bar->deleteLater();
}
rawLabels.clear();
rawLabels.resize(0);
progressBars.clear();
progressBars.resize(0);
// Connect the new UAS
UASInterface* newUAS = UASManager::instance()->getUASForId(id);
if (newUAS)
{
// New UAS exists, connect
nameLabel->setText(QString("RC Input of %1").arg(newUAS->getUASName()));
//calibrationWindow->setUASId(id);
//connect(newUAS, SIGNAL(radioCalibrationReceived(const QPointer<RadioCalibrationData>&)), calibrationWindow, SLOT(receive(const QPointer<RadioCalibrationData>&)));
connect(newUAS, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float)));
connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), this, SLOT(setChannelRaw(int,float)));
connect(newUAS, SIGNAL(remoteControlChannelScaledChanged(int,float)), this, SLOT(setChannelScaled(int,float)));
// only connect raw channels to calibration window widget
//connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), calibrationWindow, SLOT(setChannel(int,float)));
}
}
void QGCRemoteControlView::setChannelRaw(int channelId, float raw)
{
if (this->raw.count() <= channelId) {
// This is a new channel, append it
this->raw.append(raw);
appendChannelWidget(channelId);
updated = true;
} else {
// This is an existing channel, aupdate it
if (this->raw[channelId] != raw) updated = true;
this->raw[channelId] = raw;
}
}
void QGCRemoteControlView::setChannelScaled(int channelId, float normalized)
{
if (this->normalized.count() <= channelId) // using raw vector as size indicator
{
// This is a new channel, append it
this->normalized.append(normalized);
appendChannelWidget(channelId);
updated = true;
}
else
{
// This is an existing channel, update it
if (this->normalized[channelId] != normalized) updated = true;
this->normalized[channelId] = normalized;
}
}
void QGCRemoteControlView::setRemoteRSSI(float rssiNormalized)
{
if (rssi != rssiNormalized) updated = true;
rssi = rssiNormalized;
}
void QGCRemoteControlView::appendChannelWidget(int channelId)
{
// Create new layout
QHBoxLayout* layout = new QHBoxLayout();
// Add content
layout->addWidget(new QLabel(QString("Channel %1").arg(channelId + 1), this));
QLabel* raw = new QLabel(this);
// Append raw label
rawLabels.append(raw);
layout->addWidget(raw);
// Append progress bar
QProgressBar* normalized = new QProgressBar(this);
normalized->setMinimum(-100);
normalized->setMaximum(100);
normalized->setFormat("%v%");
progressBars.append(normalized);
layout->addWidget(normalized);
channelLayout->addLayout(layout);
}
void QGCRemoteControlView::redraw()
{
if(isVisible() && updated)
{
// Update percent bars and raw labels
for(int i = 0; (i < progressBars.count()) && (i < rawLabels.count()) && (i < normalized.count()) && (i < raw.count()); i++)
{
rawLabels.at(i)->setText(QString("%1 us").arg(raw.at(i), 4, 10, QChar('0')));
int vv = normalized.at(i)*100.0f;
progressBars.at(i)->setValue(vv);
}
// Update RSSI
if(rssi>0) {
//rssiBar->setValue(rssi);//*100);
}
updated = false;
}
}
void QGCRemoteControlView::changeEvent(QEvent *e)
{
Q_UNUSED(e);
// FIXME If the lines below are commented in
// runtime errors can occur on x64 systems.
// QWidget::changeEvent(e);
// switch (e->type()) {
// case QEvent::LanguageChange:
// //ui->retranslateUi(this);
// break;
// default:
// break;
// }
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Declaration of QGCRemoteControlView
* @author Lorenz Meier <mail@qgroundcontrol.org>
*/
#ifndef QGCREMOTECONTROLVIEW_H
#define QGCREMOTECONTROLVIEW_H
#include <QWidget>
#include <QVector>
#include <QPushButton>
#include "RadioCalibration/RadioCalibrationWindow.h"
namespace Ui
{
class QGCRemoteControlView;
}
class QVBoxLayout;
class QLabel;
class QProgressBar;
class QGCRemoteControlView : public QWidget
{
Q_OBJECT
public:
QGCRemoteControlView(QWidget *parent = 0);
~QGCRemoteControlView();
public slots:
void setUASId(int id);
void setChannelRaw(int channelId, float raw);
void setChannelScaled(int channelId, float normalized);
void setRemoteRSSI(float rssiNormalized);
void redraw();
protected slots:
void appendChannelWidget(int channelId);
protected:
void changeEvent(QEvent *e);
int uasId;
float rssi;
bool updated;
QVBoxLayout* channelLayout;
QVector<int> raw;
QVector<float> normalized;
QVector<QLabel*> rawLabels;
QVector<QProgressBar*> progressBars;
QProgressBar* rssiBar;
QLabel* nameLabel;
QPushButton *calibrate;
RadioCalibrationWindow *calibrationWindow;
QTimer updateTimer;
private:
Ui::QGCRemoteControlView *ui;
};
#endif // QGCREMOTECONTROLVIEW_H
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>QGCRemoteControlView</class>
<widget class="QWidget" name="QGCRemoteControlView">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>155</width>
<height>106</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
</widget>
<resources/>
<connections/>
</ui>
......@@ -24,6 +24,8 @@ This file is part of the QGROUNDCONTROL project
#include <QToolButton>
#include <QLabel>
#include <QSpacerItem>
#include <QFileDialog>
#include "QGCStatusBar.h"
#include "UASManager.h"
#include "MainWindow.h"
......
......@@ -5,7 +5,6 @@
#include "ui_QGCTabbedInfoView.h"
#include "MAVLinkDecoder.h"
#include "QGCMessageView.h"
#include "UASActionsWidget.h"
#include "UASQuickView.h"
#include "UASRawStatusView.h"
class QGCTabbedInfoView : public QWidget
......@@ -20,7 +19,6 @@ private:
MAVLinkDecoder *m_decoder;
Ui::QGCTabbedInfoView ui;
QGCMessageView *messageView;
UASActionsWidget *actionsWidget;
UASQuickView *quickView;
UASRawStatusView *rawView;
};
......
#include "AbstractCalibrator.h"
AbstractCalibrator::AbstractCalibrator(QWidget *parent) :
QWidget(parent),
pulseWidth(new QLabel()),
log(new QVector<uint16_t>())
{
}
AbstractCalibrator::~AbstractCalibrator()
{
delete log;
}
uint16_t AbstractCalibrator::logAverage()
{
// Short-circuit here if the log is empty otherwise we get a div-by-0 error.
if (log->empty())
{
return 0;
}
uint16_t total = 0;
for (int i=0; i<log->size(); ++i)
{
total += log->value(i);
}
return total/log->size();
}
uint16_t AbstractCalibrator::logExtrema()
{
uint16_t extrema = logAverage();
if (logAverage() < 1500)
{
for (int i=0; i<log->size(); ++i)
{
if (log->value(i) < extrema)
{
extrema = log->value(i);
}
}
extrema -= 5; // add 5us to prevent integer overflow
}
else
{
for (int i=0; i<log->size(); ++i)
{
if (log->value(i) > extrema)
{
extrema = log->value(i);
}
}
extrema += 5; // subtact 5us to prevent integer overflow
}
return extrema;
}
void AbstractCalibrator::channelChanged(uint16_t raw)
{
pulseWidth->setText(QString::number(raw));
if (log->size() == 5)
{
log->pop_front();
}
log->push_back(raw);
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Common aspects of radio calibration widgets
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#ifndef ABSTRACTCALIBRATOR_H
#define ABSTRACTCALIBRATOR_H
#include <QWidget>
#include <QString>
#include <QLabel>
#include <QVector>
#include <math.h>
#include <stdint.h>
/**
@brief Holds the code which is common to all the radio calibration widgets.
@author Bryan Godbolt <godbolt@ece.ualberta.ca>
*/
class AbstractCalibrator : public QWidget
{
Q_OBJECT
public:
explicit AbstractCalibrator(QWidget *parent = 0);
~AbstractCalibrator();
/** Change the setpoints of the widget. Used when
changing the display from an external source (file/uav).
@param data QVector of setpoints
*/
virtual void set(const QVector<uint16_t>& data)=0;
signals:
/** Announce a setpoint change.
@param index setpoint number - 0 based in the current implementation
@param value new value
*/
void setpointChanged(int index, uint16_t value);
public slots:
/** Slot to call when the relevant channel is updated
@param raw current channel value
*/
void channelChanged(uint16_t raw);
protected:
/** Display the current pulse width */
QLabel *pulseWidth;
/** Log of the past few samples for use in averaging and finding extrema */
QVector<uint16_t> *log;
/** Find the maximum or minimum of the data log */
uint16_t logExtrema();
/** Find the average of the log */
uint16_t logAverage();
};
#endif // ABSTRACTCALIBRATOR_H
#include "AirfoilServoCalibrator.h"
AirfoilServoCalibrator::AirfoilServoCalibrator(AirfoilType type, QWidget *parent) :
AbstractCalibrator(parent),
highPulseWidth(new QLabel()),
centerPulseWidth(new QLabel()),
lowPulseWidth(new QLabel())
{
QGridLayout *grid = new QGridLayout(this);
/* Add title */
QHBoxLayout *titleLayout = new QHBoxLayout();
QLabel* title;
if (type == AILERON) {
title = new QLabel(tr("Aileron"));
} else if (type == ELEVATOR) {
title = new QLabel(tr("Elevator"));
} else if (type == RUDDER) {
title = new QLabel(tr("Rudder"));
} else {
title = new QLabel(tr("Unknown"));
}
titleLayout->addWidget(title);
grid->addLayout(titleLayout, 0, 0, 1, 3, Qt::AlignHCenter);
/* Add current Pulse Width Display */
QLabel *pulseWidthTitle = new QLabel(tr("Pulse Width (us)"));
QHBoxLayout *pulseLayout = new QHBoxLayout();
pulseLayout->addWidget(pulseWidthTitle);
pulseLayout->addWidget(pulseWidth);
grid->addLayout(pulseLayout, 1, 0, 1, 3);
QLabel *highPulseString;
QLabel *centerPulseString;
QLabel *lowPulseString;
if (type == AILERON) {
highPulseString = new QLabel(tr("Bank Right"));
centerPulseString = new QLabel(tr("Center"));
lowPulseString = new QLabel(tr("Bank Left"));
} else if (type == ELEVATOR) {
highPulseString = new QLabel(tr("Nose Up"));
centerPulseString = new QLabel(tr("Center"));
lowPulseString = new QLabel(tr("Nose Down"));
} else if (type == RUDDER) {
highPulseString = new QLabel(tr("Nose Right"));
centerPulseString = new QLabel(tr("Center"));
lowPulseString = new QLabel(tr("Nose Left"));
} else {
highPulseString = new QLabel(tr("High"));
centerPulseString = new QLabel(tr("Center"));
lowPulseString = new QLabel(tr("Low"));
}
QPushButton *highButton = new QPushButton(tr("Set"));
QPushButton *centerButton = new QPushButton(tr("Set"));
QPushButton *lowButton = new QPushButton(tr("Set"));
grid->addWidget(highPulseString, 2, 0);
grid->addWidget(highPulseWidth, 2, 1);
grid->addWidget(highButton, 2, 2);
grid->addWidget(centerPulseString, 3, 0);
grid->addWidget(centerPulseWidth, 3, 1);
grid->addWidget(centerButton, 3, 2);
grid->addWidget(lowPulseString, 4, 0);
grid->addWidget(lowPulseWidth, 4, 1);
grid->addWidget(lowButton, 4, 2);
this->setLayout(grid);
connect(highButton, SIGNAL(clicked()), this, SLOT(setHigh()));
connect(centerButton, SIGNAL(clicked()), this, SLOT(setCenter()));
connect(lowButton, SIGNAL(clicked()), this, SLOT(setLow()));
}
void AirfoilServoCalibrator::setHigh()
{
highPulseWidth->setText(QString::number(logExtrema()));
emit setpointChanged(2, logExtrema());
}
void AirfoilServoCalibrator::setCenter()
{
centerPulseWidth->setText(QString::number(logAverage()));
emit setpointChanged(1, logAverage());
}
void AirfoilServoCalibrator::setLow()
{
lowPulseWidth->setText(QString::number(logExtrema()));
emit setpointChanged(0, logExtrema());
}
void AirfoilServoCalibrator::set(const QVector<uint16_t> &data)
{
if (data.size() == 3) {
lowPulseWidth->setText(QString::number(data[0]));
centerPulseWidth->setText(QString::number(data[1]));
highPulseWidth->setText(QString::number(data[2]));
}
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Calibration widget for 3 point airfoil servo
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#ifndef AIRFOILSERVOCALIBRATOR_H
#define AIRFOILSERVOCALIBRATOR_H
#include <QWidget>
#include <QLabel>
#include <QPushButton>
#include <QVector>
#include <QGridLayout>
#include <QHBoxLayout>
#include "AbstractCalibrator.h"
/**
@brief Calibration widget three setpoint control input.
For the helicopter autopilot at UAlberta this is used for Aileron, Elevator, and Rudder channels.
@author Bryan Godbolt <godbolt@ece.ualberta.ca>
*/
class AirfoilServoCalibrator : public AbstractCalibrator
{
Q_OBJECT
public:
enum AirfoilType {
AILERON,
ELEVATOR,
RUDDER
};
explicit AirfoilServoCalibrator(AirfoilType type = AILERON, QWidget *parent = 0);
/** @param data must have exaclty 3 elemets. they are assumed to be low center high */
void set(const QVector<uint16_t>& data);
protected slots:
void setHigh();
void setCenter();
void setLow();
protected:
QLabel *highPulseWidth;
QLabel *centerPulseWidth;
QLabel *lowPulseWidth;
};
#endif // AIRFOILSERVOCALIBRATOR_H
#include "CurveCalibrator.h"
CurveCalibrator::CurveCalibrator(QString titleString, QWidget *parent) :
AbstractCalibrator(parent),
setpoints(new QVector<uint16_t>(5)),
positions(new QVector<uint16_t>())
{
QGridLayout *grid = new QGridLayout(this);
QLabel *title = new QLabel(titleString);
grid->addWidget(title, 0, 0, 1, 5, Qt::AlignHCenter);
QLabel *pulseWidthTitle = new QLabel(tr("Pulse Width (us)"));
pulseWidth = new QLabel();
QHBoxLayout *pulseLayout = new QHBoxLayout();
pulseLayout->addWidget(pulseWidthTitle);
pulseLayout->addWidget(pulseWidth);
grid->addLayout(pulseLayout, 1, 0, 1, 5, Qt::AlignHCenter);
for (int i=0; i<=100; i=i+100/4)
positions->append(static_cast<double>(i));
setpoints->fill(1500);
plot = new QwtPlot();
grid->addWidget(plot, 2, 0, 1, 5, Qt::AlignHCenter);
plot->setAxisScale(QwtPlot::yLeft, 1000, 2000, 200);
plot->setAxisScale(QwtPlot::xBottom, 0, 100, 25);
curve = new QwtPlotCurve();
curve->setPen(QPen(QColor(QString("lime"))));
QVector<double> pos(positions->size());
QVector<double> set(setpoints->size());
for (int i=0; i<positions->size()&&i<setpoints->size(); i++)
{
pos[i] = static_cast<double>((*positions)[i]);
set[i] = static_cast<double>((*setpoints)[i]);
}
curve->setSamples(pos, set);
curve->attach(plot);
plot->replot();
QPushButton *zero = new QPushButton(tr("0 %"));
QPushButton *twentyfive = new QPushButton(tr("25 %"));
QPushButton *fifty = new QPushButton(tr("50 %"));
QPushButton *seventyfive = new QPushButton(tr("75 %"));
QPushButton *hundred = new QPushButton(tr("100 %"));
grid->addWidget(zero, 3, 0);
grid->addWidget(twentyfive, 3, 1);
grid->addWidget(fifty, 3, 2);
grid->addWidget(seventyfive, 3, 3);
grid->addWidget(hundred, 3, 4);
this->setLayout(grid);
signalMapper = new QSignalMapper(this);
signalMapper->setMapping(zero, 0);
signalMapper->setMapping(twentyfive, 1);
signalMapper->setMapping(fifty, 2);
signalMapper->setMapping(seventyfive, 3);
signalMapper->setMapping(hundred, 4);
connect(zero, SIGNAL(clicked()), signalMapper, SLOT(map()));
connect(twentyfive, SIGNAL(clicked()), signalMapper, SLOT(map()));
connect(fifty, SIGNAL(clicked()), signalMapper, SLOT(map()));
connect(seventyfive, SIGNAL(clicked()), signalMapper, SLOT(map()));
connect(hundred, SIGNAL(clicked()), signalMapper, SLOT(map()));
connect(signalMapper, SIGNAL(mapped(int)), this, SLOT(setSetpoint(int)));
}
CurveCalibrator::~CurveCalibrator()
{
delete setpoints;
delete positions;
}
void CurveCalibrator::setSetpoint(int setpoint)
{
if (setpoint == 0 || setpoint == 4) {
setpoints->replace(setpoint,logExtrema());
} else {
setpoints->replace(setpoint, logAverage());
}
QVector<double> pos(positions->size());
QVector<double> set(setpoints->size());
for (int i=0; i<positions->size()&&i<setpoints->size(); i++)
{
pos[i] = static_cast<double>((*positions)[i]);
set[i] = static_cast<double>((*setpoints)[i]);
}
curve->setSamples(pos, set);
plot->replot();
emit setpointChanged(setpoint, setpoints->value(setpoint));
}
void CurveCalibrator::set(const QVector<uint16_t> &data)
{
if (data.size() == 5) {
for (int i=0; i<data.size(); ++i)
setpoints->replace(i, data[i]);
QVector<double> pos(positions->size());
QVector<double> set(setpoints->size());
for (int i=0; i<positions->size()&&i<setpoints->size(); i++)
{
pos[i] = static_cast<double>((*positions)[i]);
set[i] = static_cast<double>((*setpoints)[i]);
}
curve->setSamples(pos, set);
plot->replot();
} else {
qDebug() << __FILE__ << __LINE__ << ": wrong data vector size";
}
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Calibration widget for 5 point inerpolated curve
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#ifndef CURVECALIBRATOR_H
#define CURVECALIBRATOR_H
#include <QWidget>
#include <QVector>
#include <qwt_plot.h>
#include <qwt_plot_curve.h>
#include <QGridLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QPushButton>
#include <QPen>
#include <QColor>
#include <QString>
#include <QSignalMapper>
#include <QDebug>
#include "AbstractCalibrator.h"
/**
@brief Calibration widget for 5 point inerpolated curve.
For the helicopter autopilot at UAlberta this is used for the throttle and pitch curves.
*/
class CurveCalibrator : public AbstractCalibrator
{
Q_OBJECT
public:
explicit CurveCalibrator(QString title = QString(), QWidget *parent = 0);
~CurveCalibrator();
void set(const QVector<uint16_t> &data);
protected slots:
void setSetpoint(int setpoint);
protected:
QVector<uint16_t> *setpoints;
QVector<uint16_t> *positions;
/** Plot to display calibration curve */
QwtPlot *plot;
/** Curve object of calibration curve */
QwtPlotCurve *curve;
QSignalMapper *signalMapper;
};
#endif // CURVECALIBRATOR_H
#include "RadioCalibrationData.h"
RadioCalibrationData::RadioCalibrationData()
{
data = new QVector<QVector<uint16_t> >(6);
(*data).insert(AILERON, QVector<uint16_t>(3));
(*data).insert(ELEVATOR, QVector<uint16_t>(3));
(*data).insert(RUDDER, QVector<uint16_t>(3));
(*data).insert(GYRO, QVector<uint16_t>(2));
(*data).insert(PITCH, QVector<uint16_t>(5));
(*data).insert(THROTTLE, QVector<uint16_t>(5));
}
RadioCalibrationData::RadioCalibrationData(const QVector<uint16_t> &aileron,
const QVector<uint16_t> &elevator,
const QVector<uint16_t> &rudder,
const QVector<uint16_t> &gyro,
const QVector<uint16_t> &pitch,
const QVector<uint16_t> &throttle)
{
data = new QVector<QVector<uint16_t> >();
(*data) << aileron
<< elevator
<< rudder
<< gyro
<< pitch
<< throttle;
}
RadioCalibrationData::RadioCalibrationData(const RadioCalibrationData &other)
:QObject()
{
data = new QVector<QVector<uint16_t> >(*other.data);
}
RadioCalibrationData::~RadioCalibrationData()
{
delete data;
}
const uint16_t* RadioCalibrationData::operator [](int i) const
{
if (i < data->size()) {
return (*data)[i].constData();
}
return NULL;
}
const QVector<uint16_t>& RadioCalibrationData::operator ()(const int i) const throw(std::out_of_range)
{
if ((i < data->size()) && (i >=0)) {
return (*data)[i];
}
throw std::out_of_range("Invalid channel index");
}
QString RadioCalibrationData::toString(RadioElement element) const
{
QString s;
foreach (float f, (*data)[element]) {
s += QString::number(f) + ", ";
}
return s.mid(0, s.length()-2);
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Class to hold the calibration data
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#ifndef RADIOCALIBRATIONDATA_H
#define RADIOCALIBRATIONDATA_H
#include <QObject>
#include <QDebug>
#include <QVector>
#include <QString>
#include <stdexcept>
#include <stdint.h>
/**
@brief Class to hold the calibration data.
@author Bryan Godbolt <godbolt@ece.ualberta.ca>
*/
class RadioCalibrationData : public QObject
{
Q_OBJECT
public:
explicit RadioCalibrationData();
RadioCalibrationData(const RadioCalibrationData&);
RadioCalibrationData(const QVector<uint16_t>& aileron,
const QVector<uint16_t>& elevator,
const QVector<uint16_t>& rudder,
const QVector<uint16_t>& gyro,
const QVector<uint16_t>& pitch,
const QVector<uint16_t>& throttle);
~RadioCalibrationData();
enum RadioElement {
AILERON=0,
ELEVATOR,
RUDDER,
GYRO,
PITCH,
THROTTLE
};
const uint16_t* operator[](int i) const;
#ifdef _MSC_VER
const QVector<uint16_t>& operator()(int i) const;
#else
const QVector<uint16_t>& operator()(int i) const throw(std::out_of_range);
#endif
void set(int element, int index, float value) {
(*data)[element][index] = value;
}
public slots:
void setAileron(int index, uint16_t value) {
set(AILERON, index, value);
}
void setElevator(int index, uint16_t value) {
set(ELEVATOR, index, value);
}
void setRudder(int index, uint16_t value) {
set(RUDDER, index, value);
}
void setGyro(int index, uint16_t value) {
set(GYRO, index, value);
}
void setPitch(int index, uint16_t value) {
set(PITCH, index, value);
}
void setThrottle(int index, uint16_t value) {
set(THROTTLE, index, value);
}
public:
/// Creates a comma seperated list of the values for a particular element
QString toString(const RadioElement element) const;
protected:
QVector<QVector<uint16_t> > *data;
void init(const QVector<float>& aileron,
const QVector<float>& elevator,
const QVector<float>& rudder,
const QVector<float>& gyro,
const QVector<float>& pitch,
const QVector<float>& throttle);
};
#endif // RADIOCALIBRATIONDATA_H
This diff is collapsed.
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Main window for radio calibration
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#ifndef RADIOCALIBRATIONWINDOW_H
#define RADIOCALIBRATIONWINDOW_H
#include <QWidget>
#include <QLabel>
#include <QGroupBox>
#include <QPushButton>
#include <QVector>
#include <QGridLayout>
#include <QHBoxLayout>
#include <QDebug>
#include <QPointer>
#include <QFile>
#include <QtXml>
#include <QTextStream>
#include <stdint.h>
#include "AirfoilServoCalibrator.h"
#include "SwitchCalibrator.h"
#include "CurveCalibrator.h"
#include "UAS.h"
#include "UASManager.h"
#include "RadioCalibrationData.h"
/**
@brief Main window for radio calibration
@author Bryan Godbolt <godbolt@ece.ualberta.ca>
*/
class RadioCalibrationWindow : public QWidget
{
Q_OBJECT
public:
explicit RadioCalibrationWindow(QWidget *parent = 0);
public slots:
void setChannel(int ch, float raw);
void loadFile();
void saveFile();
void send();
void request();
void receive(const QPointer<RadioCalibrationData>& radio);
void setUASId(int id) {
this->uasId = id;
}
protected:
AirfoilServoCalibrator *aileron;
AirfoilServoCalibrator *elevator;
AirfoilServoCalibrator *rudder;
SwitchCalibrator *gyro;
CurveCalibrator *pitch;
CurveCalibrator *throttle;
int uasId;
QPointer<RadioCalibrationData> radio;
QSignalMapper mapper;
void parseSetpoint(const QDomElement& setpoint, const QPointer<RadioCalibrationData>& radio);
};
#endif // RADIOCALIBRATIONWINDOW_H
#include "SwitchCalibrator.h"
SwitchCalibrator::SwitchCalibrator(QString titleString, QWidget *parent) :
AbstractCalibrator(parent),
defaultPulseWidth(new QLabel()),
toggledPulseWidth(new QLabel())
{
/* Add title label*/
QLabel *title = new QLabel(titleString);
QGridLayout *grid = new QGridLayout();
grid->addWidget(title, 0, 0, 1, 3);
/* Add current Pulse Width Display */
QLabel *pulseWidthTitle = new QLabel(tr("Pulse Width (us)"));
QHBoxLayout *pulseLayout = new QHBoxLayout();
pulseLayout->addWidget(pulseWidthTitle);
pulseLayout->addWidget(pulseWidth);
grid->addLayout(pulseLayout, 1, 0, 1, 3);
QLabel *defaultPulseString = new QLabel(tr("Default Position"));
QPushButton *defaultButton = new QPushButton(tr("Set"));
grid->addWidget(defaultPulseString, 2, 0);
grid->addWidget(defaultPulseWidth, 2, 1);
grid->addWidget(defaultButton, 2, 2);
QLabel *toggledPulseString = new QLabel(tr("Toggled Position"));
QPushButton *toggledButton = new QPushButton(tr("Set"));
grid->addWidget(toggledPulseString, 3, 0);
grid->addWidget(toggledPulseWidth, 3, 1);
grid->addWidget(toggledButton, 3, 2);
this->setLayout(grid);
connect(defaultButton, SIGNAL(clicked()), this, SLOT(setDefault()));
connect(toggledButton, SIGNAL(clicked()), this, SLOT(setToggled()));
}
void SwitchCalibrator::setDefault()
{
defaultPulseWidth->setText(QString::number(logExtrema()));
emit setpointChanged(0, logExtrema());
}
void SwitchCalibrator::setToggled()
{
toggledPulseWidth->setText(QString::number(logExtrema()));
emit setpointChanged(1, logExtrema());
}
void SwitchCalibrator::set(const QVector<uint16_t> &data)
{
if (data.size() == 2) {
defaultPulseWidth->setText(QString::number(data[0]));
toggledPulseWidth->setText(QString::number(data[1]));
}
}
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Calibration widget for 2 setpoint switch
* @author Bryan Godbolt <godbolt@ualberta.ca>
*/
#ifndef SWITCHCALIBRATOR_H
#define SWITCHCALIBRATOR_H
#include <QWidget>
#include <QLabel>
#include <QPushButton>
#include <QVector>
#include <QGridLayout>
#include <QHBoxLayout>
#include "AbstractCalibrator.h"
/**
@brief Calibration widget for 2 setpoint switch
@author Bryan Godbolt <godbolt@ece.ualberta.ca>
*/
class SwitchCalibrator : public AbstractCalibrator
{
Q_OBJECT
public:
explicit SwitchCalibrator(QString title=QString(), QWidget *parent = 0);
void set(const QVector<uint16_t> &data);
protected slots:
void setDefault();
void setToggled();
protected:
QLabel *defaultPulseWidth;
QLabel *toggledPulseWidth;
};
#endif // SWITCHCALIBRATOR_H
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>UASControlParameters</class>
<widget class="QWidget" name="UASControlParameters">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>204</width>
<height>246</height>
</rect>
</property>
<property name="minimumSize">
<size>
<width>200</width>
<height>150</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>267</width>
<height>16777215</height>
</size>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="0" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_5">
<item>
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QLabel" name="lbMode">
<property name="minimumSize">
<size>
<width>100</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>----</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item row="1" column="0">
<widget class="QTabWidget" name="tabWidget">
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="currentIndex">
<number>0</number>
</property>
<widget class="QWidget" name="tab">
<attribute name="title">
<string>Commands</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_2">
<item row="1" column="0">
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QLabel" name="label_2">
<property name="text">
<string>Height (m)</string>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="sbHeight">
<property name="maximum">
<double>1500.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item row="2" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
<widget class="QLabel" name="label_3">
<property name="text">
<string>Airspeed (m/s)</string>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="sbAirSpeed">
<property name="maximum">
<double>500.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item row="3" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_2">
<item>
<widget class="QLabel" name="label_4">
<property name="text">
<string>Turn Rate (rad/s)</string>
</property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="sbTurnRate">
<property name="maximum">
<double>180.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item row="4" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
<widget class="QPushButton" name="btSetCommands">
<property name="text">
<string>Set</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="btGetCommands">
<property name="text">
<string>Get</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="5" column="0">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>27</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="tab_2">
<attribute name="title">
<string>Passthrough</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_3">
<item row="0" column="0">
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QCheckBox" name="cxdle_c">
<property name="text">
<string>Elevator</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="cxdr_c">
<property name="text">
<string>Rudder</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="cxdt_c">
<property name="text">
<string>Throttle</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="cxdla_c">
<property name="text">
<string>Ailerons</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="1" column="0">
<widget class="QPushButton" name="btSetCtrl">
<property name="text">
<string>Set</string>
</property>
</widget>
</item>
<item row="2" column="0">
<spacer name="verticalSpacer_2">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>26</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>
......@@ -3,6 +3,7 @@
#include <QSettings>
#include <QTimer>
#include <QToolTip>
#include <QDebug>
#include "QGCComboBox.h"
#include "ui_QGCComboBox.h"
......
#include <QDockWidget>
#include <QDebug>
#include "QGCCommandButton.h"
#include "ui_QGCCommandButton.h"
......
......@@ -3,6 +3,7 @@
#include <QSettings>
#include <QTimer>
#include <QToolTip>
#include <QDebug>
#include "QGCParamSlider.h"
#include "ui_QGCParamSlider.h"
......
......@@ -3,6 +3,7 @@
#include <qwt_plot.h>
#include <qwt_plot_grid.h>
#include <qwt_plot_curve.h>
#include "MainWindow.h"
#include "ScrollZoomer.h"
......
#include "UASActionsWidget.h"
#include <QMessageBox>
#include <UAS.h>
UASActionsWidget::UASActionsWidget(QWidget *parent) : QWidget(parent)
{
m_uas = 0;
ui.setupUi(this);
connect(ui.changeAltitudeButton,SIGNAL(clicked()),this,SLOT(changeAltitudeClicked()));
connect(ui.changeSpeedButton,SIGNAL(clicked()),this,SLOT(changeSpeedClicked()));
connect(ui.goToWaypointButton,SIGNAL(clicked()),this,SLOT(goToWaypointClicked()));
connect(ui.armDisarmButton,SIGNAL(clicked()),this,SLOT(armButtonClicked()));
connect(UASManager::instance(),SIGNAL(activeUASSet(UASInterface*)),this,SLOT(activeUASSet(UASInterface*)));
if (UASManager::instance()->getActiveUAS())
{
activeUASSet(UASManager::instance()->getActiveUAS());
}
}
void UASActionsWidget::activeUASSet(UASInterface *uas)
{
m_uas = uas;
if (uas)
{
connect(m_uas->getWaypointManager(),SIGNAL(waypointEditableListChanged()),this,SLOT(updateWaypointList()));
connect(m_uas->getWaypointManager(),SIGNAL(currentWaypointChanged(quint16)),this,SLOT(currentWaypointChanged(quint16)));
connect(m_uas,SIGNAL(armingChanged(bool)),this,SLOT(armingChanged(bool)));
armingChanged(m_uas->isArmed());
}
updateWaypointList();
}
void UASActionsWidget::armButtonClicked()
{
if (m_uas)
{
if (m_uas->isArmed())
{
((UAS*)m_uas)->disarmSystem();
}
else
{
((UAS*)m_uas)->armSystem();
}
}
}
void UASActionsWidget::armingChanged(bool state)
{
//TODO:
//Figure out why arm/disarm is in UAS.h and not part of the interface, and fix.
if (state)
{
ui.armDisarmButton->setText("DISARM\nCurrently Armed");
}
else
{
ui.armDisarmButton->setText("ARM\nCurrently Disarmed");
}
}
void UASActionsWidget::currentWaypointChanged(quint16 wpid)
{
ui.currentWaypointLabel->setText("Current: " + QString::number(wpid));
}
void UASActionsWidget::updateWaypointList()
{
ui.waypointComboBox->clear();
if (m_uas)
{
for (int i=0;i<m_uas->getWaypointManager()->getWaypointEditableList().size();i++)
{
ui.waypointComboBox->addItem(QString::number(i));
}
}
}
UASActionsWidget::~UASActionsWidget()
{
}
void UASActionsWidget::goToWaypointClicked()
{
if (!m_uas)
{
return;
}
m_uas->getWaypointManager()->setCurrentWaypoint(ui.waypointComboBox->currentIndex());
}
void UASActionsWidget::changeAltitudeClicked()
{
QMessageBox::information(0,"Error","No implemented yet.");
}
void UASActionsWidget::changeSpeedClicked()
{
if (!m_uas)
{
return;
}
if (m_uas->getSystemType() == MAV_TYPE_QUADROTOR)
{
m_uas->setParameter(1,"WP_SPEED_MAX",QVariant(((float)ui.altitudeSpinBox->value() * 100)));
return;
}
else if (m_uas->getSystemType() == MAV_TYPE_FIXED_WING)
{
QVariant variant;
if (m_uas->getParamManager()->getParameterValue(1,"ARSPD_ENABLE",variant))
{
if (variant.toInt() == 1)
{
m_uas->setParameter(1,"TRIM_ARSPD_CN",QVariant(((float)ui.altitudeSpinBox->value() * 100)));
return;
}
}
m_uas->setParameter(1,"TRIM_ARSPD_CN",QVariant(((float)ui.altitudeSpinBox->value() * 100)));
}
}
#ifndef UASACTIONSWIDGET_H
#define UASACTIONSWIDGET_H
#include <QWidget>
#include "ui_UASActionsWidget.h"
#include <UASManager.h>
#include <UASInterface.h>
class UASActionsWidget : public QWidget
{
Q_OBJECT
public:
explicit UASActionsWidget(QWidget *parent = 0);
~UASActionsWidget();
private:
Ui::UASActionsWidget ui;
UASInterface *m_uas;
private slots:
void armButtonClicked();
void armingChanged(bool state);
void currentWaypointChanged(quint16 wpid);
void updateWaypointList();
void activeUASSet(UASInterface *uas);
void goToWaypointClicked();
void changeAltitudeClicked();
void changeSpeedClicked();
};
#endif // UASACTIONSWIDGET_H
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>UASActionsWidget</class>
<widget class="QWidget" name="UASActionsWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>321</width>
<height>363</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="margin">
<number>6</number>
</property>
<item>
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>Mission Controls</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<property name="margin">
<number>6</number>
</property>
<property name="spacing">
<number>6</number>
</property>
<item row="0" column="0">
<widget class="QComboBox" name="waypointComboBox"/>
</item>
<item row="0" column="1">
<widget class="QLabel" name="currentWaypointLabel">
<property name="text">
<string>Current:</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QPushButton" name="goToWaypointButton">
<property name="text">
<string>Go To Waypoint</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QPushButton" name="pushButton_6">
<property name="text">
<string>Restart Mission</string>
</property>
</widget>
</item>
<item row="2" column="0" colspan="2">
<layout class="QHBoxLayout" name="horizontalLayout_2">
<item>
<widget class="QSpinBox" name="altitudeSpinBox"/>
</item>
<item>
<widget class="QPushButton" name="changeAltitudeButton">
<property name="text">
<string>Change Altitude</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="3" column="0" colspan="2">
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QSpinBox" name="speedSpinBox"/>
</item>
<item>
<widget class="QPushButton" name="changeSpeedButton">
<property name="text">
<string>Change Speed</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_2">
<property name="title">
<string>Auto Actions</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
<property name="margin">
<number>6</number>
</property>
<property name="spacing">
<number>6</number>
</property>
<item row="0" column="0" colspan="2">
<widget class="QComboBox" name="comboBox"/>
</item>
<item row="0" column="2" colspan="2">
<widget class="QComboBox" name="comboBox_3"/>
</item>
<item row="1" column="0" colspan="2">
<widget class="QPushButton" name="pushButton_8">
<property name="text">
<string>Execute Action</string>
</property>
</widget>
</item>
<item row="1" column="2" colspan="2">
<widget class="QPushButton" name="pushButton_5">
<property name="text">
<string>Set Mode</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QPushButton" name="pushButton">
<property name="text">
<string>Auto</string>
</property>
</widget>
</item>
<item row="2" column="1" colspan="2">
<widget class="QPushButton" name="pushButton_2">
<property name="text">
<string>Manual</string>
</property>
</widget>
</item>
<item row="2" column="3">
<widget class="QPushButton" name="pushButton_4">
<property name="text">
<string>RTL</string>
</property>
</widget>
</item>
<item row="3" column="0" colspan="4">
<widget class="QPushButton" name="armDisarmButton">
<property name="text">
<string>ARM</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>
#include "UASControlParameters.h"
#include "ui_UASControlParameters.h"
#define CONTROL_MODE_LOCKED "MODE LOCKED"
#define CONTROL_MODE_MANUAL "MODE MANUAL"
#define CONTROL_MODE_READY "MODE TEST3"
#define CONTROL_MODE_RC_TRAINING "RC SIMULATION"
#define CONTROL_MODE_LOCKED_INDEX 1
#define CONTROL_MODE_MANUAL_INDEX 2
#define CONTROL_MODE_GUIDED_INDEX 3
#define CONTROL_MODE_AUTO_INDEX 4
#define CONTROL_MODE_TEST1_INDEX 5
#define CONTROL_MODE_TEST2_INDEX 6
#define CONTROL_MODE_TEST3_INDEX 7
#define CONTROL_MODE_READY_INDEX 8
#define CONTROL_MODE_RC_TRAINING_INDEX 9
UASControlParameters::UASControlParameters(QWidget *parent) :
QWidget(parent),
ui(new Ui::UASControlParameters)
{
ui->setupUi(this);
ui->btSetCtrl->setStatusTip(tr("Set Passthrough"));
connect(ui->btGetCommands, SIGNAL(clicked()), this, SLOT(getCommands()));
connect(ui->btSetCtrl, SIGNAL(clicked()), this, SLOT(setPassthrough()));
}
UASControlParameters::~UASControlParameters()
{
delete ui;
}
void UASControlParameters::changedMode(int mode)
{
QString modeTemp;
switch (mode) {
case (uint8_t)MAV_MODE_PREFLIGHT:
modeTemp = "LOCKED MODE";
break;
case (uint8_t)MAV_MODE_MANUAL_ARMED:
modeTemp = "A/MANUAL MODE";
break;
case (uint8_t)MAV_MODE_MANUAL_DISARMED:
modeTemp = "D/MANUAL MODE";
break;
default:
modeTemp = "UNKNOWN MODE";
break;
}
if(modeTemp != this->mode) {
ui->lbMode->setStyleSheet("background-color: rgb(165, 42, 42)");
} else {
ui->lbMode->setStyleSheet("background-color: rgb(85, 107, 47)");
}
}
void UASControlParameters::activeUasSet(UASInterface *uas)
{
if(uas) {
connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,double,quint64)));
connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(speedChanged(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
connect(uas, SIGNAL(thrustChanged(UASInterface*,double)), this, SLOT(thrustChanged(UASInterface*,double)) );
activeUAS= uas;
}
}
void UASControlParameters::updateGlobalPosition(UASInterface * a, double b, double c, double aa, quint64 ab)
{
Q_UNUSED(a);
Q_UNUSED(b);
Q_UNUSED(c);
Q_UNUSED(ab);
this->altitude=aa;
}
void UASControlParameters::speedChanged(UASInterface* uas, double vx, double vy, double vz, quint64 time)
{
Q_UNUSED(time);
Q_UNUSED(uas);
this->speed = sqrt(pow(vx, 2.0) + pow(vy, 2.0) + pow(vz, 2.0));
//ui->sbAirSpeed->setValue(speed);
}
void UASControlParameters::updateAttitude(UASInterface *uas, double roll, double pitch, double yaw, quint64 time)
{
Q_UNUSED(uas);
Q_UNUSED(pitch);
Q_UNUSED(yaw);
Q_UNUSED(time);
//ui->sbTurnRate->setValue(roll);
this->roll = roll;
}
void UASControlParameters::setCommands()
{
}
void UASControlParameters::getCommands()
{
ui->sbAirSpeed->setValue(this->speed);
ui->sbHeight->setValue(this->altitude);
ui->sbTurnRate->setValue(this->roll);
}
void UASControlParameters::setPassthrough()
{
}
void UASControlParameters::updateMode(int uas,QString mode,QString description)
{
Q_UNUSED(uas);
Q_UNUSED(description);
this->mode = mode;
ui->lbMode->setText(this->mode);
ui->lbMode->setStyleSheet("background-color: rgb(85, 107, 47)");
}
void UASControlParameters::thrustChanged(UASInterface *uas, double throttle)
{
Q_UNUSED(uas);
this->throttle= throttle;
}
#ifndef UASCONTROLPARAMETERS_H
#define UASCONTROLPARAMETERS_H
#include <QWidget>
#include "UASManager.h"
#include <QTimer>
#include <QTabWidget>
namespace Ui
{
class UASControlParameters;
}
class UASControlParameters : public QWidget
{
Q_OBJECT
public:
explicit UASControlParameters(QWidget *parent = 0);
~UASControlParameters();
public slots:
void changedMode(int mode);
void activeUasSet(UASInterface* uas);
void updateGlobalPosition(UASInterface*,double,double,double,quint64);
void speedChanged(UASInterface*,double,double,double,quint64);
void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time);
void setCommands();
void getCommands();
void setPassthrough();
void updateMode(int uas,QString mode,QString description);
void thrustChanged(UASInterface* uas,double throttle);
private:
Ui::UASControlParameters *ui;
QTimer* refreshTimerGet;
UASInterface* activeUAS;
double speed;
double roll;
double altitude;
double throttle;
QString mode;
QString REDcolorStyle;
QPointer<RadioCalibrationData> radio;
LinkInterface* hilLink;
};
#endif // UASCONTROLPARAMETERS_H
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