diff --git a/lib/QMapControl/QMapControl.pri b/lib/QMapControl/QMapControl.pri index f03639792dfc8fa4de8d83fc54e1a439ccb02d56..3fd297718324eb24d2b30daead89d8be32fee171 100644 --- a/lib/QMapControl/QMapControl.pri +++ b/lib/QMapControl/QMapControl.pri @@ -3,50 +3,50 @@ INCLUDEPATH += src # Input HEADERS += curve.h \ - geometry.h \ - imagemanager.h \ - layer.h \ - layermanager.h \ - linestring.h \ - mapadapter.h \ - mapcontrol.h \ - mapnetwork.h \ - point.h \ - tilemapadapter.h \ - wmsmapadapter.h \ - circlepoint.h \ - imagepoint.h \ - gps_position.h \ - osmmapadapter.h \ - maplayer.h \ - geometrylayer.h \ - yahoomapadapter.h \ - googlemapadapter.h \ - googlesatmapadapter.h \ - openaerialmapadapter.h \ - fixedimageoverlay.h \ - emptymapadapter.h + geometry.h \ + imagemanager.h \ + layer.h \ + layermanager.h \ + linestring.h \ + mapadapter.h \ + mapcontrol.h \ + mapnetwork.h \ + point.h \ + tilemapadapter.h \ + wmsmapadapter.h \ + circlepoint.h \ + imagepoint.h \ + gps_position.h \ + osmmapadapter.h \ + maplayer.h \ + geometrylayer.h \ + yahoomapadapter.h \ + googlemapadapter.h \ + googlesatmapadapter.h \ + openaerialmapadapter.h \ + fixedimageoverlay.h \ + emptymapadapter.h SOURCES += curve.cpp \ - geometry.cpp \ - imagemanager.cpp \ - layer.cpp \ - layermanager.cpp \ - linestring.cpp \ - mapadapter.cpp \ - mapcontrol.cpp \ - mapnetwork.cpp \ - point.cpp \ - tilemapadapter.cpp \ - wmsmapadapter.cpp \ - circlepoint.cpp \ - imagepoint.cpp \ - gps_position.cpp \ - osmmapadapter.cpp \ - maplayer.cpp \ - geometrylayer.cpp \ - yahoomapadapter.cpp \ - googlemapadapter.cpp \ - googlesatmapadapter.cpp \ - openaerialmapadapter.cpp \ - fixedimageoverlay.cpp \ - emptymapadapter.cpp + geometry.cpp \ + imagemanager.cpp \ + layer.cpp \ + layermanager.cpp \ + linestring.cpp \ + mapadapter.cpp \ + mapcontrol.cpp \ + mapnetwork.cpp \ + point.cpp \ + tilemapadapter.cpp \ + wmsmapadapter.cpp \ + circlepoint.cpp \ + imagepoint.cpp \ + gps_position.cpp \ + osmmapadapter.cpp \ + maplayer.cpp \ + geometrylayer.cpp \ + yahoomapadapter.cpp \ + googlemapadapter.cpp \ + googlesatmapadapter.cpp \ + openaerialmapadapter.cpp \ + fixedimageoverlay.cpp \ + emptymapadapter.cpp diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 039068c37fb60e85ffd996558fc9178912c2c75d..435eb052fbd949ef289fd0116d9fd5bf02b20a48 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -112,7 +112,8 @@ HEADERS += src/MG.h \ src/GAudioOutput.h \ src/LogCompressor.h \ src/ui/param/ParamTreeItem.h \ - src/ui/param/ParamTreeModel.h + src/ui/param/ParamTreeModel.h \ + src/ui/QGCParamWidget.h SOURCES += src/main.cc \ src/Core.cc \ src/uas/UASManager.cc \ @@ -159,5 +160,6 @@ SOURCES += src/main.cc \ src/GAudioOutput.cc \ src/LogCompressor.cc \ src/ui/param/ParamTreeItem.cc \ - src/ui/param/ParamTreeModel.cc + src/ui/param/ParamTreeModel.cc \ + src/ui/QGCParamWidget.cc RESOURCES = mavground.qrc diff --git a/src/Core.cc b/src/Core.cc index 095b657c3b207321e2e2b600dc32d244de25745b..607f9c9fb552b24fb16cee5703edf4f82b9cb383 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -40,11 +40,14 @@ This file is part of the PIXHAWK project #include #include -#include -#include -#include +#include "Core.h" +#include "MG.h" +#include "MainWindow.h" #include "GAudioOutput.h" +#include "UDPLink.h" +#include "MAVLinkSimulationLink.h" + /** * @brief Constructor for the main application. @@ -100,6 +103,43 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) // Remove splash screen splashScreen->finish(mainWindow); + // Connect links + // to make sure that all components are initialized when the + // first messages arrive + UDPLink* udpLink = new UDPLink(QHostAddress::Any, 14550); + mainWindow->addLink(udpLink); + + // Check if link could be connected + if (!udpLink->connect()) + { + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText("Could not connect UDP port. Is already an instance of " + qAppName() + " running?"); + msgBox.setInformativeText("You will not be able to receive data via UDP. Do you want to close the application?"); + msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); + msgBox.setDefaultButton(QMessageBox::Cancel); + int ret = msgBox.exec(); + + // Close the message box shortly after the click to prevent accidental clicks + QTimer::singleShot(5000, &msgBox, SLOT(reject())); + + // Exit application + if (ret == QMessageBox::Yes) + { + qDebug() << "EXITING"; + mainWindow->close(); + this->exit(EXIT_SUCCESS); + } + } + + MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(MG::DIR::getSupportFilesDirectory() + "/demo-log.txt"); + mainWindow->addLink(simulationLink); + + //CommConfigurationWindow* simulationWidget = new CommConfigurationWindow(simulationLink, mavlink, this); + //ui.menuNetwork->addAction(commWidget->getAction()); + //simulationLink->connect(); + + } /** diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 19e9e0d6700ebfc56aca83226735fee0b0f8d4cb..abb98452d9656428d61ec260cf6aadab0ddc72ad 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -174,7 +174,7 @@ void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message // Create buffer uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; // Write message into buffer, prepending start sign - int len = message_to_send_buffer(buffer, &message); + int len = mavlink_msg_to_send_buffer(buffer, &message); // If link is connected if (link->isConnected()) { @@ -193,7 +193,7 @@ void MAVLinkProtocol::sendHeartbeat() if (m_heartbeatsEnabled) { mavlink_message_t beat; - message_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU); + mavlink_msg_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU); sendMessage(beat); } } diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index a8aa4e0949f03d507f603f3f0b57caeb04b3dc92..84986e6be9cd6c7275739c6db9f137de2c076007 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -125,7 +125,7 @@ void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_mes { // Allocate buffer with packet data uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - unsigned int bufferlength = message_to_send_buffer(buf, msg); + unsigned int bufferlength = mavlink_msg_to_send_buffer(buf, msg); //add data into datastream memcpy(stream+(*index),buf, bufferlength); (*index) += bufferlength; @@ -149,9 +149,9 @@ void MAVLinkSimulationLink::mainloop() static float voltage = fullVoltage; static float drainRate = 0.025; // x.xx% of the capacity is linearly drained per second - attitude_t attitude; - raw_aux_t rawAuxValues; - raw_imu_t rawImuValues; + mavlink_attitude_t attitude; + mavlink_raw_aux_t rawAuxValues; + mavlink_raw_imu_t rawImuValues; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; int bufferlength; @@ -176,9 +176,9 @@ void MAVLinkSimulationLink::mainloop() { // BOOT // Pack message and get size of encoded byte string - messageSize = message_boot_pack(systemId, componentId, &msg, version); + messageSize = mavlink_msg_boot_pack(systemId, componentId, &msg, version); // Allocate buffer with packet data - bufferlength = message_to_send_buffer(buffer, &msg); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; @@ -302,9 +302,9 @@ void MAVLinkSimulationLink::mainloop() // ATTITUDE attitude.msec = time; // Pack message and get size of encoded byte string - message_attitude_encode(systemId, componentId, &msg, &attitude); + mavlink_msg_attitude_encode(systemId, componentId, &msg, &attitude); // Allocate buffer with packet data - bufferlength = message_to_send_buffer(buffer, &msg); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; @@ -315,9 +315,9 @@ void MAVLinkSimulationLink::mainloop() rawImuValues.ymag = 0; rawImuValues.zmag = 0; // Pack message and get size of encoded byte string - message_raw_imu_encode(systemId, componentId, &msg, &rawImuValues); + mavlink_msg_raw_imu_encode(systemId, componentId, &msg, &rawImuValues); // Allocate buffer with packet data - bufferlength = message_to_send_buffer(buffer, &msg); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; @@ -355,27 +355,27 @@ void MAVLinkSimulationLink::mainloop() status.vbat = voltage * 1000; // millivolts // Pack message and get size of encoded byte string - messageSize = message_sys_status_encode(systemId, componentId, &msg, &status); + messageSize = mavlink_msg_sys_status_encode(systemId, componentId, &msg, &status); // Allocate buffer with packet data - bufferlength = message_to_send_buffer(buffer, &msg); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; // Pack debug text message - statustext_t text; + mavlink_statustext_t text; text.severity = 0; strcpy((char*)(text.text), "DEBUG MESSAGE TEXT"); - message_statustext_encode(systemId, componentId, &msg, &text); - bufferlength = message_to_send_buffer(buffer, &msg); + mavlink_msg_statustext_encode(systemId, componentId, &msg, &text); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); memcpy(stream+streampointer, buffer, bufferlength); streampointer += bufferlength; /* // Pack message and get size of encoded byte string - messageSize = message_boot_pack(systemId, componentId, &msg, version); + messageSize = mavlink_msg_boot_pack(systemId, componentId, &msg, version); // Allocate buffer with packet data - bufferlength = message_to_send_buffer(buffer, &msg); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength;*/ @@ -387,9 +387,9 @@ void MAVLinkSimulationLink::mainloop() typeCounter++; // Pack message and get size of encoded byte string - messageSize = message_heartbeat_pack(systemId, componentId, &msg, mavType); + messageSize = mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType); // Allocate buffer with packet data - bufferlength = message_to_send_buffer(buffer, &msg); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; @@ -399,9 +399,9 @@ void MAVLinkSimulationLink::mainloop() // HEARTBEAT VEHICLE 2 // Pack message and get size of encoded byte string - messageSize = message_heartbeat_pack(42, componentId, &msg, MAV_FIXED_WING); + messageSize = mavlink_msg_heartbeat_pack(42, componentId, &msg, MAV_FIXED_WING); // Allocate buffer with packet data - bufferlength = message_to_send_buffer(buffer, &msg); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; @@ -413,9 +413,9 @@ void MAVLinkSimulationLink::mainloop() status2.status = MAV_STATE_STANDBY; // Pack message and get size of encoded byte string - messageSize = message_sys_status_encode(systemId, componentId, &msg, &status); + messageSize = mavlink_msg_sys_status_encode(systemId, componentId, &msg, &status); // Allocate buffer with packet data - bufferlength = message_to_send_buffer(buffer, &msg); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; @@ -441,9 +441,9 @@ void MAVLinkSimulationLink::mainloop() // Attitude // Pack message and get size of encoded byte string - messageSize = message_attitude_pack(systemId, componentId, &msg, usec, roll, pitch, yaw, 0, 0, 0); + messageSize = mavlink_msg_attitude_pack(systemId, componentId, &msg, usec, roll, pitch, yaw, 0, 0, 0); // Allocate buffer with packet data - bufferlength = message_to_send_buffer(buffer, &msg); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; @@ -482,11 +482,10 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) // Parse bytes mavlink_message_t msg; - mavlink_status_t comm; + mavlink_status_t comm = {0}; uint8_t stream[2048]; int streampointer = 0; - mavlink_message_t ret; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; int bufferlength = 0; @@ -503,16 +502,16 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) // SET THE SYSTEM MODE case MAVLINK_MSG_ID_SET_MODE: { - set_mode_t mode; - message_set_mode_decode(&msg, &mode); + mavlink_set_mode_t mode; + mavlink_msg_set_mode_decode(&msg, &mode); // Set mode indepent of mode.target status.mode = mode.mode; } // EXECUTE OPERATOR ACTIONS case MAVLINK_MSG_ID_ACTION: { - action_t action; - message_action_decode(&msg, &action); + mavlink_action_t action; + mavlink_msg_action_decode(&msg, &action); switch (action.action) { case MAV_ACTION_LAUNCH: @@ -544,25 +543,43 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) case MAVLINK_MSG_ID_MANUAL_CONTROL: { - manual_control_t control; - message_manual_control_decode(&msg, &control); + mavlink_manual_control_t control; + mavlink_msg_manual_control_decode(&msg, &control); qDebug() << "\n" << "ROLL:" << control.roll << "PITCH:" << control.pitch; } break; case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - param_request_list_t read; - message_param_request_list_decode(&msg, &read); - // Output all params - - // Pack message and get size of encoded byte string - message_param_value_pack(systemId, componentId, &msg, (int8_t*)"ROLL_K_P", 0.5f); - // Allocate buffer with packet data - bufferlength = message_to_send_buffer(buffer, &msg); - - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer+=bufferlength; + mavlink_param_request_list_t read; + mavlink_msg_param_request_list_decode(&msg, &read); + if (read.target_system == systemId) + { + // Output all params + + // Pack message and get size of encoded byte string + mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)"ROLL_K_P", 0.5f); + // Allocate buffer with packet data + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer+=bufferlength; + + // Pack message and get size of encoded byte string + mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)"PITCH_K_P", 0.6f); + // Allocate buffer with packet data + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer+=bufferlength; + + // Pack message and get size of encoded byte string + mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)"YAW_K_P", 0.8f); + // Allocate buffer with packet data + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer+=bufferlength; + } } } diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index 067dad2227cd49e882a56c4a7a985a9364b077a4..4ce6602a3cfdf85b71ff13aa3f8b67c8f273befe 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -115,7 +115,7 @@ protected: int id; QString name; qint64 timeOffset; - sys_status_t status; + mavlink_sys_status_t status; void enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg); diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc index 74b7c4fbfa9527bd70b016edfaf03523e3128133..6653af7b1a9aa57d10ee38edb2c3e5f8c6c3d151 100644 --- a/src/comm/MAVLinkXMLParser.cc +++ b/src/comm/MAVLinkXMLParser.cc @@ -91,14 +91,15 @@ bool MAVLinkXMLParser::generate() 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 cStructName = QString("%1_t").arg(messageName); + 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 message_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn message_%1_pack(%3);\n}\n"; + 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 message_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n"; - QString pack = "static inline uint16_t message_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\tuint16_t i = 0;\n\n%4\n\treturn finalize_message(msg, system_id, component_id, i);\n}\n\n"; - QString compactSend = "#if !defined(_WIN32) && !defined(__linux) && !defined(__APPLE__)\n\n#include \"global_data.h\"\n\nstatic inline void message_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmessage_%3_pack(global_data.param[PARAM_SYSTEM_ID], global_data.param[PARAM_COMPONENT_ID], &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\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\tuint16_t i = 0;\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 = ""; @@ -135,7 +136,7 @@ bool MAVLinkXMLParser::generate() // 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()); // Add decode function for this type - decodeLines += QString("\tmessage_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); + decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); } else // Handle simple types like integers and floats @@ -148,7 +149,7 @@ bool MAVLinkXMLParser::generate() // Add pack line to message_xx_pack function packLines += QString("\ti += put_%1_by_index(%2, i, msg->payload); //%3\n").arg(fieldType, fieldName, e2.text()); // Add decode function for this type - decodeLines += QString("\t%1->%2 = message_%1_get_%2(msg);\n").arg(messageName, fieldName); + 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); @@ -184,14 +185,14 @@ bool MAVLinkXMLParser::generate() // Array handling is different from simple types if (fieldType.startsWith("array")) { - unpacking += unpackingComment + QString("static inline uint16_t message_%1_get_%2(const mavlink_message_t* msg, int8_t* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); + unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, int8_t* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); decodeLines += ""; QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); prepends += "+" + arrayLength; } else { - unpacking += unpackingComment + QString("static inline %1 message_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg(fieldType, messageName, fieldName, unpackingCode); + 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); decodeLines += ""; prepends += "+sizeof(" + e2.attribute("type", "void") + ")"; } @@ -206,7 +207,7 @@ bool MAVLinkXMLParser::generate() decode = decode.arg(messageName).arg(cStructName).arg(decodeLines); compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters); QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine + "\n\n" + cStruct + "\n\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + encode + "\n" + compactSend + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + decode; - cFiles.append(qMakePair(QString("mavlink_message_%1.h").arg(messageName), cFile)); + cFiles.append(qMakePair(QString("mavlink_msg_%1.h").arg(messageName), cFile)); } } } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 46d6409723c70a4ba786ca76382983cbb2dcdb5e..07de03641f90ae07d855a65929241e84693c8cfc 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -106,9 +106,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) case MAVLINK_MSG_ID_HEARTBEAT: emit heartbeat(this); // Set new type if it has changed - if (this->type != message_heartbeat_get_type(&message)) + if (this->type != mavlink_msg_heartbeat_get_type(&message)) { - this->type = message_heartbeat_get_type(&message); + this->type = mavlink_msg_heartbeat_get_type(&message); emit systemTypeSet(this, type); } break; @@ -119,8 +119,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) break; case MAVLINK_MSG_ID_SYS_STATUS: { - sys_status_t state; - message_sys_status_decode(&message, &state); + mavlink_sys_status_t state; + mavlink_msg_sys_status_decode(&message, &state); QString audiostring = "System " + QString::number(this->getUASID()); QString stateAudio = ""; @@ -220,16 +220,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) break; case MAVLINK_MSG_ID_AUX_STATUS: { - aux_status_t status; - message_aux_status_decode(&message, &status); + mavlink_aux_status_t status; + mavlink_msg_aux_status_decode(&message, &status); emit loadChanged(this, status.load/100.0f); emit valueChanged(this, "Load", status.load/1000.0f, MG::TIME::getGroundTimeNow()); } break; case MAVLINK_MSG_ID_RAW_IMU: { - raw_imu_t raw; - message_raw_imu_decode(&message, &raw); + mavlink_raw_imu_t raw; + mavlink_msg_raw_imu_decode(&message, &raw); quint64 time = raw.msec; if (time == 0) { @@ -257,8 +257,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) break; case MAVLINK_MSG_ID_RAW_AUX: { - raw_aux_t raw; - message_raw_aux_decode(&message, &raw); + mavlink_raw_aux_t raw; + mavlink_msg_raw_aux_decode(&message, &raw); quint64 time = MG::TIME::getGroundTimeNow();//raw.msec; if (time == 0) { @@ -278,11 +278,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) break; case MAVLINK_MSG_ID_ATTITUDE: //std::cerr << std::endl; - //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << message_attitude_get_roll(message.payload) << " pitch: " << message_attitude_get_pitch(message.payload) << " yaw: " << message_attitude_get_yaw(message.payload) << std::endl; + //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; { - attitude_t attitude; - message_attitude_decode(&message, &attitude); - quint64 time = message_attitude_get_msec(&message); + mavlink_attitude_t attitude; + mavlink_msg_attitude_decode(&message, &attitude); + quint64 time = mavlink_msg_attitude_get_msec(&message); if (time == 0) { time = MG::TIME::getGroundTimeNow(); @@ -295,24 +295,24 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } time += onboardTimeOffset; } - emit valueChanged(uasId, "roll IMU", message_attitude_get_roll(&message), time); - emit valueChanged(uasId, "pitch IMU", message_attitude_get_pitch(&message), time); - emit valueChanged(uasId, "yaw IMU", message_attitude_get_yaw(&message), time); - emit valueChanged(this, "roll IMU", message_attitude_get_roll(&message), time); - emit valueChanged(this, "pitch IMU", message_attitude_get_pitch(&message), time); - emit valueChanged(this, "yaw IMU", message_attitude_get_yaw(&message), time); + emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); + emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); + emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); + emit valueChanged(this, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); + emit valueChanged(this, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); + emit valueChanged(this, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); emit valueChanged(uasId, "rollspeed IMU", attitude.rollspeed, time); emit valueChanged(uasId, "pitchspeed IMU", attitude.pitchspeed, time); emit valueChanged(uasId, "yawspeed IMU", attitude.yawspeed, time); - emit attitudeChanged(this, message_attitude_get_roll(&message), message_attitude_get_pitch(&message), message_attitude_get_yaw(&message), time); + emit attitudeChanged(this, mavlink_msg_attitude_get_roll(&message), mavlink_msg_attitude_get_pitch(&message), mavlink_msg_attitude_get_yaw(&message), time); } break; case MAVLINK_MSG_ID_POSITION: //std::cerr << std::endl; - //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << message_attitude_get_roll(message.payload) << " pitch: " << message_attitude_get_pitch(message.payload) << " yaw: " << message_attitude_get_yaw(message.payload) << std::endl; + //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; { - position_t pos; - message_position_decode(&message, &pos); + mavlink_position_t pos; + mavlink_msg_position_decode(&message, &pos); quint64 time = pos.msec; if (time == 0) { @@ -329,9 +329,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "x", pos.x, time); emit valueChanged(uasId, "y", pos.y, time); emit valueChanged(uasId, "z", pos.z, time); - //emit valueChanged(this, "roll IMU", message_attitude_get_roll(&message), time); - //emit valueChanged(this, "pitch IMU", message_attitude_get_pitch(&message), time); - //emit valueChanged(this, "yaw IMU", message_attitude_get_yaw(&message), time); + //emit valueChanged(this, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); + //emit valueChanged(this, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); + //emit valueChanged(this, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); emit valueChanged(uasId, "vx", pos.vx, time); emit valueChanged(uasId, "vy", pos.vy, time); emit valueChanged(uasId, "vz", pos.vz, time); @@ -340,40 +340,40 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) break; case MAVLINK_MSG_ID_PARAM_VALUE: { - param_value_t value; - message_param_value_decode(&message, &value); + mavlink_param_value_t value; + mavlink_msg_param_value_decode(&message, &value); emit parameterChanged(uasId, message.compid, QString((char*)value.param_id), value.param_value); } break; case MAVLINK_MSG_ID_DEBUG: - emit valueChanged(uasId, QString("debug ") + QString::number(message_debug_get_ind(&message)), message_debug_get_value(&message), MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow()); break; /* case MAVLINK_MSG_ID_WP: - emit waypointUpdated(this->getUASID(), message_emitwaypoint_get_id(message.payload), message_emitwaypoint_get_x(message.payload), message_emitwaypoint_get_y(message.payload), message_emitwaypoint_get_z(message.payload), message_emitwaypoint_get_yaw(message.payload), (message_emitwaypoint_get_autocontinue(message.payload) == 1 ? true : false), (message_emitwaypoint_get_active(message.payload) == 1 ? true : false)); + emit waypointUpdated(this->getUASID(), mavlink_msg_emitwaypoint_get_id(message.payload), mavlink_msg_emitwaypoint_get_x(message.payload), mavlink_msg_emitwaypoint_get_y(message.payload), mavlink_msg_emitwaypoint_get_z(message.payload), mavlink_msg_emitwaypoint_get_yaw(message.payload), (message_emitwaypoint_get_autocontinue(message.payload) == 1 ? true : false), (message_emitwaypoint_get_active(message.payload) == 1 ? true : false)); break; case MAVLINK_MSG_ID_SET_POSITION: - emit valueChanged(uasId, "WP X", message_gotowaypoint_get_x(message.payload), MG::TIME::getGroundTimeNow()); - emit valueChanged(uasId, "WP Y", message_gotowaypoint_get_y(message.payload), MG::TIME::getGroundTimeNow()); - emit valueChanged(uasId, "WP Z", message_gotowaypoint_get_z(message.payload), MG::TIME::getGroundTimeNow()); - emit valueChanged(uasId, "WP speed X", message_gotowaypoint_get_speedX(message.payload), MG::TIME::getGroundTimeNow()); - emit valueChanged(uasId, "WP speed Y", message_gotowaypoint_get_speedY(message.payload), MG::TIME::getGroundTimeNow()); - emit valueChanged(uasId, "WP speed Z", message_gotowaypoint_get_speedZ(message.payload), MG::TIME::getGroundTimeNow()); - emit valueChanged(uasId, "WP yaw", message_gotowaypoint_get_yaw(message.payload)/M_PI*180.0f, MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, "WP X", mavlink_msg_gotowaypoint_get_x(message.payload), MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, "WP Y", mavlink_msg_gotowaypoint_get_y(message.payload), MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, "WP Z", mavlink_msg_gotowaypoint_get_z(message.payload), MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, "WP speed X", mavlink_msg_gotowaypoint_get_speedX(message.payload), MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, "WP speed Y", mavlink_msg_gotowaypoint_get_speedY(message.payload), MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, "WP speed Z", mavlink_msg_gotowaypoint_get_speedZ(message.payload), MG::TIME::getGroundTimeNow()); + emit valueChanged(uasId, "WP yaw", mavlink_msg_gotowaypoint_get_yaw(message.payload)/M_PI*180.0f, MG::TIME::getGroundTimeNow()); break; case MAVLINK_MSG_ID_WP_REACHED: qDebug() << "WAYPOINT REACHED"; - emit waypointReached(this, message_wp_reached_get_id(message.payload)); + emit waypointReached(this, mavlink_msg_wp_reached_get_id(message.payload)); break; */ case MAVLINK_MSG_ID_STATUSTEXT: { QByteArray b; b.resize(256); - message_statustext_get_text(&message, (int8_t*)b.data()); + mavlink_msg_statustext_get_text(&message, (int8_t*)b.data()); //b.append('\0'); QString text = QString(b); - int severity = message_statustext_get_severity(&message); + int severity = mavlink_msg_statustext_get_severity(&message); //qDebug() << "RECEIVED STATUS:" << text; //emit statusTextReceived(severity, text); emit textMessageReceived(uasId, severity, text); @@ -383,11 +383,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { QByteArray b; b.resize(256); - message_pattern_detected_get_file(&message, (int8_t*)b.data()); + mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data()); b.append('\0'); QString path = QString(b); - bool detected (message_pattern_detected_get_detected(&message) == 1 ? true : false ); - emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, message_pattern_detected_get_confidence(&message), detected); + bool detected (mavlink_msg_pattern_detected_get_detected(&message) == 1 ? true : false ); + emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, mavlink_msg_pattern_detected_get_confidence(&message), detected); } break; default: @@ -411,7 +411,7 @@ void UAS::setMode(int mode) { this->mode = mode; mavlink_message_t msg; - message_set_mode_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, getUASID(), (unsigned char)mode); + mavlink_msg_set_mode_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, getUASID(), (unsigned char)mode); sendMessage(msg); } } @@ -438,7 +438,7 @@ void UAS::sendMessage(LinkInterface* link, mavlink_message_t message) // Create buffer uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; // Write message into buffer, prepending start sign - int len = message_to_send_buffer(buffer, &message); + int len = mavlink_msg_to_send_buffer(buffer, &message); // If link is connected if (link->isConnected()) { @@ -547,7 +547,7 @@ void UAS::requestWaypoints() void UAS::requestParameters() { mavlink_message_t msg; - message_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg); + mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg); sendMessage(msg); } @@ -559,7 +559,7 @@ void UAS::launch() { mavlink_message_t message; // TODO Replace MG System ID with static function call and allow to change ID in GUI - message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_LAUNCH); + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_LAUNCH); sendMessage(message); qDebug() << "UAS LAUNCHED!"; //emit commandSent(LAUNCH); @@ -573,7 +573,7 @@ void UAS::enable_motors() { mavlink_message_t message; // TODO Replace MG System ID with static function call and allow to change ID in GUI - message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_MOTORS_START); + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_MOTORS_START); sendMessage(message); } @@ -585,7 +585,7 @@ void UAS::disable_motors() { mavlink_message_t message; // TODO Replace MG System ID with static function call and allow to change ID in GUI - message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_MOTORS_STOP); + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_MOTORS_STOP); sendMessage(message); } @@ -604,7 +604,7 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double if(mode == MAV_MODE_MANUAL) { mavlink_message_t message; - message_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual); + mavlink_msg_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual); sendMessage(message); qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust; @@ -654,7 +654,7 @@ void UAS::halt() { mavlink_message_t message; // TODO Replace MG System ID with static function call and allow to change ID in GUI - message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_HALT); + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_HALT); sendMessage(message); } @@ -662,7 +662,7 @@ void UAS::go() { mavlink_message_t message; // TODO Replace MG System ID with static function call and allow to change ID in GUI - message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_CONTINUE); + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_CONTINUE); sendMessage(message); } @@ -671,7 +671,7 @@ void UAS::home() { mavlink_message_t message; // TODO Replace MG System ID with static function call and allow to change ID in GUI - message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_RETURN); + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_RETURN); sendMessage(message); } @@ -683,7 +683,7 @@ void UAS::emergencySTOP() { mavlink_message_t message; // TODO Replace MG System ID with static function call and allow to change ID in GUI - message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_EMCY_LAND); + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_EMCY_LAND); } /** @@ -711,7 +711,7 @@ bool UAS::emergencyKILL() { mavlink_message_t message; // TODO Replace MG System ID with static function call and allow to change ID in GUI - message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_EMCY_KILL); + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_EMCY_KILL); sendMessage(message); result = true; } @@ -738,7 +738,7 @@ void UAS::shutdown() // If the active UAS is set, execute command mavlink_message_t message; // TODO Replace MG System ID with static function call and allow to change ID in GUI - message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_SHUTDOWN); + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_SHUTDOWN); sendMessage(message); result = true; } diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 0f5b46e463c4dfa7b39ad43445f4f085408f1e4e..c30c182243630e2df54d3bd014598187a06e2b50 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -155,42 +155,6 @@ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent) joystick = new JoystickInput(); - // HAS TO BE THE LAST ACTION - // to make sure that all components are initialized when the - // first messages arrive - udpLink = new UDPLink(QHostAddress::Any, 14550); - LinkManager::instance()->addProtocol(udpLink, mavlink); - CommConfigurationWindow* commWidget = new CommConfigurationWindow(udpLink, mavlink, this); - ui.menuNetwork->addAction(commWidget->getAction()); - - // Check if link could be connected - if (!udpLink->connect()) - { - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText("Could not connect UDP port. Is already an instance of " + qAppName() + " running?"); - msgBox.setInformativeText("Do you want to close the application?"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::Cancel); - int ret = msgBox.exec(); - - // Close the message box shortly after the click to prevent accidental clicks - QTimer::singleShot(5000, &msgBox, SLOT(reject())); - - // Exit application - if (ret == QMessageBox::Yes) - { - qApp->exit(EXIT_SUCCESS); - } - } - - simulationLink = new MAVLinkSimulationLink(MG::DIR::getSupportFilesDirectory() + "/demo-log.txt"); - connect(simulationLink, SIGNAL(valueChanged(int,QString,double,quint64)), linechart, SLOT(appendData(int,QString,double,quint64))); - LinkManager::instance()->addProtocol(simulationLink, mavlink); - //CommConfigurationWindow* simulationWidget = new CommConfigurationWindow(simulationLink, mavlink, this); - //ui.menuNetwork->addAction(commWidget->getAction()); - //simulationLink->connect(); - // Create actions connectActions(); @@ -309,7 +273,6 @@ void MainWindow::connectActions() // Joystick configuration connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); - connect(ui.actionSimulate, SIGNAL(triggered(bool)), simulationLink, SLOT(connectLink(bool))); } void MainWindow::configure() @@ -333,6 +296,21 @@ void MainWindow::addLink() // TODO Implement the link removal! } +void MainWindow::addLink(LinkInterface *link) +{ + CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this); + ui.menuNetwork->addAction(commWidget->getAction()); + LinkManager::instance()->addProtocol(link, mavlink); + + // Special case for simulationlink + MAVLinkSimulationLink* sim = dynamic_cast(link); + if (sim) + { + connect(sim, SIGNAL(valueChanged(int,QString,double,quint64)), linechart, SLOT(appendData(int,QString,double,quint64))); + connect(ui.actionSimulate, SIGNAL(triggered(bool)), sim, SLOT(connectLink(bool))); + } +} + void MainWindow::UASCreated(UASInterface* uas) { // Connect the UAS to the full user interface diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 661cd51e2941a51f9d81d044a9e96ebe08ce73f3..4ea40db47decb13d3046d55d590ca2cb522ac326 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -113,6 +113,7 @@ public slots: void setLastAction(QString status); void setLinkStatus(QString status); void addLink(); + void addLink(LinkInterface* link); void configure(); void UASCreated(UASInterface* uas); void startVideoCapture(); diff --git a/src/ui/ParameterInterface.cc b/src/ui/ParameterInterface.cc index d977e47be5b0b1a3e41ba4e12fcf5f81fbb9270e..469d8f218f22254c64aa3b674e9d51b0be60aa2d 100644 --- a/src/ui/ParameterInterface.cc +++ b/src/ui/ParameterInterface.cc @@ -9,29 +9,18 @@ ParameterInterface::ParameterInterface(QWidget *parent) : QWidget(parent), + paramWidgets(new QMap()), + curr(-1), m_ui(new Ui::parameterWidget) { m_ui->setupUi(this); - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*))); - - components = new QMap(); - tree = new ParamTreeModel(); - - //treeView = new QTreeView(this); - //treeView->setModel(tree); - - treeWidget = new QTreeWidget(this); - QStringList headerItems; - headerItems.append("Parameter"); - headerItems.append("Value"); - treeWidget->setHeaderLabels(headerItems); - QStackedWidget* stack = m_ui->stackedWidget; - //stack->addWidget(treeView); - //stack->setCurrentWidget(treeView); - stack->addWidget(treeWidget); - stack->setCurrentWidget(treeWidget); + // Setup UI connections + connect(m_ui->vehicleComboBox, SIGNAL(activated(int)), this, SLOT(selectUAS(int))); + connect(m_ui->readParamsButton, SIGNAL(clicked()), this, SLOT(requestParameterList())); + // Setup MAV connections + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*))); } ParameterInterface::~ParameterInterface() @@ -39,6 +28,12 @@ ParameterInterface::~ParameterInterface() delete m_ui; } +void ParameterInterface::selectUAS(int index) +{ + m_ui->stackedWidget->setCurrentIndex(index); + curr = index; +} + /** * * @param uas System to add to list @@ -47,26 +42,31 @@ void ParameterInterface::addUAS(UASInterface* uas) { m_ui->vehicleComboBox->addItem(uas->getUASName()); - mav = uas; - - // Setup UI connections - connect(m_ui->readParamsButton, SIGNAL(clicked()), this, SLOT(requestParameterList())); + QGCParamWidget* param = new QGCParamWidget(uas, this); + paramWidgets->insert(uas->getUASID(), param); + m_ui->stackedWidget->addWidget(param); + if (curr == -1) + { + m_ui->stackedWidget->setCurrentWidget(param); + curr = uas->getUASID(); + qDebug() << "first widget"; + } // Connect signals - connect(uas, SIGNAL(parameterChanged(int,int,QString,float)), this, SLOT(receiveParameter(int,int,QString,float))); - //if (!paramViews.contains(uas)) - //{ - //uasViews.insert(uas, new UASView(uas, this)); - //listLayout->addWidget(uasViews.value(uas)); - - //} + connect(uas, SIGNAL(parameterChanged(int,int,QString,float)), this, SLOT(addParameter(int,int,QString,float))); } void ParameterInterface::requestParameterList() { - mav->requestParameters(); - // Clear view - treeWidget->clear(); + UASInterface* mav; + QGCParamWidget* widget = paramWidgets->value(curr); + if (widget != NULL) + { + mav = widget->getUAS(); + mav->requestParameters(); + // Clear view + widget->clear(); + } } /** @@ -77,34 +77,20 @@ void ParameterInterface::requestParameterList() */ void ParameterInterface::addComponent(int uas, int component, QString componentName) { - Q_UNUSED(uas); - QStringList list; - list.append(componentName); - list.append(QString::number(component)); - QTreeWidgetItem* comp = new QTreeWidgetItem(list); - bool updated = false; - if (components->contains(component)) updated = true; - components->insert(component, comp); - if (!updated) treeWidget->addTopLevelItem(comp); + QGCParamWidget* widget = paramWidgets->value(uas); + if (widget != NULL) + { + widget->addComponent(component, componentName); + } } -void ParameterInterface::receiveParameter(int uas, int component, QString parameterName, float value) +void ParameterInterface::addParameter(int uas, int component, QString parameterName, float value) { - Q_UNUSED(uas); - // Insert parameter into map - //tree->appendParam(component, parameterName, value); - QStringList plist; - plist.append(parameterName); - plist.append(QString::number(value)); - QTreeWidgetItem* item = new QTreeWidgetItem(plist); - - // Get component - if (!components->contains(component)) + QGCParamWidget* widget = paramWidgets->value(uas); + if (widget != NULL) { - addComponent(uas, component, "Component #" + QString::number(component)); + widget->addParameter(component, parameterName, value); } - components->value(component)->addChild(item); - //treeWidget->addTopLevelItem(new QTreeWidgetItem(list)); } /** @@ -126,6 +112,12 @@ void ParameterInterface::commitParameter(UASInterface* uas, int component, QStri } +/* +void ParameterInterface::commitParameters(UASInterface* uas) +{ + +}*/ + /** * */ diff --git a/src/ui/ParameterInterface.h b/src/ui/ParameterInterface.h index 57f1657839bf6d7c89f448c03a06cbb4b3700d80..f8848e5d7046f307886d5af1b0d0d69337cf75fa 100644 --- a/src/ui/ParameterInterface.h +++ b/src/ui/ParameterInterface.h @@ -8,6 +8,7 @@ #include "ui_ParameterInterface.h" #include "UASInterface.h" #include "ParamTreeModel.h" +#include "QGCParamWidget.h" namespace Ui { class ParameterInterface; @@ -21,20 +22,17 @@ public: public slots: void addUAS(UASInterface* uas); + void selectUAS(int index); void addComponent(int uas, int component, QString componentName); - void receiveParameter(int uas, int component, QString parameterName, float value); + void addParameter(int uas, int component, QString parameterName, float value); void requestParameterList(); void setParameter(UASInterface* uas, int component, QString parameterName, float value); void commitParameter(UASInterface* uas, int component, QString parameterName, float value); protected: virtual void changeEvent(QEvent *e); - - UASInterface* mav; - ParamTreeModel* tree; - QTreeView* treeView; - QTreeWidget* treeWidget; - QMap* components; + QMap* paramWidgets; + int curr; private: Ui::parameterWidget *m_ui; diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc new file mode 100644 index 0000000000000000000000000000000000000000..25f34419475f3b9f10a63aef61696bc13fd7b2a1 --- /dev/null +++ b/src/ui/QGCParamWidget.cc @@ -0,0 +1,82 @@ +#include + +#include "QGCParamWidget.h" +#include "UASInterface.h" +#include + +QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : + QWidget(parent), + mav(uas), + components(new QMap()) +{ + // Create tree widget + tree = new QTreeWidget(this); + + // Set tree widget as widget onto this component + QHBoxLayout* horizontalLayout; + //form->setAutoFillBackground(false); + horizontalLayout = new QHBoxLayout(this); + horizontalLayout->setSpacing(0); + horizontalLayout->setMargin(0); + horizontalLayout->setSizeConstraint(QLayout::SetMinimumSize); + + horizontalLayout->addWidget(tree); + this->setLayout(horizontalLayout); + + // Set header + QStringList headerItems; + headerItems.append("Parameter"); + headerItems.append("Value"); + tree->setHeaderLabels(headerItems); + tree->setColumnCount(2); +} + +UASInterface* QGCParamWidget::getUAS() +{ + return mav; +} + +/** + * + * @param uas System which has the component + * @param component id of the component + * @param componentName human friendly name of the component + */ +void QGCParamWidget::addComponent(int component, QString componentName) +{ + QStringList list; + list.append(componentName); + list.append(QString::number(component)); + QTreeWidgetItem* comp = new QTreeWidgetItem(list); + bool updated = false; + if (components->contains(component)) updated = true; + components->insert(component, comp); + if (!updated) + { + tree->addTopLevelItem(comp); + tree->update(); + } +} + +void QGCParamWidget::addParameter(int component, QString parameterName, float value) +{ + // Insert parameter into map + //tree->appendParam(component, parameterName, value); + QStringList plist; + plist.append(parameterName); + plist.append(QString::number(value)); + QTreeWidgetItem* item = new QTreeWidgetItem(plist); + + // Get component + if (!components->contains(component)) + { + addComponent(component, "Component #" + QString::number(component)); + } + components->value(component)->addChild(item); + tree->update(); +} + +void QGCParamWidget::clear() +{ + tree->clear(); +} diff --git a/src/ui/QGCParamWidget.h b/src/ui/QGCParamWidget.h new file mode 100644 index 0000000000000000000000000000000000000000..87c56b3874263cc284f20e64595836f7ad670edb --- /dev/null +++ b/src/ui/QGCParamWidget.h @@ -0,0 +1,30 @@ +#ifndef QGCPARAMWIDGET_H +#define QGCPARAMWIDGET_H + +#include +#include +#include +#include + +#include "UASInterface.h" + +class QGCParamWidget : public QWidget +{ +Q_OBJECT +public: + explicit QGCParamWidget(UASInterface* uas, QWidget *parent = 0); + UASInterface* getUAS(); +signals: + +public slots: + void addComponent(int component, QString componentName); + void addParameter(int component, QString parameterName, float value); + void clear(); +protected: + UASInterface* mav; + QTreeWidget* tree; + QMap* components; + +}; + +#endif // QGCPARAMWIDGET_H diff --git a/src/ui/uas/UASInfoWidget.cc b/src/ui/uas/UASInfoWidget.cc index 509f1c27664039b4dc02c79aa49fbe75712901fa..a39b49c7a26e2abf5150048ee29e1ba39ce28a66 100644 --- a/src/ui/uas/UASInfoWidget.cc +++ b/src/ui/uas/UASInfoWidget.cc @@ -83,10 +83,8 @@ void UASInfoWidget::addUAS(UASInterface* uas) { if (uas != NULL) { - // connect(uas, SIGNAL(voltageChanged(int, double)), this, SLOT(setVoltage(int, double))); connect(uas, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBattery(UASInterface*,double,double,int))); - connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(valueChanged(int,QString,double,quint64))); - connect(uas, SIGNAL(actuatorChanged(UASInterface*,int,double)), this, SLOT(actuatorChanged(UASInterface*,int,double))); + connect(uas, SIGNAL(dropRateChanged(int,float,float)), this, SLOT(updateDropRate(int,float,float))); connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateCPULoad(UASInterface*,double))); // Set this UAS as active if it is the first one @@ -99,33 +97,6 @@ void UASInfoWidget::setActiveUAS(UASInterface* uas) activeUAS = uas; } -/* -void UASInfoWidget::actuatorChanged(UASInterface* uas, int actId, double value) -{ - if (activeUAS == uas) - { - switch (actId) - { - case 0: - ui.topRotorLabel->setText(QString::number(value*3300, 'f', 2)); - ui.topRotorBar->setValue(value * 100); - break; - case 1: - ui.botRotorLabel->setText(QString::number(value*3300, 'f', 2)); - ui.botRotorBar->setValue(value * 100); - break; - case 2: - ui.leftServoLabel->setText(QString::number(value*57.2957795f, 'f', 2)); - ui.leftServoBar->setValue((value * 50.0f) + 50); - break; - case 3: - ui.rightServoLabel->setText(QString::number(value*57.2957795f, 'f', 2)); - ui.rightServoBar->setValue((value * 50.0f) + 50); - break; - } - } -}*/ - void UASInfoWidget::updateBattery(UASInterface* uas, double voltage, double percent, int seconds) { setVoltage(uas, voltage); @@ -147,67 +118,18 @@ void UASInfoWidget::updateCPULoad(UASInterface* uas, double load) void UASInfoWidget::updateDropRate(int sysId, float receiveDrop, float sendDrop) { Q_UNUSED(sysId); - ui.receiveLossBar->setValue(receiveDrop * 100.0f); - ui.sendLossBar->setValue(sendDrop * 100.0f); + ui.receiveLossBar->setValue(receiveDrop); + ui.receiveLossLabel->setText(QString::number(receiveDrop) + "%"); + ui.sendLossBar->setValue(sendDrop); + ui.sendLossLabel->setText(QString::number(receiveDrop) + "%"); } -//void UASInfoWidget::setBattery(int uasid, BatteryType type, int cells) -//{ -// this->batteryType = type; -// this->cells = cells; -// switch (batteryType) -// { -// case NICD: -// break; -// case NIMH: -// break; -// case LIION: -// break; -// case LIPOLY: -// fullVoltage = this->cells * 4.18; -// emptyVoltage = this->cells * 3.4; -// break; -// case LIFE: -// break; -// case AGZN: -// break; -// } -//} - -//double UASInfoWidget::calculateTimeRemaining() { -// quint64 dt = MG::TIME::getGroundTimeNow() - startTime; -// double seconds = dt / 1000.0f; -// double voltDifference = startVoltage - currentVoltage; -// if (voltDifference <= 0) voltDifference = 0.00000000001f; -// double dischargePerSecond = voltDifference / seconds; -// double remaining = (currentVoltage - emptyVoltage) / dischargePerSecond; -// // Can never be below 0 -// if (remaining <= 0) remaining = 0.0000000000001f; -// return remaining; -//} - void UASInfoWidget::setVoltage(UASInterface* uas, double voltage) { Q_UNUSED(uas); this->voltage = voltage; } -//void UASInfoWidget::setVoltage(int uasid, double voltage) -//{ -// // Read and update data -// currentVoltage = voltage; -// if (startVoltage == 0) startVoltage = currentVoltage; -// // This is a low pass filter to get smoother results: (0.8 * lastChargeLevel) + (0.2 * chargeLevel) -// double chargeLevel = (currentVoltage - emptyVoltage)/(fullVoltage - emptyVoltage); -// lastChargeLevel = (0.6 * lastChargeLevel) + (0.4 * chargeLevel); -// -// lastRemainingTime = calculateTimeRemaining(); -// -// ui.voltageLabel->setText(QString::number(currentVoltage, 'f', voltageDecimals)); -// setChargeLevel(0, lastChargeLevel * 100); -// setTimeRemaining(0, lastRemainingTime); -//} - void UASInfoWidget::setChargeLevel(UASInterface* uas, double chargeLevel) { if (activeUAS == uas) @@ -231,20 +153,4 @@ void UASInfoWidget::refresh() ui.loadLabel->setText(QString::number(this->load, 'f', loadDecimals)); ui.loadBar->setValue(static_cast(this->load)); - - // if(this->timeRemaining > 1 && this->timeRemaining < MG::MAX_FLIGHT_TIME) - // { - // // Filter output to get a higher stability - // static int filterTime = static_cast(this->timeRemaining); - // //filterTime = 0.8 * filterTime + 0.2 * static_cast(this->timeRemaining); - // - // int hours = filterTime % (60 * 60); - // int min = (filterTime - hours * 60) % 60; - // int sec = (filterTime - hours * 60 - min * 60); - // QString timeText; - // timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec); - // ui.voltageTimeEstimateLabel->setText(timeText); - // } else { - // ui.voltageTimeEstimateLabel->setText(tr("Calculating")); - // } }