diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 01348b2cba9438fea4fd2a26ece70fdd29e41fde..bcbcd69792443382a521b5f367d7735201370b6e 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -422,9 +422,9 @@ contains(DEPENDENCIES_PRESENT, libfreenect) {
RESOURCES += mavground.qrc
# Include RT-LAB Library
-win32:exists(src/lib/opalrt/OpalApi.h):exists(C:\OPAL-RT\RT-LAB7.2.4\Common\bin) {
+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 \
+ LIBS += -LC:/OPAL-RT/RT-LAB7.2.4/Common/bin \
-lOpalApi
INCLUDEPATH += src/lib/opalrt
HEADERS += src/comm/OpalRT.h \
diff --git a/settings/ParameterList.xml b/settings/ParameterList.xml
index b1cb8f47e903a5555a305b69ee54ab5832cb31b9..ac386300a96cc53944a7f956db8eed65a5897338 100644
--- a/settings/ParameterList.xml
+++ b/settings/ParameterList.xml
@@ -18,7 +18,48 @@
-
+
+
+
+
+
+
+
+
+
+
@@ -27,7 +68,7 @@
-
diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc
index fd4b7ba0aa5e2776129d8cb9d1645906a36ff35a..7a3cc924132296a384155ec384f83a8cbd1f38a5 100644
--- a/src/comm/OpalLink.cc
+++ b/src/comm/OpalLink.cc
@@ -41,7 +41,9 @@ OpalLink::OpalLink() :
componentID(1),
params(NULL),
opalInstID(101),
- sendRCValues(false)
+ sendRCValues(false),
+ sendRawController(false),
+ sendPosition(false)
{
start(QThread::LowPriority);
@@ -134,13 +136,13 @@ 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;
+// 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:
{
@@ -157,17 +159,29 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
/* 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_OUTPUTS, "AIL_RIGHT_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_RIGHT_OUT").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_OUTPUTS, "AIL_CENTER_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_CENTER_OUT").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]));
+ if (params->contains(OpalRT::SERVO_OUTPUTS, "AIL_LEFT_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_LEFT_OUT").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_OUTPUTS, "ELE_DOWN_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_DOWN_OUT").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_OUTPUTS, "ELE_CENTER_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_CENTER_OUT").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]));
+ if (params->contains(OpalRT::SERVO_OUTPUTS, "ELE_UP_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_UP_OUT").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]));
@@ -205,11 +219,50 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
}
break;
#endif
+#ifdef MAVLINK_ENABLED_PIXHAWK
+ case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+ {
+ mavlink_request_data_stream_t stream;
+ mavlink_msg_request_data_stream_decode(&msg, &stream);
+ switch (stream.req_stream_id)
+ {
+ case 0: // All data types
+ break;
+ case 1: // Raw Sensor Data
+ break;
+ case 2: // extended system status
+ break;
+ case 3: // rc channel data
+ sendRCValues = (stream.start_stop == 1?true:false);
+ break;
+ case 4: // raw controller
+ if (stream.start_stop == 1)
+ sendRawController = true;
+ else
+ sendRawController = false;
+ break;
+ case 5: // raw sensor fusion
+ break;
+ case 6: // position
+ sendPosition = (stream.start_stop == 1?true:false);
+ break;
+ case 7: // extra 1
+ break;
+ case 8: // extra 2
+ break;
+ case 9: // extra 3
+ break;
+ default:
+ qDebug() << __FILE__ << __LINE__ << "Received Unknown Data Strem Request with ID" << stream.req_stream_id;
+ }
+ }
+ break;
default:
{
qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet";
}
}
+#endif
}
}
@@ -282,17 +335,19 @@ void OpalLink::getSignals()
if (returnVal == EOK )
{
/* Send position info to qgroundcontrol */
- mavlink_message_t local_position;
- mavlink_msg_local_position_pack(systemID, componentID, &local_position,
- (*timestep)*1000000,
- values[OpalRT::X_POS],
- values[OpalRT::Y_POS],
- values[OpalRT::Z_POS],
- values[OpalRT::X_VEL],
- values[OpalRT::Y_VEL],
- values[OpalRT::Z_VEL]);
- receiveMessage(local_position);
-
+ if (sendPosition)
+ {
+ mavlink_message_t local_position;
+ mavlink_msg_local_position_pack(systemID, componentID, &local_position,
+ (*timestep)*1000000,
+ values[OpalRT::X_POS],
+ values[OpalRT::Y_POS],
+ values[OpalRT::Z_POS],
+ values[OpalRT::X_VEL],
+ values[OpalRT::Y_VEL],
+ values[OpalRT::Z_VEL]);
+ receiveMessage(local_position);
+ }
/* send attitude info to qgroundcontrol */
mavlink_message_t attitude;
mavlink_msg_attitude_pack(systemID, componentID, &attitude,
@@ -336,6 +391,18 @@ void OpalLink::getSignals()
);
receiveMessage(rc);
}
+ if (sendRawController)
+ {
+ mavlink_message_t rawController;
+ mavlink_msg_attitude_controller_output_pack(systemID, componentID, &rawController,
+ 1,
+ rescaleControllerOutput(values[OpalRT::CONTROLLER_AILERON]),
+ rescaleControllerOutput(values[OpalRT::CONTROLLER_ELEVATOR]),
+ 0, // yaw not used
+ 0 // thrust not used
+ );
+ receiveMessage(rawController);
+ }
}
else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready
{
@@ -408,10 +475,12 @@ uint8_t OpalLink::rescaleNorm(double norm, int ch)
return static_cast(norm*255);
break;
}
-
-
}
+int8_t OpalLink::rescaleControllerOutput(double raw)
+{
+ return static_cast((raw>=0?raw*127:raw*128));
+}
bool OpalLink::connect()
{
diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h
index 6a9d43d1ebaa79965986e8a4e81d354015aa7bd1..60dee3fbfaa9086ed206621d758a2b56b0b26d3e 100644
--- a/src/comm/OpalLink.h
+++ b/src/comm/OpalLink.h
@@ -153,8 +153,11 @@ protected:
uint16_t duty2PulseMicros(double duty);
uint8_t rescaleNorm(double norm, int ch);
+ int8_t rescaleControllerOutput(double raw);
bool sendRCValues;
+ bool sendRawController;
+ bool sendPosition;
};
#endif // OPALLINK_H
diff --git a/src/comm/OpalRT.h b/src/comm/OpalRT.h
index 6e3d7074d85251ea0e970b15f862d6b4ee3b9c59..70bc5b4795700724c4723858969ed6052d4ae056 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=42;
+ const unsigned short NUM_OUTPUT_SIGNALS=48;
/* ------------------------------ Outputs ------------------------------
*
@@ -103,7 +103,13 @@ namespace OpalRT
NORM_CHANNEL_7,
NORM_CHANNEL_8,
CONTROLLER_AILERON,
- CONTROLLER_ELEVATOR
+ CONTROLLER_ELEVATOR,
+ AIL_POUT,
+ AIL_IOUT,
+ AIL_DOUT,
+ ELE_POUT,
+ ELE_IOUT,
+ ELE_DOUT
};
/** Component IDs of the parameters. Currently they are all 1 becuase there is no advantage
@@ -112,9 +118,9 @@ namespace OpalRT
*/
enum SubsystemIds
{
- NAV_ID = 1,
- LOG_ID,
- CONTROLLER_ID,
+ NAV = 1,
+ LOG,
+ CONTROLLER,
SERVO_OUTPUTS,
SERVO_INPUTS
};
diff --git a/src/comm/Parameter.h b/src/comm/Parameter.h
index 695cd8a79d6a24b474f006db54337c1196d1e329..038444af404878e4b4b248112652f4503714f1a6 100644
--- a/src/comm/Parameter.h
+++ b/src/comm/Parameter.h
@@ -61,8 +61,8 @@ namespace OpalRT
const QGCParamID& getParamID() const {return *paramID;}
void setOpalID(unsigned short opalID) {this->opalID = opalID;}
- const QString& getSimulinkPath() {return *simulinkPath;}
- const QString& getSimulinkName() {return *simulinkName;}
+ const QString& getSimulinkPath() const {return *simulinkPath;}
+ const QString& getSimulinkName() const {return *simulinkName;}
uint8_t getComponentID() const {return componentID;}
float getValue();
void setValue(float value);
diff --git a/src/comm/ParameterList.cc b/src/comm/ParameterList.cc
index 682489ee54464aaf4e47b0efc94d9f8bfabb7d4e..809db6f28c1038decc2e6810fee1e85251bdafba 100644
--- a/src/comm/ParameterList.cc
+++ b/src/comm/ParameterList.cc
@@ -45,9 +45,15 @@ ParameterList::ParameterList()
reqdServoParams->append("AIL_RIGHT_IN");
reqdServoParams->append("AIL_CENTER_IN");
reqdServoParams->append("AIL_LEFT_IN");
+ reqdServoParams->append("AIL_RIGHT_OUT");
+ reqdServoParams->append("AIL_CENTER_OUT");
+ reqdServoParams->append("AIL_LEFT_OUT");
reqdServoParams->append("ELE_DOWN_IN");
reqdServoParams->append("ELE_CENTER_IN");
reqdServoParams->append("ELE_UP_IN");
+ reqdServoParams->append("ELE_DOWN_OUT");
+ reqdServoParams->append("ELE_CENTER_OUT");
+ reqdServoParams->append("ELE_UP_OUT");
reqdServoParams->append("RUD_LEFT_IN");
reqdServoParams->append("RUD_CENTER_IN");
reqdServoParams->append("RUD_RIGHT_IN");
@@ -285,6 +291,15 @@ bool ParameterList::read(QIODevice *device)
child = child.nextSiblingElement("Block");
}
+ if (!reqdServoParams->empty())
+ {
+ qDebug() << __FILE__ << __LINE__ << "Missing the following required servo parameters";
+ foreach(QString s, *reqdServoParams)
+ {
+ qDebug() << s;
+ }
+ }
+
delete paramConfig;
return true;
}
@@ -297,9 +312,9 @@ void ParameterList::parseBlock(const QDomElement &block)
Parameter *p;
SubsystemIds id;
if (block.attribute("name") == "Navigation")
- id = OpalRT::NAV_ID;
+ id = OpalRT::NAV;
else if (block.attribute("name") == "Controller")
- id = OpalRT::CONTROLLER_ID;
+ id = OpalRT::CONTROLLER;
else if (block.attribute("name") == "ServoOutputs")
id = OpalRT::SERVO_OUTPUTS;
else if (block.attribute("name") == "ServoInputs")
@@ -332,13 +347,6 @@ void ParameterList::parseBlock(const QDomElement &block)
}
}
- 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 5a891f20b11a179409e00ce6cbe84f5acdb71726..c3eecc052d097619c1ccb139adf5d7787cfda212 100644
--- a/src/comm/ParameterList.h
+++ b/src/comm/ParameterList.h
@@ -53,11 +53,11 @@ namespace OpalRT
const_iterator(const const_iterator& other);
const_iterator& operator+=(int i) {index += i; return *this;}
- bool operator<(const const_iterator& other) {return (this->paramList == other.paramList)
+ bool operator<(const const_iterator& other) const {return (this->paramList == other.paramList)
&&(this->indexparamList == other.paramList)
+ bool operator==(const const_iterator& other) const {return (this->paramList == other.paramList)
&&(this->index==other.index);}
- bool operator!=(const const_iterator& other) {return !((*this) == other);}
+ bool operator!=(const const_iterator& other) const {return !((*this) == other);}
const Parameter& operator*() const {return paramList[index];}
const Parameter* operator->() const {return ¶mList[index];}
diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc
index f3e234c3e5aca894f7e923c2b8d734de1ad33082..0832671744731c374bdd5f588fa47834a9c5a907 100644
--- a/src/ui/HUD.cc
+++ b/src/ui/HUD.cc
@@ -721,8 +721,8 @@ void HUD::paintHUD()
painter.drawRoundedRect(compassRect, 2, 2);
QString yawAngle;
- const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f;
- qDebug() << "YAW: " << yawDeg;
+// const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f;
+ const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f);
yawAngle.sprintf("%03d", (int)yawDeg);
paintText(yawAngle, defaultColor, 3.5f, -3.7f, compassY+ 0.9f, &painter);
diff --git a/src/ui/RadioCalibration/RadioCalibrationData.cc b/src/ui/RadioCalibration/RadioCalibrationData.cc
index aba6ff1a3286d1812ad79fb4b3ad3b728539528b..530638d4708d2f78747968f5a3a873cbe653880d 100644
--- a/src/ui/RadioCalibration/RadioCalibrationData.cc
+++ b/src/ui/RadioCalibration/RadioCalibrationData.cc
@@ -48,18 +48,14 @@ const float* RadioCalibrationData::operator [](int i) const
return NULL;
}
-const QVector& RadioCalibrationData::operator ()(int i) const
+const QVector& RadioCalibrationData::operator ()(const int i) const throw(std::out_of_range)
{
- if (i < data->size())
+ if ((i < data->size()) && (i >=0))
{
return (*data)[i];
}
- // FIXME Bryan
- // FIXME James
-
- // This is not good. If it is ever used after being returned it will cause a crash
-// return QVector();
+ throw std::out_of_range("Invalid channel index");
}
QString RadioCalibrationData::toString(RadioElement element) const
diff --git a/src/ui/RadioCalibration/RadioCalibrationData.h b/src/ui/RadioCalibration/RadioCalibrationData.h
index 4eb5b071c3626281ec41bbeb4997a2d9222986f7..f001bc340f8b3b0b1f196ef9813128eb66fce7eb 100644
--- a/src/ui/RadioCalibration/RadioCalibrationData.h
+++ b/src/ui/RadioCalibration/RadioCalibrationData.h
@@ -34,6 +34,7 @@ This file is part of the QGROUNDCONTROL project
#include
#include
#include
+#include
/**
@@ -66,7 +67,7 @@ public:
};
const float* operator[](int i) const;
- const QVector& operator()(int i) const;
+ const QVector& operator()(int i) const throw(std::out_of_range);
void set(int element, int index, float value) {(*data)[element][index] = value;}
public slots:
diff --git a/src/ui/RadioCalibration/RadioCalibrationWindow.cc b/src/ui/RadioCalibration/RadioCalibrationWindow.cc
index 4f3c7cf76eafe63b84484a74b418f86354cff264..9684ffc1100d1b7fa7b39e4f116bc683376d3cfb 100644
--- a/src/ui/RadioCalibration/RadioCalibrationWindow.cc
+++ b/src/ui/RadioCalibration/RadioCalibrationWindow.cc
@@ -52,38 +52,38 @@ RadioCalibrationWindow::RadioCalibrationWindow(QWidget *parent) :
setUASId(0);
}
-void RadioCalibrationWindow::setChannelRaw(int ch, float raw)
-{
- /** 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::setChannelRaw(int ch, float raw)
+//{
+// /** 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::setChannelScaled(int ch, float normalized)
-{
+//void RadioCalibrationWindow::setChannelScaled(int ch, float normalized)
+//{
// FIXME James
// FIXME Bryan
@@ -113,9 +113,9 @@ void RadioCalibrationWindow::setChannelScaled(int ch, float normalized)
// }
-}
+//}
-void RadioCalibrationWindow::setChannel(int ch, float raw, float normalized)
+void RadioCalibrationWindow::setChannel(int ch, float raw)
{
/** this expects a particular channel to function mapping
\todo allow run-time channel mapping
diff --git a/src/ui/RadioCalibration/RadioCalibrationWindow.h b/src/ui/RadioCalibration/RadioCalibrationWindow.h
index 3a3f55310632d49c71a7b1b9b883390a74a1e109..930515a7f5a59536d11ecc440ad364b9370fae01 100644
--- a/src/ui/RadioCalibration/RadioCalibrationWindow.h
+++ b/src/ui/RadioCalibration/RadioCalibrationWindow.h
@@ -66,9 +66,10 @@ public:
explicit RadioCalibrationWindow(QWidget *parent = 0);
public slots:
- void setChannel(int ch, float raw, float normalized);
- void setChannelRaw(int ch, float raw);
- void setChannelScaled(int ch, float normalized);
+ void setChannel(int ch, float raw);
+ // @todo remove these functions if they are not needed - were added by lm on dec 14, 2010
+// void setChannelRaw(int ch, float raw);
+// void setChannelScaled(int ch, float normalized);
void loadFile();
void saveFile();
void send();