diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 435eb05..a6d82a2 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -48,7 +48,8 @@ FORMS += src/ui/MainWindow.ui \ src/ui/XMLCommProtocolWidget.ui \ src/ui/HDDisplay.ui \ src/ui/MAVLinkSettingsWidget.ui \ - src/ui/AudioOutputWidget.ui + src/ui/AudioOutputWidget.ui \ + src/ui/QGCSensorSettingsWidget.ui INCLUDEPATH += src \ src/ui \ src/ui/linechart \ @@ -113,7 +114,8 @@ HEADERS += src/MG.h \ src/LogCompressor.h \ src/ui/param/ParamTreeItem.h \ src/ui/param/ParamTreeModel.h \ - src/ui/QGCParamWidget.h + src/ui/QGCParamWidget.h \ + src/ui/QGCSensorSettingsWidget.h SOURCES += src/main.cc \ src/Core.cc \ src/uas/UASManager.cc \ @@ -161,5 +163,6 @@ SOURCES += src/main.cc \ src/LogCompressor.cc \ src/ui/param/ParamTreeItem.cc \ src/ui/param/ParamTreeModel.cc \ - src/ui/QGCParamWidget.cc + src/ui/QGCParamWidget.cc \ + src/ui/QGCSensorSettingsWidget.cc RESOURCES = mavground.qrc diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 45133c2..c453595 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -60,6 +60,14 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile this->rate = rate; _isConnected = false; + onboardParams = QMap(); + onboardParams.insert("ROLL_K_P", 0.5f); + onboardParams.insert("PITCH_K_P", 0.5f); + onboardParams.insert("YAW_K_P", 0.5f); + onboardParams.insert("XY_K_P", 0.5f); + onboardParams.insert("ALT_K_P", 0.5f); + onboardParams.insert("SYSTEM_TYPE", 1); + // Comments on the variables can be found in the header file simulationFile = new QFile(readFile, this); @@ -91,6 +99,7 @@ MAVLinkSimulationLink::~MAVLinkSimulationLink() //TODO Check destructor // fileStream->flush(); // outStream->flush(); + delete simulationFile; } void MAVLinkSimulationLink::run() @@ -482,7 +491,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) // Parse bytes mavlink_message_t msg; - mavlink_status_t comm = {0}; + mavlink_status_t comm; uint8_t stream[2048]; int streampointer = 0; @@ -519,7 +528,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) status.mode = MAV_MODE_AUTO; break; case MAV_ACTION_RETURN: - status.status = MAV_STATE_LANDING; + status.status = MAV_STATE_ACTIVE; break; case MAV_ACTION_MOTORS_START: status.status = MAV_STATE_ACTIVE; @@ -556,7 +565,21 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) if (read.target_system == systemId) { // Output all params - + // Iterate through all components, through all parameters and emit them + QMap::iterator i; + // Iterate through all components / subsystems + for (i = onboardParams.begin(); i != onboardParams.end(); ++i) + { + // Pack message and get size of encoded byte string + mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value()); + // 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*)"ROLL_K_P", 0.5f); // Allocate buffer with packet data @@ -579,11 +602,28 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); - streampointer+=bufferlength; + streampointer+=bufferlength;*/ qDebug() << "SIMULATION SENT PARAMETERS TO GCS"; } } + break; + case MAVLINK_MSG_ID_PARAM_SET: + { + qDebug() << "SIMULATION RECEIVED COMMAND TO SET PARAMETER"; + mavlink_param_set_t set; + mavlink_msg_param_set_decode(&msg, &set); + if (set.target_system == systemId) + { + QString key = QString((char*)set.param_id); + if (onboardParams.contains(key)) + { + onboardParams.remove(key); + onboardParams.insert(key, set.param_value); + } + } + } + break; } diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index 4ce6602..b6b5564 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -37,8 +37,10 @@ This file is part of the PIXHAWK project #include #include #include +#include #include #include + #include "LinkInterface.h" class MAVLinkSimulationLink : public LinkInterface @@ -116,6 +118,7 @@ protected: QString name; qint64 timeOffset; mavlink_sys_status_t status; + QMap onboardParams; 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 48e0605..19709c2 100644 --- a/src/comm/MAVLinkXMLParser.cc +++ b/src/comm/MAVLinkXMLParser.cc @@ -140,6 +140,21 @@ 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").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); } + else if (fieldType.startsWith("string")) + { + int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); + QString arrayType = fieldType.split("[").first(); + packParameters += QString(", const ") + QString("char*") + " " + fieldName; + packArguments += ", " + messageName + "->" + fieldName; + + // Add field to C structure + cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg("char", 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()); + // 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").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); + } else // Handle simple types like integers and floats { @@ -182,6 +197,10 @@ bool MAVLinkXMLParser::generate() { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(prepends, fieldType.split("[").at(1).split("]").first()); } + else if (fieldType.startsWith("string")) + { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string + 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 @@ -192,6 +211,13 @@ bool MAVLinkXMLParser::generate() QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); prepends += "+" + arrayLength; } + else if (fieldType.startsWith("string")) + { + unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, char* 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 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/uas/UAS.cc b/src/uas/UAS.cc index 8ac5e5e..6ae1fd6 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -556,8 +556,74 @@ void UAS::requestParameters() void UAS::writeParameters() { + //mavlink_message_t msg; + qDebug() << __FILE__ << __LINE__ << __func__ << "IS NOT IMPLEMENTED!"; +} + +void UAS::enableAllDataTransmission(bool enabled) +{ + // Buffers to write data to + mavlink_message_t msg; + mavlink_request_stream_t stream; + // Select the message to request from now on + // 0 is a magic ID and will enable/disable the standard message set except for heartbeat + stream.req_message_id = 0; + // Select the update rate in Hz the message should be send + // All messages will be send with their default rate + stream.req_message_rate = 0; + // Start / stop the message + stream.start_stop = (enabled) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + sendMessage(msg); +} + +void UAS::enableRawSensorDataTransmission(bool enabled) +{ + // Buffers to write data to mavlink_message_t msg; + mavlink_request_stream_t stream; + // Select the message to request from now on + stream.req_message_id = MAVLINK_MSG_ID_RAW_IMU; + // Select the update rate in Hz the message should be send + stream.req_message_rate = 200; + // Start / stop the message + stream.start_stop = (enabled) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + sendMessage(msg); +} + +void UAS::enableExtendedSystemStatusTransmission(bool enabled) +{ + // FIXME + qDebug() << __FILE__ << __LINE__ << __func__ << "IS NOT IMPLEMENTED!"; +} + +void UAS::enableRCChannelDataTransmission(bool enabled) +{ + // FIXME + qDebug() << __FILE__ << __LINE__ << __func__ << "IS NOT IMPLEMENTED!"; +} +void UAS::enableRawControllerDataTransmission(bool enabled) +{ + // FIXME + qDebug() << __FILE__ << __LINE__ << __func__ << "IS NOT IMPLEMENTED!"; +} + +void UAS::enableRawSensorFusionTransmission(bool enabled) +{ + // FIXME + qDebug() << __FILE__ << __LINE__ << __func__ << "IS NOT IMPLEMENTED!"; } void UAS::setParameter(int component, QString id, float value) @@ -570,9 +636,23 @@ void UAS::setParameter(int component, QString id, float value) // Copy string into buffer, ensuring not to exceed the buffer size char* s = (char*)id.toStdString().c_str(); - for (int i = 0; (i < id.length() && i < sizeof(p.param_id)); i++) + for (int i = 0; i < sizeof(p.param_id); i++) { - p.param_id[i] = *s; + // String characters + if (i < id.length() && i < (sizeof(p.param_id) - 1)) + { + p.param_id[i] = s[i]; + } + // Null termination at end of string or end of buffer + else if (i == id.length() || i == (sizeof(p.param_id) - 1)) + { + p.param_id[i] = '\0'; + } + // Zero fill + else + { + p.param_id[i] = 0; + } } mavlink_msg_param_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &p); sendMessage(msg); diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 21737d2..57d16c1 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -194,6 +194,13 @@ public slots: /** @brief Write parameters to permanent storage */ void writeParameters(); + void enableAllDataTransmission(bool enabled); + void enableRawSensorDataTransmission(bool enabled); + void enableExtendedSystemStatusTransmission(bool enabled); + void enableRCChannelDataTransmission(bool enabled); + void enableRawControllerDataTransmission(bool enabled); + void enableRawSensorFusionTransmission(bool enabled); + signals: /** @brief The main/battery voltage has changed/was updated */ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 3757ed0..0002a00 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -198,9 +198,13 @@ public slots: * @brief Set the current robot as focused in the user interface */ virtual void setSelected() = 0; - // TODO EMAV REMOVE - //virtual void sendMessage(LinkInterface* link, mavlink_message_t message) = 0; - //virtual void sendMessage(mavlink_message_t message) = 0; + + virtual void enableAllDataTransmission(bool enabled) = 0; + virtual void enableRawSensorDataTransmission(bool enabled) = 0; + virtual void enableExtendedSystemStatusTransmission(bool enabled) = 0; + virtual void enableRCChannelDataTransmission(bool enabled) = 0; + virtual void enableRawControllerDataTransmission(bool enabled) = 0; + virtual void enableRawSensorFusionTransmission(bool enabled) = 0; protected: QColor color; diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index d025588..8176b97 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -35,7 +35,7 @@ 0 0 1000 - 22 + 25 @@ -105,7 +105,8 @@ - :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg + :/images/categories/preferences-system.svg + :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg Settings @@ -117,7 +118,8 @@ - :/images/control/launch.svg:/images/control/launch.svg + :/images/control/launch.svg + :/images/control/launch.svg:/images/control/launch.svg Liftoff diff --git a/src/ui/ParameterInterface.cc b/src/ui/ParameterInterface.cc index 546c214..eece06d 100644 --- a/src/ui/ParameterInterface.cc +++ b/src/ui/ParameterInterface.cc @@ -4,6 +4,7 @@ #include "ParamTreeModel.h" #include "UASManager.h" #include "ui_ParameterInterface.h" +#include "QGCSensorSettingsWidget.h" #include @@ -30,6 +31,7 @@ ParameterInterface::~ParameterInterface() void ParameterInterface::selectUAS(int index) { m_ui->stackedWidget->setCurrentIndex(index); + m_ui->sensorSettings->setCurrentIndex(index); curr = index; } @@ -44,11 +46,17 @@ void ParameterInterface::addUAS(UASInterface* uas) QGCParamWidget* param = new QGCParamWidget(uas, this); paramWidgets->insert(uas->getUASID(), param); m_ui->stackedWidget->addWidget(param); + + + QGCSensorSettingsWidget* sensor = new QGCSensorSettingsWidget(uas, this); + m_ui->sensorSettings->addWidget(sensor); + + // Set widgets as default if (curr == -1) { + m_ui->sensorSettings->setCurrentWidget(param); m_ui->stackedWidget->setCurrentWidget(param); curr = uas->getUASID(); - qDebug() << "first widget"; } } diff --git a/src/ui/ParameterInterface.ui b/src/ui/ParameterInterface.ui index 7d6d62a..df9a778 100644 --- a/src/ui/ParameterInterface.ui +++ b/src/ui/ParameterInterface.ui @@ -13,108 +13,28 @@ Form - - - - - Activate Extended Output + + + + + Vehicle - - - 6 - - - - - Send RAW Sensor data - - - - - - - Send extended status - - - - - - - Send RC-values - - - - - - - Send raw controller outputs - - - - - - - - Calibration Wizards - - - - 6 - - - - - Start dynamic calibration - - - - - - - Date unknown - - - - - - - Start static calibration - - - - - - - Date unknown - - - - - + + - + Onboard Parameters - - - 6 - - - - - Vehicle - - - - - - - + + + + 0 + @@ -124,6 +44,17 @@ + + + + 0 + + + + + + + diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index f270e3a..c194fe9 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -108,9 +108,9 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, void QGCParamWidget::requestParameterList() { - mav->requestParameters(); - // Clear view + // Clear view and request param list clear(); + mav->requestParameters(); } /** @@ -127,12 +127,34 @@ void QGCParamWidget::setParameters() { //mav->setParameter(component, parameterName, value); // Iterate through all components, through all parameters and emit them - /*QMap::iterator i; + QMap::iterator i; + // Iterate through all components / subsystems for (i = components->begin(); i != components->end(); ++i) { + // Get all parameters of this component int compid = i.key(); - i.value()->children(); - }*/ + QTreeWidgetItem* item = i.value(); + for (int j = 0; j < item->childCount(); ++j) + { + QTreeWidgetItem* param = item->child(j); + // First column is name, second column value + bool ok = true; + QString key = param->data(0, Qt::DisplayRole).toString(); + float value = param->data(1, Qt::DisplayRole).toFloat(&ok); + // Send parameter to MAV + if (ok) + { + emit parameterChanged(compid, key, value); + qDebug() << "KEY:" << key << "VALUE:" << value; + } + else + { + qDebug() << __FILE__ << __LINE__ << "CONVERSION ERROR!"; + } + } + } + qDebug() << __FILE__ << __LINE__ << "SETTING ALL PARAMETERS"; + requestParameterList(); } void QGCParamWidget::writeParameters() @@ -143,4 +165,5 @@ void QGCParamWidget::writeParameters() void QGCParamWidget::clear() { tree->clear(); + components->clear(); } diff --git a/src/ui/QGCSensorSettingsWidget.cc b/src/ui/QGCSensorSettingsWidget.cc new file mode 100644 index 0000000..e3af813 --- /dev/null +++ b/src/ui/QGCSensorSettingsWidget.cc @@ -0,0 +1,32 @@ +#include "QGCSensorSettingsWidget.h" +#include "ui_QGCSensorSettingsWidget.h" + +QGCSensorSettingsWidget::QGCSensorSettingsWidget(UASInterface* uas, QWidget *parent) : + QWidget(parent), + mav(uas), + ui(new Ui::QGCSensorSettingsWidget) +{ + ui->setupUi(this); + + connect(ui->sendRawCheckBox, SIGNAL(toggled(bool)), mav, SLOT(enableRawSensorDataTransmission(bool))); + connect(ui->sendControllerCheckBox, SIGNAL(toggled(bool)), mav, SLOT(enableRawControllerDataTransmission(bool))); + connect(ui->sendExtendedCheckBox, SIGNAL(toggled(bool)), mav, SLOT(enableExtendedSystemStatusTransmission(bool))); + connect(ui->sendRCCheckBox, SIGNAL(toggled(bool)), mav, SLOT(enableRCChannelDataTransmission(bool))); +} + +QGCSensorSettingsWidget::~QGCSensorSettingsWidget() +{ + delete ui; +} + +void QGCSensorSettingsWidget::changeEvent(QEvent *e) +{ + QWidget::changeEvent(e); + switch (e->type()) { + case QEvent::LanguageChange: + ui->retranslateUi(this); + break; + default: + break; + } +} diff --git a/src/ui/QGCSensorSettingsWidget.h b/src/ui/QGCSensorSettingsWidget.h new file mode 100644 index 0000000..d19c129 --- /dev/null +++ b/src/ui/QGCSensorSettingsWidget.h @@ -0,0 +1,26 @@ +#ifndef QGCSENSORSETTINGSWIDGET_H +#define QGCSENSORSETTINGSWIDGET_H + +#include + +#include "UASInterface.h" + +namespace Ui { + class QGCSensorSettingsWidget; +} + +class QGCSensorSettingsWidget : public QWidget { + Q_OBJECT +public: + QGCSensorSettingsWidget(UASInterface* uas, QWidget *parent = 0); + ~QGCSensorSettingsWidget(); + +protected: + UASInterface* mav; + void changeEvent(QEvent *e); + +private: + Ui::QGCSensorSettingsWidget *ui; +}; + +#endif // QGCSENSORSETTINGSWIDGET_H diff --git a/src/ui/QGCSensorSettingsWidget.ui b/src/ui/QGCSensorSettingsWidget.ui new file mode 100644 index 0000000..19d7665 --- /dev/null +++ b/src/ui/QGCSensorSettingsWidget.ui @@ -0,0 +1,101 @@ + + + QGCSensorSettingsWidget + + + + 0 + 0 + 350 + 545 + + + + Form + + + + + + Activate Extended Output + + + + 6 + + + + + Send RAW Sensor data + + + + + + + Send extended status + + + + + + + Send RC-values + + + + + + + Send raw controller outputs + + + + + + + + + + Calibration Wizards + + + + 6 + + + + + Start dynamic calibration + + + + + + + Date unknown + + + + + + + Start static calibration + + + + + + + Date unknown + + + + + + + + + + +