diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index fa0aafdb7df81763d4ee5b5a7a25dfebd236f86c..6fe6281bb6d89817fb278e5009e4884d416f877c 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -219,6 +219,12 @@ HEADERS += src/MG.h \
src/ui/map/MAV2DIcon.h \
src/ui/QGCRemoteControlView.h \
src/ui/WaypointGlobalView.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/ui/map3D/Q3DWidget.h \
src/ui/map3D/CheetahModel.h \
src/ui/map3D/CheetahGL.h \
@@ -292,6 +298,12 @@ SOURCES += src/main.cc \
src/ui/map/Waypoint2DIcon.cc \
src/ui/map/MAV2DIcon.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/WaypointGlobalView.cc \
src/ui/map3D/Q3DWidget.cc \
src/ui/map3D/CheetahModel.cc \
@@ -305,7 +317,7 @@ SOURCES += src/main.cc \
RESOURCES = mavground.qrc
# Include RT-LAB Library
-win32:exists(src/lib/opalrt/OpalApi.h) {
+win32:exists(src/lib/opalrt/OpalApi.h):exists(C:\OPAL-RT\RT-LAB7.2.4\Common\bin) {
message("Building support for Opal-RT")
LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin \
-lOpalApi
diff --git a/settings/ParameterList.xml b/settings/ParameterList.xml
index 3894080f4c43a26c423c7b3cce235ec4cb77a275..b1cb8f47e903a5555a305b69ee54ab5832cb31b9 100644
--- a/settings/ParameterList.xml
+++ b/settings/ParameterList.xml
@@ -2,9 +2,9 @@
-
+
-
+
@@ -23,6 +23,117 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/settings/RadioCalibration.xml b/settings/RadioCalibration.xml
new file mode 100644
index 0000000000000000000000000000000000000000..e200b0219f0d0370adf63454165bfae57513da1f
--- /dev/null
+++ b/settings/RadioCalibration.xml
@@ -0,0 +1,9 @@
+
+
+ 1000.0, 1500.0, 2000.0
+ 1000.0, 1500.0, 2000.0
+ 1000.0, 1500.0, 2000.0
+ 2000.0, 1000.0
+ 1000.0, 1250.0, 1500.0, 1750.0, 2000.0
+ 1000.0, 1250.0, 1500.0, 1750.0, 2000.0
+
diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc
index 5551c091c3ea35ab989543b4b61bae462f4fdb10..1c29ef43cd839676afc434f8d1525f01e51cc4c7 100644
--- a/src/comm/OpalLink.cc
+++ b/src/comm/OpalLink.cc
@@ -40,7 +40,8 @@ OpalLink::OpalLink() :
systemID(1),
componentID(1),
params(NULL),
- opalInstID(101)
+ opalInstID(101),
+ sendRCValues(false)
{
start(QThread::LowPriority);
@@ -133,6 +134,77 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
}
}
break;
+ case MAVLINK_MSG_ID_REQUEST_RC_CHANNELS:
+ {
+ mavlink_request_rc_channels_t rc;
+ mavlink_msg_request_rc_channels_decode(&msg, &rc);
+ this->sendRCValues = static_cast(rc.enabled);
+ }
+ break;
+#ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES
+ case MAVLINK_MSG_ID_RADIO_CALIBRATION:
+ {
+ mavlink_radio_calibration_t radio;
+ mavlink_msg_radio_calibration_decode(&msg, &radio);
+// qDebug() << "RADIO CALIBRATION RECEIVED";
+// qDebug() << "AILERON: " << radio.aileron[0] << " " << radio.aileron[1] << " " << radio.aileron[2];
+// qDebug() << "ELEVATOR: " << radio.elevator[0] << " " << radio.elevator[1] << " " << radio.elevator[2];
+// qDebug() << "RUDDER: " << radio.rudder[0] << " " << radio.rudder[1] << " " << radio.rudder[2];
+// qDebug() << "GYRO: " << radio.gyro[0] << " " << radio.gyro[1];
+// qDebug() << "PITCH: " << radio.pitch[0] << radio.pitch[1] << radio.pitch[2] << radio.pitch[3] << radio.pitch[4];
+// qDebug() << "THROTTLE: " << radio.throttle[0] << radio.throttle[1] << radio.throttle[2] << radio.throttle[3] << radio.throttle[4];
+
+ /* AILERON SERVO */
+ if (params->contains(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN").setValue(((radio.aileron[0]>900 /*in us?*/)?radio.aileron[0]/1000:radio.aileron[0]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN").setValue(((radio.aileron[1]>900 /*in us?*/)?radio.aileron[1]/1000:radio.aileron[1]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN").setValue(((radio.aileron[2]>900 /*in us?*/)?radio.aileron[2]/1000:radio.aileron[2]));
+ /* ELEVATOR SERVO */
+ if (params->contains(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN").setValue(((radio.elevator[0]>900 /*in us?*/)?radio.elevator[0]/1000:radio.elevator[0]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN").setValue(((radio.elevator[1]>900 /*in us?*/)?radio.elevator[1]/1000:radio.elevator[1]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "ELE_UP_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "ELE_UP_IN").setValue(((radio.elevator[2]>900 /*in us?*/)?radio.elevator[2]/1000:radio.elevator[2]));
+ /* THROTTLE SERVO */
+ if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET0_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET0_IN").setValue(((radio.throttle[0]>900 /*in us?*/)?radio.throttle[0]/1000:radio.throttle[0]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET1_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET1_IN").setValue(((radio.throttle[1]>900 /*in us?*/)?radio.throttle[1]/1000:radio.throttle[1]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET2_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET2_IN").setValue(((radio.throttle[2]>900 /*in us?*/)?radio.throttle[2]/1000:radio.throttle[2]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET3_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET3_IN").setValue(((radio.throttle[3]>900 /*in us?*/)?radio.throttle[3]/1000:radio.throttle[3]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET4_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET4_IN").setValue(((radio.throttle[4]>900 /*in us?*/)?radio.throttle[4]/1000:radio.throttle[4]));
+ /* RUDDER SERVO */
+ if (params->contains(OpalRT::SERVO_INPUTS, "RUD_LEFT_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "RUD_LEFT_IN").setValue(((radio.rudder[0]>900 /*in us?*/)?radio.rudder[0]/1000:radio.rudder[0]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "RUD_CENTER_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "RUD_CENTER_IN").setValue(((radio.rudder[1]>900 /*in us?*/)?radio.rudder[1]/1000:radio.rudder[1]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "RUD_RIGHT_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "RUD_RIGHT_IN").setValue(((radio.rudder[2]>900 /*in us?*/)?radio.rudder[2]/1000:radio.rudder[2]));
+ /* GYRO MODE/GAIN SWITCH */
+ if (params->contains(OpalRT::SERVO_INPUTS, "GYRO_DEF_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "GYRO_DEF_IN").setValue(((radio.gyro[0]>900 /*in us?*/)?radio.gyro[0]/1000:radio.gyro[0]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "GYRO_TOG_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "GYRO_TOG_IN").setValue(((radio.gyro[1]>900 /*in us?*/)?radio.gyro[1]/1000:radio.gyro[1]));
+ /* PITCH SERVO */
+ if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET0_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET0_IN").setValue(((radio.pitch[0]>900 /*in us?*/)?radio.pitch[0]/1000:radio.pitch[0]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET1_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET1_IN").setValue(((radio.pitch[1]>900 /*in us?*/)?radio.pitch[1]/1000:radio.pitch[1]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET2_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET2_IN").setValue(((radio.pitch[2]>900 /*in us?*/)?radio.pitch[2]/1000:radio.pitch[2]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET3_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET3_IN").setValue(((radio.pitch[3]>900 /*in us?*/)?radio.pitch[3]/1000:radio.pitch[3]));
+ if (params->contains(OpalRT::SERVO_INPUTS, "PIT_SET4_IN"))
+ params->getParameter(OpalRT::SERVO_INPUTS, "PIT_SET4_IN").setValue(((radio.pitch[4]>900 /*in us?*/)?radio.pitch[4]/1000:radio.pitch[4]));
+ }
+ break;
+#endif
default:
{
qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet";
@@ -248,8 +320,10 @@ void OpalLink::getSignals()
receiveMessage(bias);
/* send radio outputs */
- mavlink_message_t rc;
- mavlink_msg_rc_channels_pack(systemID, componentID, &rc,
+ if (sendRCValues)
+ {
+ mavlink_message_t rc;
+ mavlink_msg_rc_channels_pack(systemID, componentID, &rc,
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_1]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_2]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_3]),
@@ -258,17 +332,18 @@ void OpalLink::getSignals()
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_6]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_7]),
duty2PulseMicros(values[OpalRT::RAW_CHANNEL_8]),
- static_cast(values[OpalRT::NORM_CHANNEL_1]*255),
- static_cast(values[OpalRT::NORM_CHANNEL_2]*255),
- static_cast(values[OpalRT::NORM_CHANNEL_3]*255),
- static_cast(values[OpalRT::NORM_CHANNEL_4]*255),
- static_cast(values[OpalRT::NORM_CHANNEL_5]*255),
- static_cast(values[OpalRT::NORM_CHANNEL_6]*255),
- static_cast(values[OpalRT::NORM_CHANNEL_7]*255),
- static_cast(values[OpalRT::NORM_CHANNEL_8]*255),
+ rescaleNorm(values[OpalRT::NORM_CHANNEL_1], OpalRT::NORM_CHANNEL_1),
+ rescaleNorm(values[OpalRT::NORM_CHANNEL_2], OpalRT::NORM_CHANNEL_2),
+ rescaleNorm(values[OpalRT::NORM_CHANNEL_3], OpalRT::NORM_CHANNEL_3),
+ rescaleNorm(values[OpalRT::NORM_CHANNEL_4], OpalRT::NORM_CHANNEL_4),
+ rescaleNorm(values[OpalRT::NORM_CHANNEL_5], OpalRT::NORM_CHANNEL_5),
+ rescaleNorm(values[OpalRT::NORM_CHANNEL_6], OpalRT::NORM_CHANNEL_6),
+ rescaleNorm(values[OpalRT::NORM_CHANNEL_7], OpalRT::NORM_CHANNEL_7),
+ rescaleNorm(values[OpalRT::NORM_CHANNEL_8], OpalRT::NORM_CHANNEL_8),
0 //rssi unused
);
- receiveMessage(rc);
+ receiveMessage(rc);
+ }
}
else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready
{
@@ -323,14 +398,33 @@ uint16_t OpalLink::duty2PulseMicros(double duty)
return static_cast(duty/70*1000000);
}
+uint8_t OpalLink::rescaleNorm(double norm, int ch)
+{
+ switch(ch)
+ {
+ case OpalRT::NORM_CHANNEL_1:
+ case OpalRT::NORM_CHANNEL_2:
+ case OpalRT::NORM_CHANNEL_4:
+ default:
+ // three setpoints
+ return static_cast((norm+1)/2*255);
+ break;
+ case OpalRT::NORM_CHANNEL_5:
+ //two setpoints
+ case OpalRT::NORM_CHANNEL_3:
+ case OpalRT::NORM_CHANNEL_6:
+ return static_cast(norm*255);
+ break;
+ }
+}
+
bool OpalLink::connect()
{
short modelState;
- /// \todo allow configuration of instid in window
if ((OpalConnect(opalInstID, false, &modelState) == EOK)
&& (OpalGetSignalControl(0, true) == EOK)
&& (OpalGetParameterControl(true) == EOK))
diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h
index 27fe685df06ced81474b18704ec2d7eead5c45c7..6a9d43d1ebaa79965986e8a4e81d354015aa7bd1 100644
--- a/src/comm/OpalLink.h
+++ b/src/comm/OpalLink.h
@@ -152,6 +152,9 @@ protected:
unsigned short opalInstID;
uint16_t duty2PulseMicros(double duty);
+ uint8_t rescaleNorm(double norm, int ch);
+
+ bool sendRCValues;
};
#endif // OPALLINK_H
diff --git a/src/comm/OpalRT.h b/src/comm/OpalRT.h
index 51a7f2e428928c56d6a082d47c3e8653cca5daf3..6e3d7074d85251ea0e970b15f862d6b4ee3b9c59 100644
--- a/src/comm/OpalRT.h
+++ b/src/comm/OpalRT.h
@@ -41,7 +41,7 @@ namespace OpalRT
Configuration info for the model
*/
- const unsigned short NUM_OUTPUT_SIGNALS=57;
+ const unsigned short NUM_OUTPUT_SIGNALS=42;
/* ------------------------------ Outputs ------------------------------
*
@@ -86,7 +86,7 @@ namespace OpalRT
B_W_0,
B_W_1,
B_W_2,
- RAW_CHANNEL_1 = 39,
+ RAW_CHANNEL_1 = 24,
RAW_CHANNEL_2,
RAW_CHANNEL_3,
RAW_CHANNEL_4,
diff --git a/src/comm/ParameterList.cc b/src/comm/ParameterList.cc
index 0b1d3762468c953925a21ca7c61fc16408534004..682489ee54464aaf4e47b0efc94d9f8bfabb7d4e 100644
--- a/src/comm/ParameterList.cc
+++ b/src/comm/ParameterList.cc
@@ -32,7 +32,8 @@ using namespace OpalRT;
ParameterList::ParameterList()
:params(new QMap >),
- paramList(new QList >())
+ paramList(new QList >()),
+ reqdServoParams(new QStringList())
{
QDir settingsDir = QDir(qApp->applicationDirPath());
@@ -40,40 +41,20 @@ ParameterList::ParameterList()
settingsDir.cdUp();
settingsDir.cd("settings");
+ // Enforce a list of parameters which are necessary for flight
+ reqdServoParams->append("AIL_RIGHT_IN");
+ reqdServoParams->append("AIL_CENTER_IN");
+ reqdServoParams->append("AIL_LEFT_IN");
+ reqdServoParams->append("ELE_DOWN_IN");
+ reqdServoParams->append("ELE_CENTER_IN");
+ reqdServoParams->append("ELE_UP_IN");
+ reqdServoParams->append("RUD_LEFT_IN");
+ reqdServoParams->append("RUD_CENTER_IN");
+ reqdServoParams->append("RUD_RIGHT_IN");
+
QString filename(settingsDir.path() + "/ParameterList.xml");
if ((QFile::exists(filename)) && open(filename))
{
- /* Populate the map with parameter names. There is no elegant way of doing this so all
- parameter paths and names must be known at compile time and defined here.
- Note: This function is written in a way that calls a lot of copy constructors and is
- therefore not particularly efficient. However since it is only called once memory
- and computation time are sacrificed for code clarity when adding and modifying
- parameters.
- When defining the path, the trailing slash is necessary
- */
- // Parameter *p;
- // /* Component: Navigation Filter */
- // p = new Parameter("avionics_src/sm_ampro/NAV_FILT_INIT/",
- // "Value",
- // OpalRT::NAV_ID,
- // QGCParamID("NAV_FILT_INIT"));
- // (*params)[OpalRT::NAV_ID].insert(p->getParamID(), *p);
- // delete p;
- //
- // p = new Parameter("avionics_src/sm_ampro/Gain/",
- // "Gain",
- // OpalRT::NAV_ID,
- // QGCParamID("TEST_OUTP_GAIN"));
- // (*params)[OpalRT::NAV_ID].insert(p->getParamID(), *p);
- // delete p;
- //
- // /* Component: Log Facility */
- // p = new Parameter("avionics_src/sm_ampro/LOG_FILE_ON/",
- // "Value",
- // OpalRT::LOG_ID,
- // QGCParamID("LOG_FILE_ON"));
- // (*params)[OpalRT::LOG_ID].insert(p->getParamID(), *p);
- // delete p;
/* Get a list of the available parameters from opal-rt */
QMap *opalParams = new QMap;
@@ -271,6 +252,8 @@ bool ParameterList::open(QString filename)
read(¶mFile);
+ paramFile.close();
+
return true;
}
@@ -336,11 +319,25 @@ void ParameterList::parseBlock(const QDomElement &block)
static_cast(id),
QGCParamID(e.attribute("QGCParamID")));
(*params)[id].insert(p->getParamID(), *p);
+ if (reqdServoParams->contains((QString)p->getParamID()))
+ reqdServoParams->removeAt(reqdServoParams->indexOf((QString)p->getParamID()));
+
delete p;
- }
+
+
+ }
else
{
- qDebug() << __FILE__ << ":" << __LINE__ << ": error in xml doc";
+ qDebug() << __FILE__ << ":" << __LINE__ << ": error in xml doc in block" << block.attribute("name");
+ }
+ }
+
+ if (!reqdServoParams->empty())
+ {
+ qDebug() << __FILE__ << __LINE__ << "Missing the following required servo parameters";
+ foreach(QString s, *reqdServoParams)
+ {
+ qDebug() << s;
}
}
diff --git a/src/comm/ParameterList.h b/src/comm/ParameterList.h
index f93ffac779a3a4b96f55408ab7bc655409812bae..5a891f20b11a179409e00ce6cbe84f5acdb71726 100644
--- a/src/comm/ParameterList.h
+++ b/src/comm/ParameterList.h
@@ -31,6 +31,7 @@ This file is part of the QGROUNDCONTROL project
#include
#include
#include
+#include
#include "mavlink_types.h"
#include "QGCParamID.h"
@@ -119,6 +120,10 @@ namespace OpalRT
are made through the map container.
*/
QList > *paramList;
+ /**
+ List of parameters which are necessary to control the servos.
+ */
+ QStringList *reqdServoParams;
/**
Get the list of available parameters from Opal-RT.
\param[out] opalParams Map of parameter paths/names to ids which are valid in Opal-RT
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 26ccc7d17e2293db805a7ee7ca94e38bfc190b06..570f39c967ca2673d396128cd7fc4a6b808ebb6f 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -570,6 +570,41 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "b_w[2]", bias.gyro_2, time);
}
break;
+ case MAVLINK_MSG_ID_RADIO_CALIBRATION:
+ {
+ mavlink_radio_calibration_t radioMsg;
+ mavlink_msg_radio_calibration_decode(&message, &radioMsg);
+ QVector aileron;
+ QVector elevator;
+ QVector rudder;
+ QVector gyro;
+ QVector pitch;
+ QVector throttle;
+
+ for (int i=0; i radioData = new RadioCalibrationData(aileron,
+ elevator,
+ rudder,
+ gyro,
+ pitch,
+ throttle);
+ emit radioCalibrationReceived(radioData);
+ delete radioData;
+ }
+ break;
+
#endif
default:
{
@@ -920,6 +955,10 @@ void UAS::enableRCChannelDataTransmission(bool enabled)
// Send message twice to increase chance of reception
sendMessage(msg);
sendMessage(msg);
+#elif defined(MAVLINK_ENABLED_UALBERTA_MESSAGES)
+ mavlink_message_t msg;
+ mavlink_msg_request_rc_channels_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, enabled);
+ sendMessage(msg);
#endif
}
@@ -1191,7 +1230,7 @@ void UAS::receiveButton(int buttonIndex)
break;
}
- qDebug() << __FILE__ << __LINE__ << ": Received button clicked signal (button # is: " << buttonIndex << "), UNIMPLEMENTED IN MAVLINK!";
+// qDebug() << __FILE__ << __LINE__ << ": Received button clicked signal (button # is: " << buttonIndex << "), UNIMPLEMENTED IN MAVLINK!";
}
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index 6a112aca1f6e1de1216c2f4afd12c63613be07f6..7141405745ff0628a3d6e99e9a9ee30968c4cee0 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -36,10 +36,12 @@ This file is part of the QGROUNDCONTROL project
#include
#include
#include
+#include
#include "LinkInterface.h"
#include "ProtocolInterface.h"
#include "UASWaypointManager.h"
+#include "RadioCalibration/RadioCalibrationData.h"
/**
* @brief Interface for all robots.
@@ -354,6 +356,8 @@ signals:
void remoteControlChannelChanged(int channelId, float raw, 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&);
/**
* @brief Localization quality changed
diff --git a/src/ui/QGCRemoteControlView.cc b/src/ui/QGCRemoteControlView.cc
index 78f93c441f9096a82ef5430adcfdd3fa172d958c..d0c574609e567ad8e5c382057f385be9139c048e 100644
--- a/src/ui/QGCRemoteControlView.cc
+++ b/src/ui/QGCRemoteControlView.cc
@@ -25,6 +25,7 @@ This file is part of the QGROUNDCONTROL project
* @file
* @brief Implementation of QGCRemoteControlView
* @author Lorenz Meier
+ * @author Bryan Godbolt
*/
#include
@@ -70,6 +71,14 @@ QGCRemoteControlView::QGCRemoteControlView(QWidget *parent) :
layout->addItem(rssiLayout, 2, 0, 1, 2);
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)));
}
@@ -89,6 +98,8 @@ void QGCRemoteControlView::setUASId(int id)
// The UAS exists, disconnect any existing connections
disconnect(uas, SIGNAL(remoteControlChannelChanged(int,float,float)), this, SLOT(setChannel(int,float,float)));
disconnect(uas, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float)));
+ disconnect(uas, SIGNAL(radioCalibrationReceived(const QPointer)), calibrationWindow, SLOT(receive(const QPointer&)));
+ disconnect(uas, SIGNAL(remoteControlChannelChanged(int,float,float)), calibrationWindow, SLOT(setChannel(int,float,float)));
}
}
@@ -98,7 +109,10 @@ void QGCRemoteControlView::setUASId(int id)
{
// New UAS exists, connect
nameLabel->setText(QString("RC Input of %1").arg(newUAS->getUASName()));
+ calibrationWindow->setUASId(id);
+ connect(newUAS, SIGNAL(radioCalibrationReceived(const QPointer&)), calibrationWindow, SLOT(receive(const QPointer&)));
connect(newUAS, SIGNAL(remoteControlChannelChanged(int,float,float)), this, SLOT(setChannel(int,float,float)));
+ connect(newUAS, SIGNAL(remoteControlChannelChanged(int,float,float)), calibrationWindow, SLOT(setChannel(int,float,float)));
connect(newUAS, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float)));
}
}
diff --git a/src/ui/QGCRemoteControlView.h b/src/ui/QGCRemoteControlView.h
index 85c191f9c35c77c614e8f25431fa40c5463ee089..c4044cdfd1b48257b2a2f16025ebb32f62ed44ef 100644
--- a/src/ui/QGCRemoteControlView.h
+++ b/src/ui/QGCRemoteControlView.h
@@ -32,6 +32,9 @@ This file is part of the QGROUNDCONTROL project
#include
#include
+#include
+
+#include "RadioCalibration/RadioCalibrationWindow.h"
namespace Ui {
class QGCRemoteControlView;
@@ -68,6 +71,8 @@ protected:
QVector progressBars;
QProgressBar* rssiBar;
QLabel* nameLabel;
+ QPushButton *calibrate;
+ RadioCalibrationWindow *calibrationWindow;
private:
Ui::QGCRemoteControlView *ui;
diff --git a/src/ui/RadioCalibration/AbstractCalibrator.cc b/src/ui/RadioCalibration/AbstractCalibrator.cc
new file mode 100644
index 0000000000000000000000000000000000000000..82df4a4c1074ae3cda5641a622898f1b62b86168
--- /dev/null
+++ b/src/ui/RadioCalibration/AbstractCalibrator.cc
@@ -0,0 +1,54 @@
+#include "AbstractCalibrator.h"
+
+AbstractCalibrator::AbstractCalibrator(QWidget *parent) :
+ QWidget(parent),
+ pulseWidth(new QLabel()),
+ log(new QVector())
+{
+}
+
+AbstractCalibrator::~AbstractCalibrator()
+{
+ delete log;
+}
+
+float AbstractCalibrator::logAverage()
+{
+ float total = 0;
+ for (int i=0; isize(); ++i)
+ total += log->value(i);
+ return floor(total/log->size());
+}
+
+float AbstractCalibrator::logExtrema()
+{
+ float extrema = logAverage();
+ if (logAverage() < 1500)
+ {
+ for (int i=0; isize(); ++i)
+ {
+ if (log->value(i) < extrema)
+ extrema = log->value(i);
+ }
+ extrema -= 5; // add 5us to prevent integer overflow
+ }
+ else
+ {
+ for (int i=0; isize(); ++i)
+ {
+ if (log->value(i) > extrema)
+ extrema = log->value(i);
+ }
+ extrema += 5; // subtact 5us to prevent integer overflow
+ }
+
+ return extrema;
+}
+
+void AbstractCalibrator::channelChanged(float raw)
+{
+ pulseWidth->setText(QString::number(static_cast(raw)));
+ if (log->size() == 5)
+ log->pop_front();
+ log->push_back(raw);
+}
diff --git a/src/ui/RadioCalibration/AbstractCalibrator.h b/src/ui/RadioCalibration/AbstractCalibrator.h
new file mode 100644
index 0000000000000000000000000000000000000000..bbed707b965f058ab62f5d65ef09559149765c82
--- /dev/null
+++ b/src/ui/RadioCalibration/AbstractCalibrator.h
@@ -0,0 +1,82 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT
+
+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 .
+
+======================================================================*/
+
+/**
+ * @file
+ * @brief Common aspects of radio calibration widgets
+ * @author Bryan Godbolt
+ */
+
+#ifndef ABSTRACTCALIBRATOR_H
+#define ABSTRACTCALIBRATOR_H
+
+#include
+#include
+#include
+#include
+
+#include
+
+/**
+ @brief Holds the code which is common to all the radio calibration widgets.
+
+ @author Bryan Godbolt
+ */
+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& 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, float value);
+
+public slots:
+ /** Slot to call when the relevant channel is updated
+ @param raw current channel value
+ */
+ void channelChanged(float raw);
+
+protected:
+ /** Display the current pulse width */
+ QLabel *pulseWidth;
+
+ /** Log of the past few samples for use in averaging and finding extrema */
+ QVector *log;
+ /** Find the maximum or minimum of the data log */
+ float logExtrema();
+ /** Find the average of the log */
+ float logAverage();
+};
+
+#endif // ABSTRACTCALIBRATOR_H
diff --git a/src/ui/RadioCalibration/AirfoilServoCalibrator.cc b/src/ui/RadioCalibration/AirfoilServoCalibrator.cc
new file mode 100644
index 0000000000000000000000000000000000000000..99f134cc804dcd40ecccee15d044da8bcb288a94
--- /dev/null
+++ b/src/ui/RadioCalibration/AirfoilServoCalibrator.cc
@@ -0,0 +1,117 @@
+#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"));
+ }
+
+ 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 Left"));
+ centerPulseString = new QLabel(tr("Center"));
+ lowPulseString = new QLabel(tr("Bank Right"));
+ }
+ else if (type == ELEVATOR)
+ {
+ highPulseString = new QLabel(tr("Nose Down"));
+ centerPulseString = new QLabel(tr("Center"));
+ lowPulseString = new QLabel(tr("Nose Up"));
+ }
+ else if (type == RUDDER)
+ {
+ highPulseString = new QLabel(tr("Nose Left"));
+ centerPulseString = new QLabel(tr("Center"));
+ lowPulseString = new QLabel(tr("Nose Right"));
+ }
+ 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(static_cast(logExtrema())));
+ emit setpointChanged(2, logExtrema());
+}
+
+void AirfoilServoCalibrator::setCenter()
+{
+ centerPulseWidth->setText(QString::number(static_cast(logAverage())));
+ emit setpointChanged(1, logAverage());
+}
+
+void AirfoilServoCalibrator::setLow()
+{
+ lowPulseWidth->setText(QString::number(static_cast(logExtrema())));
+ emit setpointChanged(0, logExtrema());
+}
+
+void AirfoilServoCalibrator::set(const QVector &data)
+{
+ if (data.size() == 3)
+ {
+ lowPulseWidth->setText(QString::number(data[0]));
+ centerPulseWidth->setText(QString::number(data[1]));
+ highPulseWidth->setText(QString::number(data[2]));
+ }
+}
diff --git a/src/ui/RadioCalibration/AirfoilServoCalibrator.h b/src/ui/RadioCalibration/AirfoilServoCalibrator.h
new file mode 100644
index 0000000000000000000000000000000000000000..caf442e484cf818320887429b6a0ae00b17f41a5
--- /dev/null
+++ b/src/ui/RadioCalibration/AirfoilServoCalibrator.h
@@ -0,0 +1,75 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT
+
+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 .
+
+======================================================================*/
+
+/**
+ * @file
+ * @brief Calibration widget for 3 point airfoil servo
+ * @author Bryan Godbolt
+ */
+
+#ifndef AIRFOILSERVOCALIBRATOR_H
+#define AIRFOILSERVOCALIBRATOR_H
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#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
+ */
+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& data);
+
+protected slots:
+ void setHigh();
+ void setCenter();
+ void setLow();
+
+protected:
+ QLabel *highPulseWidth;
+ QLabel *centerPulseWidth;
+ QLabel *lowPulseWidth;
+};
+
+#endif // AIRFOILSERVOCALIBRATOR_H
diff --git a/src/ui/RadioCalibration/CurveCalibrator.cc b/src/ui/RadioCalibration/CurveCalibrator.cc
new file mode 100644
index 0000000000000000000000000000000000000000..8cf261f9c32116fd2baf00cfe7ba5f0f4bcb47f9
--- /dev/null
+++ b/src/ui/RadioCalibration/CurveCalibrator.cc
@@ -0,0 +1,104 @@
+#include "CurveCalibrator.h"
+
+CurveCalibrator::CurveCalibrator(QString titleString, QWidget *parent) :
+ AbstractCalibrator(parent),
+ setpoints(new QVector(5)),
+ positions(new QVector())
+{
+ 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(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"))));
+ curve->setData(*positions, *setpoints);
+ 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, static_cast(logExtrema()));
+ }
+ else
+ {
+ setpoints->replace(setpoint, static_cast(logAverage()));
+ }
+ curve->setData(*positions, *setpoints);
+ plot->replot();
+
+ emit setpointChanged(setpoint, static_cast(setpoints->value(setpoint)));
+}
+
+void CurveCalibrator::set(const QVector &data)
+{
+ if (data.size() == 5)
+ {
+ for (int i=0; ireplace(i, static_cast(data[i]));
+ curve->setData(*positions, *setpoints);
+ plot->replot();
+ }
+ else
+ {
+ qDebug() << __FILE__ << __LINE__ << ": wrong data vector size";
+ }
+}
diff --git a/src/ui/RadioCalibration/CurveCalibrator.h b/src/ui/RadioCalibration/CurveCalibrator.h
new file mode 100644
index 0000000000000000000000000000000000000000..b90249ac0c31bb70bc45e06b5ff7885f587dded6
--- /dev/null
+++ b/src/ui/RadioCalibration/CurveCalibrator.h
@@ -0,0 +1,77 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT
+
+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 .
+
+======================================================================*/
+
+/**
+ * @file
+ * @brief Calibration widget for 5 point inerpolated curve
+ * @author Bryan Godbolt
+ */
+
+#ifndef CURVECALIBRATOR_H
+#define CURVECALIBRATOR_H
+
+#include
+#include
+#include
+#include
+//#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#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 &data);
+
+protected slots:
+ void setSetpoint(int setpoint);
+
+protected:
+ QVector *setpoints;
+ QVector *positions;
+ /** Plot to display calibration curve */
+ QwtPlot *plot;
+ /** Curve object of calibration curve */
+ QwtPlotCurve *curve;
+
+ QSignalMapper *signalMapper;
+};
+
+#endif // CURVECALIBRATOR_H
diff --git a/src/ui/RadioCalibration/RadioCalibrationData.cc b/src/ui/RadioCalibration/RadioCalibrationData.cc
new file mode 100644
index 0000000000000000000000000000000000000000..4d4822ea2dc77b065927eb16c3ab1b8b5dfdb872
--- /dev/null
+++ b/src/ui/RadioCalibration/RadioCalibrationData.cc
@@ -0,0 +1,70 @@
+#include "RadioCalibrationData.h"
+
+RadioCalibrationData::RadioCalibrationData()
+{
+ data = new QVector >(6);
+ (*data).insert(AILERON, QVector(3));
+ (*data).insert(ELEVATOR, QVector(3));
+ (*data).insert(RUDDER, QVector(3));
+ (*data).insert(GYRO, QVector(2));
+ (*data).insert(PITCH, QVector(5));
+ (*data).insert(THROTTLE, QVector(5));
+}
+
+RadioCalibrationData::RadioCalibrationData(const QVector &aileron,
+ const QVector &elevator,
+ const QVector &rudder,
+ const QVector &gyro,
+ const QVector &pitch,
+ const QVector &throttle)
+{
+ data = new QVector >();
+ (*data) << aileron
+ << elevator
+ << rudder
+ << gyro
+ << pitch
+ << throttle;
+}
+
+RadioCalibrationData::RadioCalibrationData(const RadioCalibrationData &other)
+ :QObject()
+{
+ data = new QVector >(*other.data);
+}
+
+RadioCalibrationData::~RadioCalibrationData()
+{
+ delete data;
+}
+
+const float* RadioCalibrationData::operator [](int i) const
+{
+ if (i < data->size())
+ {
+ return (*data)[i].constData();
+ }
+
+ return NULL;
+}
+
+const QVector& RadioCalibrationData::operator ()(int i) const
+{
+ if (i < data->size())
+ {
+ return (*data)[i];
+ }
+
+ // This is not good. If it is ever used after being returned it will cause a crash
+// return QVector();
+}
+
+QString RadioCalibrationData::toString(RadioElement element) const
+{
+ QString s;
+ foreach (float f, (*data)[element])
+ {
+ s += QString::number(f) + ", ";
+ }
+ return s.mid(0, s.length()-2);
+}
diff --git a/src/ui/RadioCalibration/RadioCalibrationData.h b/src/ui/RadioCalibration/RadioCalibrationData.h
new file mode 100644
index 0000000000000000000000000000000000000000..4eb5b071c3626281ec41bbeb4997a2d9222986f7
--- /dev/null
+++ b/src/ui/RadioCalibration/RadioCalibrationData.h
@@ -0,0 +1,96 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT
+
+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 .
+
+======================================================================*/
+
+/**
+ * @file
+ * @brief Class to hold the calibration data
+ * @author Bryan Godbolt
+ */
+
+#ifndef RADIOCALIBRATIONDATA_H
+#define RADIOCALIBRATIONDATA_H
+
+#include
+#include
+#include
+#include
+
+
+/**
+ @brief Class to hold the calibration data.
+ @author Bryan Godbolt
+ */
+class RadioCalibrationData : public QObject
+{
+Q_OBJECT
+
+public:
+ explicit RadioCalibrationData();
+ RadioCalibrationData(const RadioCalibrationData&);
+ RadioCalibrationData(const QVector& aileron,
+ const QVector& elevator,
+ const QVector& rudder,
+ const QVector& gyro,
+ const QVector& pitch,
+ const QVector& throttle);
+ ~RadioCalibrationData();
+
+ enum RadioElement
+ {
+ AILERON=0,
+ ELEVATOR,
+ RUDDER,
+ GYRO,
+ PITCH,
+ THROTTLE
+ };
+
+ const float* operator[](int i) const;
+ const QVector& operator()(int i) const;
+ void set(int element, int index, float value) {(*data)[element][index] = value;}
+
+public slots:
+ void setAileron(int index, float value) {set(AILERON, index, value);}
+ void setElevator(int index, float value) {set(ELEVATOR, index, value);}
+ void setRudder(int index, float value) {set(RUDDER, index, value);}
+ void setGyro(int index, float value) {set(GYRO, index, value);}
+ void setPitch(int index, float value) {set(PITCH, index, value);}
+ void setThrottle(int index, float 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 > *data;
+
+ void init(const QVector& aileron,
+ const QVector& elevator,
+ const QVector& rudder,
+ const QVector& gyro,
+ const QVector& pitch,
+ const QVector& throttle);
+
+};
+
+#endif // RADIOCALIBRATIONDATA_H
diff --git a/src/ui/RadioCalibration/RadioCalibrationWindow.cc b/src/ui/RadioCalibration/RadioCalibrationWindow.cc
new file mode 100644
index 0000000000000000000000000000000000000000..effb3b52ea7013457bf32c3974ab238169c85cc7
--- /dev/null
+++ b/src/ui/RadioCalibration/RadioCalibrationWindow.cc
@@ -0,0 +1,312 @@
+#include "RadioCalibrationWindow.h"
+
+RadioCalibrationWindow::RadioCalibrationWindow(QWidget *parent) :
+ QWidget(parent, Qt::Window),
+ radio(new RadioCalibrationData())
+{
+ QGridLayout *grid = new QGridLayout();
+
+ aileron = new AirfoilServoCalibrator(AirfoilServoCalibrator::AILERON);
+ grid->addWidget(aileron, 0, 0, 1, 1, Qt::AlignTop);
+
+ elevator = new AirfoilServoCalibrator(AirfoilServoCalibrator::ELEVATOR);
+ grid->addWidget(elevator, 0, 1, 1, 1, Qt::AlignTop);
+
+ rudder = new AirfoilServoCalibrator(AirfoilServoCalibrator::RUDDER);
+ grid->addWidget(rudder, 0, 2, 1, 1, Qt::AlignTop);
+
+ gyro = new SwitchCalibrator(tr("Gyro Mode/Gain"));
+ grid->addWidget(gyro, 0, 3, 1, 1, Qt::AlignTop);
+
+
+ pitch = new CurveCalibrator(tr("Collective Pitch"));
+ grid->addWidget(pitch, 1, 0, 1, 2);
+
+ throttle = new CurveCalibrator(tr("Throttle"));
+ grid->addWidget(throttle, 1, 2, 1, 2);
+
+ /* Buttons for loading/transmitting calibration data */
+ QHBoxLayout *hbox = new QHBoxLayout();
+ QPushButton *load = new QPushButton(tr("Load File"));
+ QPushButton *save = new QPushButton(tr("Save File"));
+ QPushButton *transmit = new QPushButton(tr("Transmit to UAV"));
+ QPushButton *get = new QPushButton(tr("Get from UAV"));
+ hbox->addWidget(load);
+ hbox->addWidget(save);
+ hbox->addWidget(transmit);
+ hbox->addWidget(get);
+ grid->addLayout(hbox, 2, 0, 1, 4);
+ this->setLayout(grid);
+
+ connect(load, SIGNAL(clicked()), this, SLOT(loadFile()));
+ connect(save, SIGNAL(clicked()), this, SLOT(saveFile()));
+ connect(transmit, SIGNAL(clicked()), this, SLOT(send()));
+ connect(get, SIGNAL(clicked()), this, SLOT(request()));
+
+ connect(aileron, SIGNAL(setpointChanged(int,float)), radio, SLOT(setAileron(int,float)));
+ connect(elevator, SIGNAL(setpointChanged(int,float)), radio, SLOT(setElevator(int,float)));
+ connect(rudder, SIGNAL(setpointChanged(int,float)), radio, SLOT(setRudder(int,float)));
+ connect(gyro, SIGNAL(setpointChanged(int,float)), radio, SLOT(setGyro(int,float)));
+ connect(pitch, SIGNAL(setpointChanged(int,float)), radio, SLOT(setPitch(int,float)));
+ connect(throttle, SIGNAL(setpointChanged(int,float)), radio, SLOT(setThrottle(int,float)));
+ setUASId(0);
+}
+
+void RadioCalibrationWindow::setChannel(int ch, float raw, float normalized)
+{
+ /** this expects a particular channel to function mapping
+ \todo allow run-time channel mapping
+ */
+ switch (ch)
+ {
+ case 0:
+ aileron->channelChanged(raw);
+ break;
+ case 1:
+ elevator->channelChanged(raw);
+ break;
+ case 2:
+ throttle->channelChanged(raw);
+ break;
+ case 3:
+ rudder->channelChanged(raw);
+ break;
+ case 4:
+ gyro->channelChanged(raw);
+ break;
+ case 5:
+ pitch->channelChanged(raw);
+ break;
+
+
+ }
+}
+
+void RadioCalibrationWindow::saveFile()
+{
+ QString fileName(QFileDialog::getSaveFileName(this,
+ tr("Save RC Calibration"),
+ "settings/",
+ tr("XML Files (*.xml)")));
+ if (fileName.isEmpty())
+ return;
+
+ QDomDocument *rcConfig = new QDomDocument();
+
+ QFile rcFile(fileName);
+ if (rcFile.exists())
+ {
+ rcFile.remove();
+ }
+ if (!rcFile.open(QFile::WriteOnly | QFile::Text))
+ {
+ qDebug() << __FILE__ << __LINE__ << "could not open" << rcFile.fileName() << "for writing";
+ return;
+ }
+
+ QDomElement root;
+ rcConfig->appendChild(root=rcConfig->createElement("channels"));
+ QDomElement e;
+ QDomText t;
+
+ // Aileron
+ e = rcConfig->createElement("threeSetpoint");
+ e.setAttribute("name", "Aileron");
+ e.setAttribute("number", "1");
+ t = rcConfig->createTextNode(radio->toString(RadioCalibrationData::AILERON));
+ e.appendChild(t);
+ root.appendChild(e);
+ // Elevator
+ e = rcConfig->createElement("threeSetpoint");
+ e.setAttribute("name", "Elevator");
+ e.setAttribute("number", "2");
+ t = rcConfig->createTextNode(radio->toString(RadioCalibrationData::ELEVATOR));
+ e.appendChild(t);
+ root.appendChild(e);
+ // Rudder
+ e = rcConfig->createElement("threeSetpoint");
+ e.setAttribute("name", "Rudder");
+ e.setAttribute("number", "4");
+ t = rcConfig->createTextNode(radio->toString(RadioCalibrationData::RUDDER));
+ e.appendChild(t);
+ root.appendChild(e);
+ // Gyro Mode/Gain
+ e = rcConfig->createElement("twoSetpoint");
+ e.setAttribute("name", "Gyro");
+ e.setAttribute("number", "5");
+ t = rcConfig->createTextNode(radio->toString(RadioCalibrationData::GYRO));
+ e.appendChild(t);
+ root.appendChild(e);
+ // Throttle
+ e = rcConfig->createElement("fiveSetpoint");
+ e.setAttribute("name", "Throttle");
+ e.setAttribute("number", "3");
+ t = rcConfig->createTextNode(radio->toString(RadioCalibrationData::THROTTLE));
+ e.appendChild(t);
+ root.appendChild(e);
+ // Pitch
+ e = rcConfig->createElement("fiveSetpoint");
+ e.setAttribute("name", "Pitch");
+ e.setAttribute("number", "6");
+ t = rcConfig->createTextNode(radio->toString(RadioCalibrationData::PITCH));
+ e.appendChild(t);
+ root.appendChild(e);
+
+
+ QTextStream out(&rcFile);
+ const int IndentSize = 4;
+ rcConfig->save(out, IndentSize);
+ rcFile.close();
+
+}
+
+void RadioCalibrationWindow::loadFile()
+{
+ QString fileName(QFileDialog::getOpenFileName(this,
+ tr("Load RC Calibration"),
+ "settings/",
+ tr("XML Files (*.xml)")));
+
+ if (fileName.isEmpty())
+ return;
+
+ QFile rcFile(fileName);
+ if (!rcFile.exists())
+ {
+ return;
+ }
+
+ if (!rcFile.open(QIODevice::ReadOnly))
+ {
+ return;
+ }
+
+ QDomDocument *rcConfig = new QDomDocument();
+
+ QString errorStr;
+ int errorLine;
+ int errorColumn;
+
+ if (!rcConfig->setContent(&rcFile, true, &errorStr, &errorLine,
+ &errorColumn))
+ {
+ qDebug() << "Error reading XML Parameter File on line: " << errorLine << errorStr;
+ return;
+ }
+
+ rcFile.close();
+ QDomElement root = rcConfig->documentElement();
+ if (root.tagName() != "channels") {
+ qDebug() << __FILE__ << __LINE__ << "This is not a Radio Calibration xml file";
+ return;
+ }
+
+
+ QPointer newRadio = new RadioCalibrationData();
+ QDomElement child = root.firstChildElement();
+ while (!child.isNull())
+ {
+ parseSetpoint(child, newRadio);
+ child = child.nextSiblingElement();
+ }
+
+ receive(newRadio);
+
+ delete newRadio;
+ delete rcConfig;
+}
+
+void RadioCalibrationWindow::parseSetpoint(const QDomElement &setpoint, const QPointer& newRadio)
+{
+ QVector setpoints;
+ QStringList setpointList = setpoint.text().split(",", QString::SkipEmptyParts);
+ foreach (QString setpoint, setpointList)
+ setpoints << setpoint.trimmed().toFloat();
+
+// qDebug() << __FILE__ << __LINE__ << ": " << setpoint.tagName() << ": " << setpoint.attribute("name") ;
+ if (setpoint.tagName() == "threeSetpoint")
+ {
+ if (setpoints.isEmpty())
+ setpoints << 0 << 0 << 0;
+ for (int i=0; i<3; ++i)
+ {
+ if (setpoint.attribute("name").toUpper() == "AILERON")
+ newRadio->setAileron(i, setpoints[i]);
+ else if(setpoint.attribute("name").toUpper() == "ELEVATOR")
+ newRadio->setElevator(i, setpoints[i]);
+ else if(setpoint.attribute("name").toUpper() == "RUDDER")
+ newRadio->setRudder(i, setpoints[i]);
+ }
+ }
+ else if (setpoint.tagName() == "twoSetpoint")
+ {
+ if (setpoints.isEmpty())
+ setpoints << 0 << 0;
+ for (int i=0; i<2; ++i)
+ {
+ if (setpoint.attribute("name").toUpper() == "GYRO")
+ newRadio->setGyro(i, setpoints[i]);
+ }
+ }
+ else if (setpoint.tagName() == "fiveSetpoint")
+ {
+ if (setpoints.isEmpty())
+ setpoints << 0 << 0 << 0 << 0 << 0;
+ for (int i=0; i<5; ++i)
+ {
+ if (setpoint.attribute("name").toUpper() == "PITCH")
+ newRadio->setPitch(i, setpoints[i]);
+ else if (setpoint.attribute("name").toUpper() == "THROTTLE")
+ newRadio->setThrottle(i, setpoints[i]);
+ }
+ }
+}
+
+void RadioCalibrationWindow::send()
+{
+ qDebug() << __FILE__ << __LINE__ << "uasId = " << uasId;
+#ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES
+ UAS *uas = dynamic_cast(UASManager::instance()->getUASForId(uasId));
+ if (uas)
+ {
+ mavlink_message_t msg;
+ mavlink_msg_radio_calibration_pack(uasId, 0, &msg,
+ (*radio)[RadioCalibrationData::AILERON],
+ (*radio)[RadioCalibrationData::ELEVATOR],
+ (*radio)[RadioCalibrationData::RUDDER],
+ (*radio)[RadioCalibrationData::GYRO],
+ (*radio)[RadioCalibrationData::PITCH],
+ (*radio)[RadioCalibrationData::THROTTLE]);
+ uas->sendMessage(msg);
+ }
+#endif
+}
+
+void RadioCalibrationWindow::request()
+{
+ qDebug() << __FILE__ << __LINE__ << "READ FROM UAV";
+ UAS *uas = dynamic_cast(UASManager::instance()->getUASForId(uasId));
+ if (uas)
+ {
+ mavlink_message_t msg;
+ mavlink_msg_action_pack(uasId, 0, &msg, 0, 0, ::MAV_ACTION_CALIBRATE_RC);
+ uas->sendMessage(msg);
+ }
+}
+
+void RadioCalibrationWindow::receive(const QPointer& radio)
+{
+ if (radio)
+ {
+ if (this->radio)
+ delete this->radio;
+ this->radio = new RadioCalibrationData(*radio);
+
+ aileron->set((*radio)(RadioCalibrationData::AILERON));
+ elevator->set((*radio)(RadioCalibrationData::ELEVATOR));
+ rudder->set((*radio)(RadioCalibrationData::RUDDER));
+ gyro->set((*radio)(RadioCalibrationData::GYRO));
+ pitch->set((*radio)(RadioCalibrationData::PITCH));
+ throttle->set((*radio)(RadioCalibrationData::THROTTLE));
+ }
+}
diff --git a/src/ui/RadioCalibration/RadioCalibrationWindow.h b/src/ui/RadioCalibration/RadioCalibrationWindow.h
new file mode 100644
index 0000000000000000000000000000000000000000..1ec038c754c062314ec22d29baf0fd228cab6df4
--- /dev/null
+++ b/src/ui/RadioCalibration/RadioCalibrationWindow.h
@@ -0,0 +1,92 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT
+
+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 .
+
+======================================================================*/
+
+/**
+ * @file
+ * @brief Main window for radio calibration
+ * @author Bryan Godbolt
+ */
+
+#ifndef RADIOCALIBRATIONWINDOW_H
+#define RADIOCALIBRATIONWINDOW_H
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "AirfoilServoCalibrator.h"
+#include "SwitchCalibrator.h"
+#include "CurveCalibrator.h"
+
+#include "mavlink.h"
+#include "mavlink_types.h"
+#include "UAS.h"
+#include "UASManager.h"
+#include "RadioCalibrationData.h"
+
+/**
+ @brief Main window for radio calibration
+ @author Bryan Godbolt
+ */
+class RadioCalibrationWindow : public QWidget
+{
+Q_OBJECT
+
+public:
+ explicit RadioCalibrationWindow(QWidget *parent = 0);
+
+public slots:
+ void setChannel(int ch, float raw, float normalized);
+ void loadFile();
+ void saveFile();
+ void send();
+ void request();
+ void receive(const QPointer& radio);
+ void setUASId(int id) {this->uasId = id;}
+
+
+protected:
+ AirfoilServoCalibrator *aileron;
+ AirfoilServoCalibrator *elevator;
+ AirfoilServoCalibrator *rudder;
+ SwitchCalibrator *gyro;
+ CurveCalibrator *pitch;
+ CurveCalibrator *throttle;
+ int uasId;
+ QPointer radio;
+ QSignalMapper mapper;
+
+ void parseSetpoint(const QDomElement& setpoint, const QPointer& radio);
+};
+
+#endif // RADIOCALIBRATIONWINDOW_H
diff --git a/src/ui/RadioCalibration/SwitchCalibrator.cc b/src/ui/RadioCalibration/SwitchCalibrator.cc
new file mode 100644
index 0000000000000000000000000000000000000000..61c5e583c9b20d782a7a11c7b6d42aaa3d6cd59b
--- /dev/null
+++ b/src/ui/RadioCalibration/SwitchCalibrator.cc
@@ -0,0 +1,58 @@
+#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(static_cast(logExtrema())));
+ emit setpointChanged(0, logExtrema());
+}
+
+void SwitchCalibrator::setToggled()
+{
+ toggledPulseWidth->setText(QString::number(static_cast(logExtrema())));
+ emit setpointChanged(1, logExtrema());
+}
+
+void SwitchCalibrator::set(const QVector &data)
+{
+ if (data.size() == 2)
+ {
+ defaultPulseWidth->setText(QString::number(data[0]));
+ toggledPulseWidth->setText(QString::number(data[1]));
+ }
+}
diff --git a/src/ui/RadioCalibration/SwitchCalibrator.h b/src/ui/RadioCalibration/SwitchCalibrator.h
new file mode 100644
index 0000000000000000000000000000000000000000..ae947004a18d3ac817e1a0a175190a2b0e377c18
--- /dev/null
+++ b/src/ui/RadioCalibration/SwitchCalibrator.h
@@ -0,0 +1,64 @@
+/*=====================================================================
+
+QGroundControl Open Source Ground Control Station
+
+(c) 2009, 2010 QGROUNDCONTROL PROJECT
+
+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 .
+
+======================================================================*/
+
+/**
+ * @file
+ * @brief Calibration widget for 2 setpoint switch
+ * @author Bryan Godbolt
+ */
+
+#ifndef SWITCHCALIBRATOR_H
+#define SWITCHCALIBRATOR_H
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "AbstractCalibrator.h"
+
+/**
+ @brief Calibration widget for 2 setpoint switch
+ @author Bryan Godbolt
+ */
+class SwitchCalibrator : public AbstractCalibrator
+{
+Q_OBJECT
+public:
+ explicit SwitchCalibrator(QString title=QString(), QWidget *parent = 0);
+
+ void set(const QVector &data);
+
+protected slots:
+ void setDefault();
+ void setToggled();
+
+protected:
+ QLabel *defaultPulseWidth;
+ QLabel *toggledPulseWidth;
+
+};
+
+#endif // SWITCHCALIBRATOR_H