diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 6458bfde96085753a2e0be7aad2c942b6f5d1dd8..36004902502af7623a1dc8a1a01d8a73978220e9 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -18,8 +18,8 @@ TARGET = qgroundcontrol BASEDIR = . BUILDDIR = build LANGUAGE = C++ -CONFIG += debug_and_release - #console +CONFIG += debug_and_release \ + console OBJECTS_DIR = $$BUILDDIR/obj MOC_DIR = $$BUILDDIR/moc UI_HEADERS_DIR = src/ui/generated @@ -230,11 +230,23 @@ SOURCES += src/main.cc \ RESOURCES = mavground.qrc # Include RT-LAB Library -win32:exists(C:\OPAL-RT\RT-LAB7.2.4\Common\bin) { +win32:exists(src/lib/opalrt/OpalApi.h) { + message("Building support for Opal-RT") LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin \ -lOpalApi INCLUDEPATH += src/lib/opalrt - SOURCES += src/comm/OpalLink.cc - HEADERS += src/comm/OpalLink.h + HEADERS += src/comm/OpalRT.h \ + src/comm/OpalLink.h \ + src/comm/Parameter.h \ + src/comm/QGCParamID.h \ + src/comm/ParameterList.h \ + src/ui/OpalLinkConfigurationWindow.h + SOURCES += src/comm/OpalRT.cc \ + src/comm/OpalLink.cc \ + src/comm/Parameter.cc \ + src/comm/QGCParamID.cc \ + src/comm/ParameterList.cc \ + src/ui/OpalLinkConfigurationWindow.cc + FORMS += src/ui/OpalLinkSettings.ui DEFINES += OPAL_RT } diff --git a/settings/ParameterList.xml b/settings/ParameterList.xml new file mode 100644 index 0000000000000000000000000000000000000000..3894080f4c43a26c423c7b3cce235ec4cb77a275 --- /dev/null +++ b/settings/ParameterList.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Core.cc b/src/Core.cc index 54e7632fc038709694cdc00f0f92c5d182d24437..6bd8877df21591e0b22454790daa89eb3f702fb6 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -142,7 +142,6 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) // Add OpalRT Link, but do not connect OpalLink* opalLink = new OpalLink(); mainWindow->addLink(opalLink); -#warning OPAL LINK NOW AUTO CONNECTING IN CORE.CC #endif // MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(MG::DIR::getSupportFilesDirectory() + "/demo-log.txt"); MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(":/demo-log.txt"); diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc index 312737a8da1bca36873eb46a10d1d5e5717d3ab5..d4b7dfef67b85408ba6bf4c9491c130a5918f17e 100644 --- a/src/comm/MAVLinkXMLParser.cc +++ b/src/comm/MAVLinkXMLParser.cc @@ -204,31 +204,33 @@ bool MAVLinkXMLParser::generate() usedMessageNames->insert(messageName, QString::number(e.lineNumber())); } - QString channelType = "mavlink_channel_t"; - QString messageType = "mavlink_message_t"; + QString channelType("mavlink_channel_t"); + QString messageType("mavlink_message_t"); // Build up function call - QString commentContainer = "/**\n * @brief Send a %1 message\n *\n%2 * @return length of the message in bytes (excluding serial stream start sign)\n */\n"; - QString commentEntry = " * @param %1 %2\n"; + QString commentContainer("/**\n * @brief Send a %1 message\n *\n%2 * @return length of the message in bytes (excluding serial stream start sign)\n */\n"); + QString commentEntry(" * @param %1 %2\n"); QString idDefine = QString("#define MAVLINK_MSG_ID_%1 %2").arg(messageName.toUpper(), QString::number(messageId)); - QString arrayDefines = ""; + QString arrayDefines; QString cStructName = QString("mavlink_%1_t").arg(messageName); - QString cStruct = "typedef struct __%1 \n{\n%2\n} %1;"; - QString cStructLines = ""; - QString encode = "static inline uint16_t mavlink_msg_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn mavlink_msg_%1_pack(%3);\n}\n"; + QString cStruct("typedef struct __%1 \n{\n%2\n} %1;"); + QString cStructLines; + QString encode("static inline uint16_t mavlink_msg_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn mavlink_msg_%1_pack(%3);\n}\n"); - QString decode = "static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n"; - QString pack = "static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, i);\n}\n\n"; - QString compactSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; + QString decode("static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n"); + QString pack("static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, i);\n}\n\n"); + QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"); //QString compactStructSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_struct_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_encode(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; - QString unpacking = ""; - QString prepends = ""; - QString packParameters = ""; - QString packArguments = "system_id, component_id, msg"; - QString packLines = ""; - QString decodeLines = ""; - QString sendArguments = ""; - QString commentLines = ""; + QString unpacking; + QString prepends; + QString packParameters; + QString packArguments("system_id, component_id, msg"); + QString packLines; + QString decodeLines; + QString sendArguments; + QString commentLines; + + // Get the message fields QDomNode f = e.firstChild(); @@ -241,6 +243,9 @@ bool MAVLinkXMLParser::generate() QString fieldName = e2.attribute("name", ""); QString fieldText = e2.text(); + QString unpackingCode; + QString unpackingComment = QString("/**\n * @brief Get field %1 from %2 message\n *\n * @return %3\n */\n").arg(fieldName, messageName, fieldText); + // Send arguments are the same for integral types and arrays sendArguments += ", " + fieldName; @@ -255,7 +260,7 @@ bool MAVLinkXMLParser::generate() // Add field to C structure cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg("int8_t", fieldName, QString::number(arrayLength), fieldText); // Add pack line to message_xx_pack function - packLines += QString("\ti += put_%1_by_index(%2, %3, i, msg->payload); //%4\n").arg(arrayType, fieldName, QString::number(arrayLength), e2.text()); + packLines += QString("\ti += put_%1_by_index(%2, %3, i, msg->payload); //%4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); // Add decode function for this type decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); @@ -275,6 +280,29 @@ bool MAVLinkXMLParser::generate() decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); } + // Expand array handling to all valid mavlink data types + else if(fieldType.contains('[') && fieldType.contains(']')) + { + int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); + QString arrayType = fieldType.split("[").first(); + packParameters += QString(", const ") + arrayType + "* " + fieldName; + packArguments += ", " + messageName + "->" + fieldName; + + // Add field to C structure + cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); + // Add pack line to message_xx_pack function + packLines += QString("\ti += put_array_by_index((int8_t*)%1, sizeof(%2)*%3, i, msg->payload); //%4\n").arg(fieldName, arrayType, QString::number(arrayLength), fieldText); + // Add decode function for this type + decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); + arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); + + unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, sizeof(%2)*%3);\n\treturn sizeof(%2)*%3;").arg(prepends, arrayType, QString::number(arrayLength)); + + unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, %3* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, arrayType, unpackingCode); +// decodeLines += ""; + prepends += QString("+sizeof(%1)*%2").arg(arrayType, QString::number(arrayLength)); + + } else // Handle simple types like integers and floats { @@ -289,9 +317,9 @@ bool MAVLinkXMLParser::generate() decodeLines += QString("\t%1->%2 = mavlink_msg_%1_get_%2(msg);\n").arg(messageName, fieldName); } commentLines += commentEntry.arg(fieldName, fieldText); - QString unpackingComment = QString("/**\n * @brief Get field %1 from %2 message\n *\n * @return %3\n */\n").arg(fieldName, messageName, fieldText); + // - QString unpackingCode; +// QString unpackingCode; if (fieldType == "uint8_t" || fieldType == "int8_t") { @@ -322,6 +350,7 @@ bool MAVLinkXMLParser::generate() unpackingCode = QString("\n\tstrcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(prepends, fieldType.split("[").at(1).split("]").first()); } + // Generate the message decoding function // Array handling is different from simple types if (fieldType.startsWith("array")) @@ -338,6 +367,10 @@ bool MAVLinkXMLParser::generate() QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); prepends += "+" + arrayLength; } + else if(fieldType.contains('[') && fieldType.contains(']')) + { + // prevent this case from being caught in the following else + } else { unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg(fieldType, messageName, fieldName, unpackingCode); diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc index 90ff9805e5df7480cad1b72fe10841b644539786..5551c091c3ea35ab989543b4b61bae462f4fdb10 100644 --- a/src/comm/OpalLink.cc +++ b/src/comm/OpalLink.cc @@ -35,10 +35,12 @@ OpalLink::OpalLink() : heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), m_heartbeatsEnabled(true), getSignalsTimer(new QTimer(this)), - getSignalsPeriod(1000), + getSignalsPeriod(10), receiveBuffer(new QQueue()), systemID(1), - componentID(1) + componentID(1), + params(NULL), + opalInstID(101) { start(QThread::LowPriority); @@ -67,29 +69,83 @@ qint64 OpalLink::bytesAvailable() void OpalLink::writeBytes(const char *bytes, qint64 length) { - + /* decode the message */ + mavlink_message_t msg; + mavlink_status_t status; + int decodeSuccess = 0; + for (int i=0; (!(decodeSuccess=mavlink_parse_char(this->getId(), bytes[i], &msg, &status))&& ibegin(); paramIter != params->end(); ++paramIter) + { + mavlink_msg_param_value_pack(systemID, + (*paramIter).getComponentID(), + ¶m, + (*paramIter).getParamID().toInt8_t(), + (static_cast(*paramIter)).getValue(), + params->count(), + params->indexOf(*paramIter)); + receiveMessage(param); + } + + + } + case MAVLINK_MSG_ID_PARAM_SET: + { + +// qDebug() << "OpalLink::writeBytes(): Attempt to set a parameter"; + + mavlink_param_set_t param; + mavlink_msg_param_set_decode(&msg, ¶m); + OpalRT::QGCParamID paramName((char*)param.param_id); + +// qDebug() << "OpalLink::writeBytes():paramName: " << paramName; + + if ((*params).contains(param.target_component, paramName)) + { + OpalRT::Parameter p = (*params)(param.target_component, paramName); +// qDebug() << __FILE__ << ":" << __LINE__ << ": " << p; + // Set the param value in Opal-RT + p.setValue(param.param_value); + + // Get the param value from Opal-RT to make sure it was set properly + mavlink_message_t paramMsg; + mavlink_msg_param_value_pack(systemID, + p.getComponentID(), + ¶mMsg, + p.getParamID().toInt8_t(), + p.getValue(), + params->count(), + params->indexOf(p)); + receiveMessage(paramMsg); + } + } + break; + default: + { + qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet"; + } + } + } } void OpalLink::readBytes() { receiveDataMutex.lock(); - const qint64 maxLength = 2048; - char bytes[maxLength]; - qDebug() << "OpalLink::readBytes(): Reading a message. size of buffer: " << receiveBuffer->count(); - QByteArray message = receiveBuffer->dequeue(); - if (maxLength < message.size()) - { - qDebug() << "OpalLink::readBytes:: Buffer Overflow"; - - memcpy(bytes, message.data(), maxLength); - } - else - { - memcpy(bytes, message.data(), message.size()); - } - - emit bytesReceived(this, message); + emit bytesReceived(this, receiveBuffer->dequeue()); receiveDataMutex.unlock(); } @@ -104,7 +160,9 @@ void OpalLink::receiveMessage(mavlink_message_t message) // If link is connected if (isConnected()) { + receiveDataMutex.lock(); receiveBuffer->enqueue(QByteArray(buffer, len)); + receiveDataMutex.unlock(); readBytes(); } @@ -115,54 +173,120 @@ void OpalLink::heartbeat() if (m_heartbeatsEnabled) { - qDebug() << "OpalLink::heartbeat(): Generate a heartbeat"; mavlink_message_t beat; mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC); receiveMessage(beat); } } +void OpalLink::setSignals(double *values) +{ + unsigned short numSignals = 2; + unsigned short logicalId = 1; + unsigned short signalIndex[] = {0,1}; + int returnValue; + returnValue = OpalSetSignals( numSignals, logicalId, signalIndex, values); + if (returnValue != EOK) + { + OpalRT::OpalErrorMsg::displayLastErrorMsg(); + } +} void OpalLink::getSignals() { -// qDebug() << "OpalLink::getSignals(): Attempting to acquire signals"; -// -// -// unsigned long timeout = 0; -// unsigned short acqGroup = 0; //this is actually group 1 in the model -// unsigned short allocatedSignals = NUM_OUTPUT_SIGNALS; -// unsigned short *numSignals = new unsigned short(0); -// double *timestep = new double(0); -// double values[NUM_OUTPUT_SIGNALS] = {}; -// unsigned short *lastValues = new unsigned short(false); -// unsigned short *decimation = new unsigned short(0); -// -// int returnVal = OpalGetSignals(timeout, acqGroup, allocatedSignals, numSignals, timestep, -// values, lastValues, decimation); -// -// if (returnVal == EOK ) -// { -// qDebug() << "OpalLink::getSignals: Timestep=" << *timestep;// << ", Last? " << (bool)(*lastValues); -// } -// else if (returnVal == EAGAIN) -// { -// qDebug() << "OpalLink::getSignals: Data was not ready"; -// } -// // if returnVal == EAGAIN => data just wasn't ready -// else if (returnVal != EAGAIN) -// { -// getSignalsTimer->stop(); -// displayErrorMsg(); -// } -// /* deallocate used memory */ -// -// delete timestep; -// delete lastValues; -// delete lastValues; -// delete decimation; + unsigned long timeout = 0; + unsigned short acqGroup = 0; //this is actually group 1 in the model + unsigned short *numSignals = new unsigned short(0); + double *timestep = new double(0); + double values[OpalRT::NUM_OUTPUT_SIGNALS] = {}; + unsigned short *lastValues = new unsigned short(false); + unsigned short *decimation = new unsigned short(0); + + while (!(*lastValues)) + { + int returnVal = OpalGetSignals(timeout, acqGroup, OpalRT::NUM_OUTPUT_SIGNALS, numSignals, timestep, + values, lastValues, decimation); + + 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); + + /* send attitude info to qgroundcontrol */ + mavlink_message_t attitude; + mavlink_msg_attitude_pack(systemID, componentID, &attitude, + (*timestep)*1000000, + values[OpalRT::ROLL], + values[OpalRT::PITCH], + values[OpalRT::YAW], + values[OpalRT::ROLL_SPEED], + values[OpalRT::PITCH_SPEED], + values[OpalRT::YAW_SPEED] + ); + receiveMessage(attitude); + + /* send bias info to qgroundcontrol */ + mavlink_message_t bias; + mavlink_msg_nav_filter_bias_pack(systemID, componentID, &bias, + (*timestep)*1000000, + values[OpalRT::B_F_0], + values[OpalRT::B_F_1], + values[OpalRT::B_F_2], + values[OpalRT::B_W_0], + values[OpalRT::B_W_1], + values[OpalRT::B_W_2] + ); + receiveMessage(bias); + + /* send radio outputs */ + 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]), + duty2PulseMicros(values[OpalRT::RAW_CHANNEL_4]), + duty2PulseMicros(values[OpalRT::RAW_CHANNEL_5]), + 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), + 0 //rssi unused + ); + receiveMessage(rc); + } + else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready + { + getSignalsTimer->stop(); + OpalRT::OpalErrorMsg::displayLastErrorMsg(); + } + } + + /* deallocate used memory */ + + delete numSignals; + delete timestep; + delete lastValues; + delete decimation; } + /* * Administrative @@ -170,7 +294,7 @@ void OpalLink::getSignals() */ void OpalLink::run() { - qDebug() << "OpalLink::run():: Starting the thread"; +// qDebug() << "OpalLink::run():: Starting the thread"; } int OpalLink::getId() @@ -189,11 +313,16 @@ void OpalLink::setName(QString name) emit nameChanged(this->name); } -bool OpalLink::isConnected() { - //qDebug() << "OpalLink::isConnected:: connectState: " << connectState; +bool OpalLink::isConnected() { return connectState; } +uint16_t OpalLink::duty2PulseMicros(double duty) +{ + /* duty cycle assumed to be of a signal at 70 Hz */ + return static_cast(duty/70*1000000); +} + @@ -201,19 +330,24 @@ bool OpalLink::connect() { short modelState; - /// \todo allow configuration of instid in window -// if (OpalConnect(101, false, &modelState) == EOK) -// { + /// \todo allow configuration of instid in window + if ((OpalConnect(opalInstID, false, &modelState) == EOK) + && (OpalGetSignalControl(0, true) == EOK) + && (OpalGetParameterControl(true) == EOK)) + { connectState = true; + if (params) + delete params; + params = new OpalRT::ParameterList(); emit connected(); heartbeatTimer->start(1000/heartbeatRate); getSignalsTimer->start(getSignalsPeriod); -// } -// else -// { -// connectState = false; -// displayErrorMsg(); -// } + } + else + { + connectState = false; + OpalRT::OpalErrorMsg::displayLastErrorMsg(); + } emit connected(connectState); return connectState; @@ -221,26 +355,16 @@ bool OpalLink::connect() bool OpalLink::disconnect() { - return false; + // OpalDisconnect returns void so its success or failure cannot be tested + OpalDisconnect(); + heartbeatTimer->stop(); + getSignalsTimer->stop(); + connectState = false; + emit connected(connectState); + return true; } -void OpalLink::displayErrorMsg() -{ - setLastErrorMsg(); - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText(lastErrorMsg); - msgBox.exec(); -} -void OpalLink::setLastErrorMsg() -{ -// char buf[512]; -// unsigned short len; -// OpalGetLastErrMsg(buf, sizeof(buf), &len); -// lastErrorMsg.clear(); -// lastErrorMsg.append(buf); -} /* diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h index ad0cbe65b3747e42fc76dd17cb3fa7c13fa8e64a..f72ba9d23b3da4c29d9b36f1e73bcb8d31ff7184 100644 --- a/src/comm/OpalLink.h +++ b/src/comm/OpalLink.h @@ -32,11 +32,13 @@ This file is part of the QGROUNDCONTROL project #include #include -#include +#include #include #include #include #include +#include + #include "LinkInterface.h" #include "LinkManager.h" @@ -44,31 +46,18 @@ This file is part of the QGROUNDCONTROL project #include "mavlink.h" #include "mavlink_types.h" #include "configuration.h" - +#include "OpalRT.h" +#include "ParameterList.h" +#include "Parameter.h" +#include "QGCParamID.h" +#include "OpalApi.h" #include "errno.h" - - - - -// FIXME -//#include "OpalApi.h" - - - - - #include "string.h" -/* - Configuration info for the model - */ - -#define NUM_OUTPUT_SIGNALS 6 - /** - * @brief Interface to OPAL-RT targets + * @brief Interface to OpalRT targets * - * This is an interface to the Opal-RT hardware-in-the-loop (HIL) simulator. + * This is an interface to the OpalRT hardware-in-the-loop (HIL) simulator. * This class receives MAVLink packets as if it is a true link, but it * interprets the packets internally, and calls the appropriate api functions. * @@ -100,34 +89,32 @@ public: qint64 getBitsSent(); qint64 getBitsReceived(); - bool connect(); - bool disconnect(); - qint64 bytesAvailable(); void run(); -public slots: + int getOpalInstID() {return static_cast(opalInstID);} +public slots: void writeBytes(const char *bytes, qint64 length); - void readBytes(); void heartbeat(); void getSignals(); + void setOpalInstID(int instID) {opalInstID = static_cast(instID);} + protected slots: void receiveMessage(mavlink_message_t message); - - + void setSignals(double *values); protected: QString name; @@ -144,9 +131,6 @@ protected: QMutex statisticsMutex; QMutex receiveDataMutex; - QString lastErrorMsg; - void setLastErrorMsg(); - void displayErrorMsg(); void setName(QString name); @@ -163,7 +147,12 @@ protected: const int systemID; const int componentID; + void getParameterList(); + OpalRT::ParameterList *params; + + unsigned short opalInstID; + uint16_t duty2PulseMicros(double duty); }; #endif // OPALLINK_H diff --git a/src/comm/OpalRT.cc b/src/comm/OpalRT.cc new file mode 100644 index 0000000000000000000000000000000000000000..21917ae2fdf68e1480012aafb905bc4952cb283f --- /dev/null +++ b/src/comm/OpalRT.cc @@ -0,0 +1,26 @@ +#include "OpalRT.h" + +namespace OpalRT +{ + // lastErrorMsg = QString(); + void OpalErrorMsg::displayLastErrorMsg() + { + static QString lastErrorMsg; + setLastErrorMsg(); + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText(lastErrorMsg); + msgBox.exec(); + } + + void OpalErrorMsg::setLastErrorMsg() + { + char* buf = new char[512]; + unsigned short len; + static QString lastErrorMsg; + OpalGetLastErrMsg(buf, sizeof(buf), &len); + lastErrorMsg.clear(); + lastErrorMsg.append(buf); + delete buf; + } +} diff --git a/src/comm/OpalRT.h b/src/comm/OpalRT.h new file mode 100644 index 0000000000000000000000000000000000000000..51a7f2e428928c56d6a082d47c3e8653cca5daf3 --- /dev/null +++ b/src/comm/OpalRT.h @@ -0,0 +1,130 @@ +/*===================================================================== + +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 Types used for Opal-RT interface configuration + * @author Bryan Godbolt + */ + +#ifndef OPALRT_H +#define OPALRT_H + +#include +#include + +#include "OpalApi.h" + +namespace OpalRT +{ + /** + Configuration info for the model + */ + + const unsigned short NUM_OUTPUT_SIGNALS=57; + + /* ------------------------------ Outputs ------------------------------ + * + * Port 1: Navigation state estimates + * 1 t [s] time elapsed since INS mode started + * 2-4 p^n [m] navigation frame position (N,E,D) + * 5-7 v^n [m/s] navigation frame velocity (N,E,D) + * 8-10 Euler angles [rad] (roll, pitch, yaw) + * 11-13 Angular rates + * 14-16 b_f [m/s^2] accelerometer biases + * 17-19 b_w [rad/s] gyro biases + * + * Port 2: Navigation system status + * 1 mode (0: initialization, 1: INS) + * 2 t_GPS time elapsed since last valid GPS measurement + * 3 solution status (0: SOL_COMPUTED, 1: INSUFFICIENT_OBS) + * 4 position solution type ( 0: NONE, 34: NARROW_FLOAT, + * 49: WIDE_INT, 50: NARROW_INT) + * 5 # obs (number of visible satellites) + * + * Port 3: Covariance matrix diagonal + * 1-15 diagonal elements of estimation error covariance matrix P + */ + enum SignalPort + { + T_ELAPSED, + X_POS, + Y_POS, + Z_POS, + X_VEL, + Y_VEL, + Z_VEL, + ROLL, + PITCH, + YAW, + ROLL_SPEED, + PITCH_SPEED, + YAW_SPEED, + B_F_0, + B_F_1, + B_F_2, + B_W_0, + B_W_1, + B_W_2, + RAW_CHANNEL_1 = 39, + RAW_CHANNEL_2, + RAW_CHANNEL_3, + RAW_CHANNEL_4, + RAW_CHANNEL_5, + RAW_CHANNEL_6, + RAW_CHANNEL_7, + RAW_CHANNEL_8, + NORM_CHANNEL_1, + NORM_CHANNEL_2, + NORM_CHANNEL_3, + NORM_CHANNEL_4, + NORM_CHANNEL_5, + NORM_CHANNEL_6, + NORM_CHANNEL_7, + NORM_CHANNEL_8, + CONTROLLER_AILERON, + CONTROLLER_ELEVATOR + }; + + /** Component IDs of the parameters. Currently they are all 1 becuase there is no advantage + to dividing them between component ids. However this syntax is used so that this can + easily be changed in the future. + */ + enum SubsystemIds + { + NAV_ID = 1, + LOG_ID, + CONTROLLER_ID, + SERVO_OUTPUTS, + SERVO_INPUTS + }; + + class OpalErrorMsg + { + static QString lastErrorMsg; + public: + static void displayLastErrorMsg(); + static void setLastErrorMsg(); + }; +} +#endif // OPALRT_H diff --git a/src/comm/Parameter.cc b/src/comm/Parameter.cc new file mode 100644 index 0000000000000000000000000000000000000000..512982e019d507a98f0f8caa13b95f60aed12a12 --- /dev/null +++ b/src/comm/Parameter.cc @@ -0,0 +1,120 @@ +/*===================================================================== + +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 Implementation of class OpalRT::Parameter + * @author Bryan Godbolt + */ +#include "Parameter.h" +using namespace OpalRT; + +//Parameter::Parameter(char *simulinkPath, char *simulinkName, uint8_t componentID, +// QGCParamID paramID, unsigned short opalID) +// : simulinkPath(new QString(simulinkPath)), +// simulinkName(new QString(simulinkName)), +// componentID(componentID), +// paramID(new QGCParamID(paramID)), +// opalID(opalID) +// +//{ +//} +Parameter::Parameter(QString simulinkPath, QString simulinkName, uint8_t componentID, + QGCParamID paramID, unsigned short opalID) + : simulinkPath(new QString(simulinkPath)), + simulinkName(new QString(simulinkName)), + componentID(componentID), + paramID(new QGCParamID(paramID)), + opalID(opalID) + +{ +} +Parameter::Parameter(const Parameter &other) + : componentID(other.componentID), + opalID(other.opalID) +{ + simulinkPath = new QString(*other.simulinkPath); + simulinkName = new QString(*other.simulinkName); + paramID = new QGCParamID(*other.paramID); +} + +Parameter::~Parameter() +{ + delete simulinkPath; + delete simulinkName; + delete paramID; +} + +bool Parameter::operator ==(const Parameter& other) const +{ + return + (*simulinkPath) == *(other.simulinkPath) + && *simulinkName == *(other.simulinkName) + && componentID == other.componentID + && *paramID == *(other.paramID) + && opalID == other.opalID; + +} + +float Parameter::getValue() +{ + unsigned short allocatedParams = 1; + unsigned short numParams; + unsigned short numValues = 1; + unsigned short returnedNumValues; + double value; + + int returnVal = OpalGetParameters(allocatedParams, &numParams, &opalID, + numValues, &returnedNumValues, &value); + + if (returnVal != EOK) + { + OpalRT::OpalErrorMsg::displayLastErrorMsg(); + return FLT_MAX; + } + + return static_cast(value); +} + +void Parameter::setValue(float val) +{ + unsigned short allocatedParams = 1; + unsigned short numParams; + unsigned short numValues = 1; + unsigned short returnedNumValues; + double value = static_cast(val); + + int returnVal = OpalSetParameters(allocatedParams, &numParams, &opalID, + numValues, &returnedNumValues, &value); + if (returnVal != EOK) + { + //qDebug() << __FILE__ << ":" << __LINE__ << ": Error numer: " << QString::number(returnVal); + OpalErrorMsg::displayLastErrorMsg(); + } +} + +Parameter::operator QString() const +{ + return *simulinkPath + *simulinkName + " " + QString::number(componentID) + + " " + *paramID + " " + QString::number(opalID); +} diff --git a/src/comm/Parameter.h b/src/comm/Parameter.h new file mode 100644 index 0000000000000000000000000000000000000000..695cd8a79d6a24b474f006db54337c1196d1e329 --- /dev/null +++ b/src/comm/Parameter.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 Parameter Object used to intefrace with an OpalRT Simulink Parameter + \see OpalLink + \see OpalRT::ParameterList + * @author Bryan Godbolt + */ + +#ifndef PARAMETER_H +#define PARAMETER_H + +#include +#include + +#include "mavlink_types.h" +#include "QGCParamID.h" +#include "OpalApi.h" +#include "OpalRT.h" +#include + +namespace OpalRT +{ + class Parameter + { + public: +// Parameter(char *simulinkPath = "", +// char *simulinkName = "", +// uint8_t componentID = 0, +// QGCParamID paramID = QGCParamID(), +// unsigned short opalID = 0); + Parameter(QString simulinkPath = QString(), + QString simulinkName = QString(), + uint8_t componentID = 0, + QGCParamID paramID = QGCParamID(), + unsigned short opalID = 0); + Parameter(const Parameter& other); + ~Parameter(); + + const QGCParamID& getParamID() const {return *paramID;} + void setOpalID(unsigned short opalID) {this->opalID = opalID;} + const QString& getSimulinkPath() {return *simulinkPath;} + const QString& getSimulinkName() {return *simulinkName;} + uint8_t getComponentID() const {return componentID;} + float getValue(); + void setValue(float value); + + bool operator==(const Parameter& other) const; + operator QString() const; + + protected: + QString *simulinkPath; + QString *simulinkName; + uint8_t componentID; + QGCParamID *paramID; + unsigned short opalID; + }; +} + +#endif // PARAMETER_H diff --git a/src/comm/ParameterList.cc b/src/comm/ParameterList.cc new file mode 100644 index 0000000000000000000000000000000000000000..0b1d3762468c953925a21ca7c61fc16408534004 --- /dev/null +++ b/src/comm/ParameterList.cc @@ -0,0 +1,347 @@ +/*===================================================================== + +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 Implementation of class OpalRT::ParameterList + * @author Bryan Godbolt + */ + +#include "ParameterList.h" +using namespace OpalRT; + +ParameterList::ParameterList() + :params(new QMap >), + paramList(new QList >()) +{ + + QDir settingsDir = QDir(qApp->applicationDirPath()); + if (settingsDir.dirName() == "bin") + settingsDir.cdUp(); + settingsDir.cd("settings"); + + 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; + getParameterList(opalParams); + + /* Iterate over the parameters we want to use in qgc and populate their ids */ + QMap >::iterator componentIter; + QMap::iterator paramIter; + QString s; + for (componentIter = params->begin(); componentIter != params->end(); ++componentIter) + { + paramList->append(QList()); + for (paramIter = (*componentIter).begin(); paramIter != (*componentIter).end(); ++paramIter) + { + paramList->last().append(paramIter.operator ->()); + s = (*paramIter).getSimulinkPath() + (*paramIter).getSimulinkName(); + if (opalParams->contains(s)) + { + (*paramIter).setOpalID(opalParams->value(s)); + // qDebug() << __FILE__ << " Line:" << __LINE__ << ": Successfully added " << s; + } + else + { + qWarning() << __FILE__ << " Line:" << __LINE__ << ": " << s << " was not found in param list"; + } + } + } + delete opalParams; + } +} + +ParameterList::~ParameterList() +{ + delete params; + delete paramList; +} + +/** + Get the list of parameters in the simulink model. This function does not require + any prior knowlege of the parameters. It works by first calling OpalGetParameterList to + get the number of paramters, then allocates the required amount of memory and then gets + the paramter list using a second call to OpalGetParameterList. + */ +void ParameterList::getParameterList(QMap *opalParams) +{ + /* inputs */ + unsigned short allocatedParams=0; + unsigned short allocatedPathLen=0; + unsigned short allocatedNameLen=0; + unsigned short allocatedVarLen=0; + + /* outputs */ + unsigned short numParams; + unsigned short *idParam=NULL; + unsigned short maxPathLen; + char **paths=NULL; + unsigned short maxNameLen; + char **names=NULL; + unsigned short maxVarLen; + char **var=NULL; + + int returnValue; + + returnValue = OpalGetParameterList(allocatedParams, &numParams, idParam, + allocatedPathLen, &maxPathLen, paths, + allocatedNameLen, &maxNameLen, names, + allocatedVarLen, &maxVarLen, var); + if (returnValue!=E2BIG) + { +// OpalRT::setLastErrorMsg(); + OpalRT::OpalErrorMsg::displayLastErrorMsg(); + return; + } + + // allocate memory for parameter list + + idParam = new unsigned short[numParams]; + allocatedParams = numParams; + + paths = new char*[numParams]; + for (int i=0; iinsert(path+name, idParam[i]); + else + opalParams->insert(path+'/'+name, idParam[i]); + } +// Dump out the list of parameters +// QMap::const_iterator paramPrint; +// for (paramPrint = opalParams->begin(); paramPrint != opalParams->end(); ++paramPrint) +// qDebug() << paramPrint.key(); + + +} + +int ParameterList::indexOf(const Parameter &p) +{ + // incase p is a copy of the actual parameter we want (i.e., addresses differ) + Parameter *pPtr = &((*params)[p.getComponentID()][p.getParamID()]); + + QList >::const_iterator iter; + int index = -1; + for (iter = paramList->begin(); iter != paramList->end(); ++iter) + { + if ((index = (*iter).indexOf(pPtr)) != -1) + return index; + } + return index; +} + + +ParameterList::const_iterator::const_iterator(QList paramList) +{ + this->paramList = QList(paramList); + index = 0; +} + +ParameterList::const_iterator::const_iterator(const const_iterator &other) +{ + paramList = QList(other.paramList); + index = other.index; +} + +ParameterList::const_iterator ParameterList::begin() const +{ + QList > compList = params->values(); + QList paramList; + QList >::const_iterator compIter; + for (compIter = compList.begin(); compIter != compList.end(); ++compIter) + paramList.append((*compIter).values()); + return const_iterator(paramList); +} + +ParameterList::const_iterator ParameterList::end() const +{ + const_iterator iter = begin(); + return iter+=iter.paramList.size(); +} + +int ParameterList::count() +{ + int count = 0; + QList >::const_iterator iter; + for (iter = paramList->begin(); iter != paramList->end(); ++iter) + count += (*iter).count(); + return count; +} + +/* Functions related to reading the xml config file */ + +bool ParameterList::open(QString filename) +{ + QFile paramFile(filename); + if (!paramFile.exists()) + { + /// \todo open dialog box (maybe: that could also go in comm config window) + return false; + } + + if (!paramFile.open(QIODevice::ReadOnly)) + { + return false; + } + + read(¶mFile); + + return true; +} + +bool ParameterList::read(QIODevice *device) +{ + QDomDocument *paramConfig = new QDomDocument(); + + QString errorStr; + int errorLine; + int errorColumn; + + if (!paramConfig->setContent(device, true, &errorStr, &errorLine, + &errorColumn)) + { + qDebug() << "Error reading XML Parameter File on line: " << errorLine << errorStr; + return false; + } + + QDomElement root = paramConfig->documentElement(); + if (root.tagName() != "ParameterList") { + qDebug() << __FILE__ << __LINE__ << "This is not a parameter list xml file"; + return false; + } + + QDomElement child = root.firstChildElement("Block"); + while (!child.isNull()) + { + parseBlock(child); + child = child.nextSiblingElement("Block"); + } + + delete paramConfig; + return true; +} + +void ParameterList::parseBlock(const QDomElement &block) +{ + + QDomNodeList paramList; + QDomElement e; + Parameter *p; + SubsystemIds id; + if (block.attribute("name") == "Navigation") + id = OpalRT::NAV_ID; + else if (block.attribute("name") == "Controller") + id = OpalRT::CONTROLLER_ID; + else if (block.attribute("name") == "ServoOutputs") + id = OpalRT::SERVO_OUTPUTS; + else if (block.attribute("name") == "ServoInputs") + id = OpalRT::SERVO_INPUTS; + + paramList = block.elementsByTagName("Parameter"); + for (int i=0; i < paramList.size(); ++i) + { + e = paramList.item(i).toElement(); + if (e.hasAttribute("SimulinkPath") && + e.hasAttribute("SimulinkParameterName") && + e.hasAttribute("QGCParamID")) + { + + p = new Parameter(e.attribute("SimulinkPath"), + e.attribute("SimulinkParameterName"), + static_cast(id), + QGCParamID(e.attribute("QGCParamID"))); + (*params)[id].insert(p->getParamID(), *p); + delete p; + } + else + { + qDebug() << __FILE__ << ":" << __LINE__ << ": error in xml doc"; + } + } + +} diff --git a/src/comm/ParameterList.h b/src/comm/ParameterList.h new file mode 100644 index 0000000000000000000000000000000000000000..f93ffac779a3a4b96f55408ab7bc655409812bae --- /dev/null +++ b/src/comm/ParameterList.h @@ -0,0 +1,142 @@ +/*===================================================================== + +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 . + +======================================================================*/ + +#ifndef PARAMETERLIST_H +#define PARAMETERLIST_H + +#include +#include +#include +#include +#include +#include +#include + +#include "mavlink_types.h" +#include "QGCParamID.h" +#include "Parameter.h" +#include "OpalRT.h" + +namespace OpalRT +{ + class ParameterList + { + public: + + class const_iterator + { + friend class ParameterList; + + public: + inline const_iterator() {} + 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) + &&(this->indexparamList == other.paramList) + &&(this->index==other.index);} + bool operator!=(const const_iterator& other) {return !((*this) == other);} + const Parameter& operator*() const {return paramList[index];} + const Parameter* operator->() const {return ¶mList[index];} + + const_iterator& operator++() {++index; return *this;} + private: + const_iterator(QList); + QList paramList; + int index; + }; + + + ParameterList(); + ~ParameterList(); + + /** Count the number of parameters in the list. + \return Total number of parameters + */ + int count(); + + /** Find p in the list and return its index. + \note In order to use this index to look up p, the component is also needed. + \return the index of p or -1 if p is not found + \example + \code + int compid = OpalRT::CONTROLLER_ID; + Parameter p("simulinkpath", "simulinkparamname", compid, QGCParamID("PID_GAIN")); + ParameterList pList; + if ((int index=pList.indexOf(p)) != -1) + qDebug() << "PID_GAIN is at index " << index; + \endcode + */ + int indexOf(const Parameter& p); + bool contains(int compid, QGCParamID paramid) const {return (*params)[compid].contains(paramid);} + + /// Get a parameter from the list + const Parameter getParameter(int compid, QGCParamID paramid) const {return (*params)[compid][paramid];} + Parameter& getParameter(int compid, QGCParamID paramid) {return (*params)[compid][paramid];} + const Parameter getParameter(int compid, int index) const {return *((*paramList)[compid][index]);} + + /** Convenient syntax for calling OpalRT::Parameter::getParameter() */ + Parameter& operator()(int compid, QGCParamID paramid) {return getParameter(compid, paramid);} + Parameter& operator()(uint8_t compid, QGCParamID paramid) {return getParameter(static_cast(compid), paramid);} + + const_iterator begin() const; + const_iterator end() const; + + protected: + /** Store the parameters mapped by componentid, and paramid. + \code + // Look up a parameter + int compid = 1; + QGCParamID paramid("PID_GAIN"); + Parameter p = params[compid][paramid]; + \endcode + */ + QMap > *params; + /** + Store pointers to the parameters to allow fast lookup by index. + This variable may be changed to const pointers to ensure all changes + are made through the map container. + */ + QList > *paramList; + /** + 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 + */ + void getParameterList(QMap* opalParams); + + /** + Open a file for reading in the xml config data + */ + bool open(QString filename=QString()); + /** + Attempt to read XML configuration data from device + \param[in] the device to read the xml data from + \return true if the configuration was read successfully, false otherwise + */ + bool read(QIODevice *device); + + void parseBlock(const QDomElement &block); + }; +} +#endif // PARAMETERLIST_H diff --git a/src/comm/QGCParamID.cc b/src/comm/QGCParamID.cc new file mode 100644 index 0000000000000000000000000000000000000000..804835a6540fce7ac406b42d96d11a5acb56cd3e --- /dev/null +++ b/src/comm/QGCParamID.cc @@ -0,0 +1,51 @@ +/*===================================================================== + +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 Implementation of class OpalRT::QGCParamID + * @author Bryan Godbolt + */ + +#include "QGCParamID.h" +using namespace OpalRT; + +QGCParamID::QGCParamID(const char paramid[]):data(paramid) +{ +} + +QGCParamID::QGCParamID(const QString s):data(s) +{ + +} + +QGCParamID::QGCParamID(const QGCParamID &other):data(other.data) +{ + +} + +// +//QDataStream& operator<<(QDataStream& stream, const QGCParamID& paramid) +//{ +// return stream << paramid.data; +//} diff --git a/src/comm/QGCParamID.h b/src/comm/QGCParamID.h new file mode 100644 index 0000000000000000000000000000000000000000..90cf2fcff92727946fca1212263dfd7253fe6fde --- /dev/null +++ b/src/comm/QGCParamID.h @@ -0,0 +1,72 @@ +/*===================================================================== + +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 Stores the paramid used for mavlink + * @author Bryan Godbolt + */ + +#ifndef QGCPARAMID_H +#define QGCPARAMID_H + +#include + +#include "mavlink_types.h" + +//namespace OpalRT +//{ +// class QGCParamID; +//} +// +//QDataStream& operator<<(QDataStream&, const OpalRT::QGCParamID&); + +namespace OpalRT +{ + /** Stores a param_id for the mavlink parameter packets. This class adds the convenience + of storing the id as a string (e.g., easy comparison). + + \todo Fix: warning: deprecated conversion from string constant to 'char*' + */ + class QGCParamID + { +// friend QDataStream& operator<<(QDataStream& stream, const QGCParamID& paramid); + public: + + QGCParamID(const char[]); + QGCParamID(const QString); + QGCParamID() {} + QGCParamID(const QGCParamID& other); + + bool operator<(const QGCParamID& other) const {return data(data);} + int8_t* toInt8_t() const {return (int8_t*)data.toAscii().data();} + + protected: + QString data; + }; +} +#endif // QGCPARAMID_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index e34ebfd5d85602ed0dda9372ebb885b726fd3f9f..5d65c4f4f09d196a995b5c17de3c85d73604eedb 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -538,6 +538,21 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit textMessageReceived(uasId, message.compid, severity, text); } break; +#ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES + case MAVLINK_MSG_ID_NAV_FILTER_BIAS: + { + mavlink_nav_filter_bias_t bias; + mavlink_msg_nav_filter_bias_decode(&message, &bias); + quint64 time = MG::TIME::getGroundTimeNow(); + emit valueChanged(uasId, "b_f[0]", bias.accel_0, time); + emit valueChanged(uasId, "b_f[1]", bias.accel_1, time); + emit valueChanged(uasId, "b_f[2]", bias.accel_2, time); + emit valueChanged(uasId, "b_w[0]", bias.gyro_0, time); + emit valueChanged(uasId, "b_w[1]", bias.gyro_1, time); + emit valueChanged(uasId, "b_w[2]", bias.gyro_2, time); + } + break; +#endif default: { if (!unknownPackets.contains(message.msgid)) diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index a51303e885c4926de4372ae72ed3d7e02c208730..5aba706ef7494e04afa19a83e73594c83be68501 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -43,6 +43,7 @@ This file is part of the QGROUNDCONTROL project #include "MAVLinkSimulationLink.h" #ifdef OPAL_RT #include "OpalLink.h" +#include "OpalLinkConfigurationWindow.h" #endif #include "MAVLinkProtocol.h" #include "MAVLinkSettingsWidget.h" @@ -114,6 +115,10 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn OpalLink* opal = dynamic_cast(link); if (opal != 0) { + QWidget* conf = new OpalLinkConfigurationWindow(opal, this); + QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.linkGroupBox); + layout->addWidget(conf); + ui.linkGroupBox->setLayout(layout); ui.linkGroupBox->setTitle(tr("Opal-RT Link")); } #endif diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index b38304e9f10a20cfdbe148110667758fe66f53b4..ac739b9810426eae879b7cefae341933f4565b0a 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -979,6 +979,13 @@ void MainWindow::loadEngineerView() parametersDockWidget->show(); } + // RADIO CONTROL VIEW + if (rcViewDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, rcViewDockWidget); + rcViewDockWidget->show(); + } + this->show(); } diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index 49c27902d82da3085c6f171a5979af21b92d71cc..7a936e1e7ed83c5d9050db2678bf617d92692843 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -38,7 +38,7 @@ 0 0 1000 - 22 + 23 @@ -113,7 +113,7 @@ - + :/images/actions/system-log-out.svg:/images/actions/system-log-out.svg @@ -125,7 +125,7 @@ - + :/images/categories/preferences-system.svg :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg @@ -138,7 +138,7 @@ - + :/images/control/launch.svg :/images/control/launch.svg:/images/control/launch.svg @@ -148,7 +148,7 @@ - + :/images/control/land.svg:/images/control/land.svg @@ -173,7 +173,7 @@ - + :/images/actions/list-add.svg:/images/actions/list-add.svg @@ -182,7 +182,7 @@ - + :/images/categories/applications-system.svg:/images/categories/applications-system.svg @@ -194,7 +194,7 @@ - + :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg @@ -203,7 +203,7 @@ - + :/images/status/weather-overcast.svg:/images/status/weather-overcast.svg @@ -212,7 +212,7 @@ - + :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg @@ -221,7 +221,7 @@ - + :/images/devices/input-gaming.svg:/images/devices/input-gaming.svg @@ -230,7 +230,7 @@ - + :/images/status/network-wireless-encrypted.svg:/images/status/network-wireless-encrypted.svg @@ -242,7 +242,7 @@ - + :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg @@ -257,7 +257,7 @@ true - + :/images/control/launch.svg:/images/control/launch.svg @@ -269,7 +269,7 @@ - + :/images/status/network-transmit-receive.svg:/images/status/network-transmit-receive.svg @@ -278,7 +278,7 @@ - + :/images/devices/network-wired.svg:/images/devices/network-wired.svg @@ -287,7 +287,7 @@ - + :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg @@ -296,7 +296,7 @@ - + :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg @@ -305,7 +305,7 @@ - + :/images/status/software-update-available.svg:/images/status/software-update-available.svg @@ -314,7 +314,7 @@ - + :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg @@ -323,9 +323,7 @@ - - - + actionExit diff --git a/src/ui/OpalLinkConfigurationWindow.cc b/src/ui/OpalLinkConfigurationWindow.cc new file mode 100644 index 0000000000000000000000000000000000000000..eba4328156d593d7a229c64ae2857c7dce5c8e52 --- /dev/null +++ b/src/ui/OpalLinkConfigurationWindow.cc @@ -0,0 +1,25 @@ +#include "OpalLinkConfigurationWindow.h" + +OpalLinkConfigurationWindow::OpalLinkConfigurationWindow(OpalLink* link, + QWidget *parent, + Qt::WindowFlags flags) : + QWidget(parent, flags), + link(link) + +{ + + + ui.setupUi(this); + + ui.opalInstIDSpinBox->setValue(this->link->getOpalInstID()); + + connect(ui.opalInstIDSpinBox, SIGNAL(valueChanged(int)), link, SLOT(setOpalInstID(int))); + connect(link, SIGNAL(connected(bool)), this, SLOT(allowSettingsAccess(bool))); + this->show(); +} + +void OpalLinkConfigurationWindow::allowSettingsAccess(bool enabled) +{ + ui.paramFileButton->setEnabled(enabled); + ui.servoConfigFileButton->setEnabled(enabled); +} diff --git a/src/ui/OpalLinkConfigurationWindow.h b/src/ui/OpalLinkConfigurationWindow.h new file mode 100644 index 0000000000000000000000000000000000000000..fdac559189aed2f16c0175e0b06dff18513d384f --- /dev/null +++ b/src/ui/OpalLinkConfigurationWindow.h @@ -0,0 +1,27 @@ +#ifndef OPALLINKCONFIGURATIONWINDOW_H +#define OPALLINKCONFIGURATIONWINDOW_H + +#include +#include + +#include "LinkInterface.h" +#include "ui_OpalLinkSettings.h" +#include "OpalLink.h" + +class OpalLinkConfigurationWindow : public QWidget +{ +Q_OBJECT +public: + explicit OpalLinkConfigurationWindow(OpalLink* link, QWidget *parent = 0, Qt::WindowFlags flags = Qt::Sheet); +signals: + +public slots: + + void allowSettingsAccess(bool enabled); + +private: + Ui::OpalLinkSettings ui; + OpalLink* link; +}; + +#endif // OPALLINKCONFIGURATIONWINDOW_H diff --git a/src/ui/OpalLinkSettings.ui b/src/ui/OpalLinkSettings.ui new file mode 100644 index 0000000000000000000000000000000000000000..8fd6796fb68e9073cb0da17ba018b4f23d91b709 --- /dev/null +++ b/src/ui/OpalLinkSettings.ui @@ -0,0 +1,173 @@ + + + OpalLinkSettings + + + + 0 + 0 + 537 + 250 + + + + OpalLink Configuration + + + + 6 + + + + + Model Instance ID + + + + + + + Opal-RT Parameter File + + + + + + + Servo Configuration File + + + + + + + Qt::Vertical + + + + 431 + 47 + + + + + + + + + + + + + + + + + + + + + + false + + + Change + + + + :/images/status/folder-open.svg:/images/status/folder-open.svg + + + + + + + false + + + Change + + + + :/images/status/folder-open.svg:/images/status/folder-open.svg + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + 101 + + + 200 + + + 101 + + + + + + + + + Delete + + + Delete this link + + + + + Connect + + + Connect this link + + + + + Close + + + Close the configuration window + + + + + + + + + actionClose + triggered() + OpalLinkSettings + close() + + + -1 + -1 + + + 224 + 195 + + + + + diff --git a/src/ui/QGCRemoteControlView.cc b/src/ui/QGCRemoteControlView.cc index 1968570750abfd7d71ddcd11e57175d7f3c9cab6..26d8efd7ee4630e487c796d7bdbbf7039f79ca7e 100644 --- a/src/ui/QGCRemoteControlView.cc +++ b/src/ui/QGCRemoteControlView.cc @@ -45,6 +45,7 @@ QGCRemoteControlView::QGCRemoteControlView(QWidget *parent) : channelLayout(new QVBoxLayout()), ui(new Ui::QGCRemoteControlView) { + //ui->setupUi(this); QGridLayout* layout = new QGridLayout(this); layout->addLayout(channelLayout, 1, 0, 1, 2);