From 75b86d828f480f20397f5d3b11dbb85a553e70ba Mon Sep 17 00:00:00 2001 From: lm Date: Fri, 11 Feb 2011 11:32:24 +0100 Subject: [PATCH] Added a number of fixes --- src/comm/MAVLinkSimulationMAV.cc | 14 +++-- src/uas/UAS.cc | 34 +++++++++++- src/uas/UASInterface.h | 1 + src/ui/QGCParamWidget.cc | 116 +++++++++++++++++++++++++++++++++------ src/ui/QGCParamWidget.h | 14 ++++- 5 files changed, 153 insertions(+), 26 deletions(-) diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 416d37b..c32cac7 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -317,21 +317,25 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) mavlink_msg_action_decode(&msg, &action); if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) { + mavlink_action_ack_t ack; + ack.action = action.action; switch (action.action) { case MAV_ACTION_TAKEOFF: flying = true; + ack.result = 1; break; default: { - mavlink_statustext_t text; - mavlink_message_t r_msg; - sprintf((char*)text.text, "MAV%d ignored unknown action %d", systemid, action.action); - mavlink_msg_statustext_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &text); - link->sendMAVLinkMessage(&r_msg); + ack.result = 0; } break; } + + // Give feedback about action + mavlink_message_t r_msg; + mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack); + link->sendMAVLinkMessage(&r_msg); } } break; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index baf152b..1de78ad 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -256,7 +256,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } emit loadChanged(this,state.load/10.0f); - emit valueChanged(uasId, "Load", "%", ((float)state.load)/1000.0f, getUnixTime()); + emit valueChanged(uasId, "Load", "%", ((float)state.load)/10.0f, getUnixTime()); if (this->mode != static_cast(state.mode)) { @@ -369,7 +369,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit gpsLocalizationChanged(this, status.gps_fix); } break; - #endif // PIXHAWK +#endif // PIXHAWK case MAVLINK_MSG_ID_RAW_IMU: { mavlink_raw_imu_t raw; @@ -387,6 +387,23 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "mag z", "raw", static_cast(raw.zmag), time); } break; + case MAVLINK_MSG_ID_SCALED_IMU: + { + mavlink_scaled_imu_t scaled; + mavlink_msg_scaled_imu_decode(&message, &scaled); + quint64 time = getUnixTime(scaled.usec); + + emit valueChanged(uasId, "accel x", "g", scaled.xacc/1000.0f, time); + emit valueChanged(uasId, "accel y", "g", scaled.yacc/1000.0f, time); + emit valueChanged(uasId, "accel z", "g", scaled.zacc/1000.0f, time); + emit valueChanged(uasId, "gyro roll", "rad/s", scaled.xgyro/1000.0f, time); + emit valueChanged(uasId, "gyro pitch", "rad/s", scaled.ygyro/1000.0f, time); + emit valueChanged(uasId, "gyro yaw", "rad/s", scaled.zgyro/1000.0f, time); + emit valueChanged(uasId, "mag x", "tesla", scaled.xmag/1000.0f, time); + emit valueChanged(uasId, "mag y", "tesla", scaled.ymag/1000.0f, time); + emit valueChanged(uasId, "mag z", "tesla", scaled.zmag/1000.0f, time); + } + break; case MAVLINK_MSG_ID_ATTITUDE: //std::cerr << 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; @@ -678,6 +695,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // Emit change emit parameterChanged(uasId, message.compid, parameterName, val); + emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, val); + } + break; + case MAVLINK_MSG_ID_ACTION_ACK: + mavlink_action_ack_t ack; + mavlink_msg_action_ack_decode(&message, &ack); + if (ack.result == 1) + { + emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed action: %1").arg(ack.action)); + } + else + { + emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Rejected action: %1").arg(ack.action)); } break; case MAVLINK_MSG_ID_DEBUG: diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index c7dd112..e0d7978 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -369,6 +369,7 @@ signals: void waypointReached(UASInterface* uas, int id); void autoModeChanged(bool autoMode); void parameterChanged(int uas, int component, QString parameterName, float value); + void parameterChanged(int uas, int component, int parameterCount, int parameterId, QString parameterName, float value); void patternDetected(int uasId, QString patternPath, float confidence, bool detected); void letterDetected(int uasId, QString letter, float confidence, bool detected); /** diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index 513df66..4739866 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -42,14 +42,19 @@ This file is part of the QGROUNDCONTROL project */ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : QWidget(parent), + mav(uas), components(new QMap()), paramGroups(), changedValues(), - parameters() + parameters(), + transmissionListMode(false), + transmissionActive(false), + transmissionStarted(0) { // Create tree widget tree = new QTreeWidget(this); + statusLabel = new QLabel(); tree->setColumnWidth(0, 150); // Set tree widget as widget onto this component @@ -60,42 +65,50 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : horizontalLayout->setMargin(0); horizontalLayout->setSizeConstraint(QLayout::SetMinimumSize); + // Parameter tree horizontalLayout->addWidget(tree, 0, 0, 1, 3); + + // Status line + statusLabel->setText(tr("Click read to get parameters")); + horizontalLayout->addWidget(statusLabel, 1, 0, 1, 3); + + + // BUTTONS QPushButton* refreshButton = new QPushButton(tr("Refresh")); refreshButton->setToolTip(tr("Load parameters currently in non-permanent memory of aircraft.")); refreshButton->setWhatsThis(tr("Load parameters currently in non-permanent memory of aircraft.")); connect(refreshButton, SIGNAL(clicked()), this, SLOT(requestParameterList())); - horizontalLayout->addWidget(refreshButton, 1, 0); + horizontalLayout->addWidget(refreshButton, 2, 0); QPushButton* setButton = new QPushButton(tr("Transmit")); setButton->setToolTip(tr("Set current parameters in non-permanent onboard memory")); setButton->setWhatsThis(tr("Set current parameters in non-permanent onboard memory")); connect(setButton, SIGNAL(clicked()), this, SLOT(setParameters())); - horizontalLayout->addWidget(setButton, 1, 1); + horizontalLayout->addWidget(setButton, 2, 1); QPushButton* writeButton = new QPushButton(tr("Write (ROM)")); writeButton->setToolTip(tr("Copy current parameters in non-permanent memory of the aircraft to permanent memory. Transmit your parameters first to write these.")); writeButton->setWhatsThis(tr("Copy current parameters in non-permanent memory of the aircraft to permanent memory. Transmit your parameters first to write these.")); connect(writeButton, SIGNAL(clicked()), this, SLOT(writeParameters())); - horizontalLayout->addWidget(writeButton, 1, 2); - - QPushButton* readButton = new QPushButton(tr("Read (ROM)")); - readButton->setToolTip(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them.")); - readButton->setWhatsThis(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them.")); - connect(readButton, SIGNAL(clicked()), this, SLOT(readParameters())); - horizontalLayout->addWidget(readButton, 2, 2); + horizontalLayout->addWidget(writeButton, 2, 2); QPushButton* loadFileButton = new QPushButton(tr("Load File")); loadFileButton->setToolTip(tr("Load parameters from a file on this computer in the view. To write them to the aircraft, use transmit after loading them.")); loadFileButton->setWhatsThis(tr("Load parameters from a file on this computer in the view. To write them to the aircraft, use transmit after loading them.")); connect(loadFileButton, SIGNAL(clicked()), this, SLOT(loadParameters())); - horizontalLayout->addWidget(loadFileButton, 2, 0); + horizontalLayout->addWidget(loadFileButton, 3, 0); QPushButton* saveFileButton = new QPushButton(tr("Save File")); saveFileButton->setToolTip(tr("Save parameters in this view to a file on this computer.")); saveFileButton->setWhatsThis(tr("Save parameters in this view to a file on this computer.")); connect(saveFileButton, SIGNAL(clicked()), this, SLOT(saveParameters())); - horizontalLayout->addWidget(saveFileButton, 2, 1); + horizontalLayout->addWidget(saveFileButton, 3, 1); + + QPushButton* readButton = new QPushButton(tr("Read (ROM)")); + readButton->setToolTip(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them.")); + readButton->setWhatsThis(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them.")); + connect(readButton, SIGNAL(clicked()), this, SLOT(readParameters())); + horizontalLayout->addWidget(readButton, 3, 2); // Set layout this->setLayout(horizontalLayout); @@ -113,7 +126,7 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : connect(tree, SIGNAL(itemChanged(QTreeWidgetItem*,int)), this, SLOT(parameterItemChanged(QTreeWidgetItem*,int))); // New parameters from UAS - connect(uas, SIGNAL(parameterChanged(int,int,QString,float)), this, SLOT(addParameter(int,int,QString,float))); + connect(uas, SIGNAL(parameterChanged(int,int,int,int,QString,float)), this, SLOT(addParameter(int,int,int,int,QString,float))); } /** @@ -170,6 +183,67 @@ void QGCParamWidget::addComponent(int uas, int component, QString componentName) * @param component id of the component * @param parameterName human friendly name of the parameter */ +void QGCParamWidget::addParameter(int uas, int component, int paramCount, int paramId, QString parameterName, float value) +{ + addParameter(uas, component, parameterName, value); + int missCount = 0; + foreach (int key, transmissionMissingPackets.keys()) + { + missCount += transmissionMissingPackets.value(key)->count(); + } + + statusLabel->setText(tr("Got Param #%1/%2: %3 (%4 missing)").arg(paramId).arg(parameterName).arg(value).arg(missCount)); + + // List mode is different from single parameter transfers + if (transmissionListMode) + { + // Only accept the list size once on the first packet from + // each component + if (!transmissionListSizeKnown.contains(component)) + { + // If this was the first packet to announce the list size + // set all packets not received yet as missing to enforce + // retransmission until full list is received + if (!transmissionMissingPackets.contains(component)) + { + transmissionMissingPackets.insert(component, new QList()); + } + + for (int i = 0; i < paramCount; ++i) + { + if (!transmissionMissingPackets.value(component)->contains(i)) + { + transmissionMissingPackets.value(component)->append(i); + } + } + transmissionListSizeKnown.insert(component, true); + } + } + + // Mark this parameter as received + int index = transmissionMissingPackets.value(component)->indexOf(paramId); + // If the MAV sent the parameter without request, it wont be in missing list + if (index != -1) transmissionMissingPackets.value(component)->removeAt(index); + + + // Check if last parameter was received + if (transmissionMissingPackets.count() == 0) + { + this->transmissionActive = false; + this->transmissionListMode = false; + transmissionListSizeKnown.clear(); + foreach (int key, transmissionMissingPackets.keys()) + { + transmissionMissingPackets.value(key)->clear(); + } + } +} + +/** + * @param uas System which has the component + * @param component id of the component + * @param parameterName human friendly name of the parameter + */ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, float value) { Q_UNUSED(uas); @@ -298,6 +372,15 @@ void QGCParamWidget::requestParameterList() clear(); parameters.clear(); received.clear(); + // Clear transmission state + transmissionListMode = true; + transmissionListSizeKnown.clear(); + foreach (int key, transmissionMissingPackets.keys()) + { + transmissionMissingPackets.value(key)->clear(); + } + transmissionActive = true; + transmissionStarted = QGC::groundTimeUsecs(); mav->requestParameters(); } @@ -322,12 +405,13 @@ void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column) bool ok; QString str = current->data(0, Qt::DisplayRole).toString(); float value = current->data(1, Qt::DisplayRole).toDouble(&ok); - // Send parameter to MAV + // Set parameter on changed list to be transmitted to MAV if (ok) { if (ok) { - qDebug() << "PARAM CHANGED: COMP:" << key << "KEY:" << str << "VALUE:" << value; + statusLabel->setText(tr("Changed Param %1:%2: %3").arg(key).arg(str).arg(value)); + //qDebug() << "PARAM CHANGED: COMP:" << key << "KEY:" << str << "VALUE:" << value; // Changed values list if (map->contains(str)) map->remove(str); map->insert(str, value); @@ -375,7 +459,7 @@ void QGCParamWidget::saveParameters() for (j = comp->begin(); j != comp->end(); ++j) { QString paramValue("%1"); - paramValue = paramValue.arg(j.value(), 18, 'g'); + paramValue = paramValue.arg(j.value(), 25, 'g', 12); in << mav->getUASID() << "\t" << compid << "\t" << j.key() << "\t" << paramValue << "\n"; in.flush(); } diff --git a/src/ui/QGCParamWidget.h b/src/ui/QGCParamWidget.h index cf38850..f30663f 100644 --- a/src/ui/QGCParamWidget.h +++ b/src/ui/QGCParamWidget.h @@ -34,6 +34,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include "UASInterface.h" @@ -53,6 +54,8 @@ signals: public slots: /** @brief Add a component to the list */ void addComponent(int uas, int component, QString componentName); + /** @brief Add a parameter to the list with retransmission / safety checks */ + void addParameter(int uas, int component, int paramCount, int paramId, QString parameterName, float value); /** @brief Add a parameter to the list */ void addParameter(int uas, int component, QString parameterName, float value); /** @brief Request list of parameters from MAV */ @@ -75,14 +78,19 @@ public slots: /** @brief Load parameters from a file */ void loadParameters(); protected: - UASInterface* mav; ///< The MAV this widget is controlling - QTreeWidget* tree; ///< The parameter tree + UASInterface* mav; ///< The MAV this widget is controlling + QTreeWidget* tree; ///< The parameter tree + QLabel* statusLabel; ///< Parameter transmission label QMap* components; ///< The list of components QMap* > paramGroups; ///< Parameter groups QMap* > changedValues; ///< Changed values QMap* > parameters; ///< All parameters QVector received; ///< Successfully received parameters - + QMap* > transmissionMissingPackets; ///< Missing packets + bool transmissionListMode; ///< Currently requesting list + QMap transmissionListSizeKnown; ///< List size initialized? + bool transmissionActive; ///< Missing packets, working on list? + quint64 transmissionStarted; ///< Timeout }; #endif // QGCPARAMWIDGET_H