From 80f4f133a0fff71dcc8f52bf6503e1b733428f7b Mon Sep 17 00:00:00 2001 From: treymarc Date: Tue, 29 Apr 2014 17:59:30 +0200 Subject: [PATCH] remove duplicate code --- src/uas/UAS.cc | 175 ++++++++++++++++++------------------------------- 1 file changed, 63 insertions(+), 112 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 7c1453746..018611c23 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2551,125 +2551,76 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, parameters.insert(compId, new QMap()); } - // Insert parameter into registry - if (parameters.value(compId)->contains(paramName)) { - parameters.value(compId)->remove(paramName); - } QVariant param; // Insert with correct type // TODO: This is a hack for MAV_AUTOPILOT_ARDUPILOTMEGA until the new version of MAVLink and a fix for their param handling. - switch (rawValue.param_type) - { - case MAV_PARAM_TYPE_REAL32: - { - if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - param = QVariant(paramValue.param_float); - } - else { - param = QVariant(paramValue.param_float); - } - parameters.value(compId)->insert(paramName, param); - // Emit change - emit parameterChanged(uasId, compId, paramName, param); - emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); -// qDebug() << "RECEIVED PARAM:" << param; - } - break; - case MAV_PARAM_TYPE_UINT8: - { - if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - param = QVariant(QChar((unsigned char)paramValue.param_float)); - } - else { - param = QVariant(QChar((quint8)paramValue.param_float)); - } - parameters.value(compId)->insert(paramName, param); - // Emit change - emit parameterChanged(uasId, compId, paramName, param); - emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); - //qDebug() << "RECEIVED PARAM:" << param; - } - break; - case MAV_PARAM_TYPE_INT8: - { - if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - param = QVariant(QChar((char)paramValue.param_float)); - } - else { - param = QVariant(QChar((qint8)paramValue.param_float)); - } - parameters.value(compId)->insert(paramName, param); - // Emit change - emit parameterChanged(uasId, compId, paramName, param); - emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); - //qDebug() << "RECEIVED PARAM:" << param; - } - break; - case MAV_PARAM_TYPE_INT16: - { - if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - param = QVariant((short)paramValue.param_float); - } - else { - param = QVariant((qint16)paramValue.param_float); - } - parameters.value(compId)->insert(paramName, param); - // Emit change - emit parameterChanged(uasId, compId, paramName, param); - emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); - //qDebug() << "RECEIVED PARAM:" << param; - } - break; - case MAV_PARAM_TYPE_UINT16: - { - if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - param = QVariant((unsigned short)paramValue.param_float); - } - else { - param = QVariant((quint16)paramValue.param_float); - } - parameters.value(compId)->insert(paramName, param); - // Emit change - emit parameterChanged(uasId, compId, paramName, param); - emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); - //qDebug() << "RECEIVED PARAM:" << param; - } - break; - case MAV_PARAM_TYPE_UINT32: - { - if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - param = QVariant((unsigned int)paramValue.param_float); - } - else { - param = QVariant((quint32)paramValue.param_float); - } - parameters.value(compId)->insert(paramName, param); - // Emit change - emit parameterChanged(uasId, compId, paramName, param); - emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); - } - break; - case MAV_PARAM_TYPE_INT32: - { - if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { - param = QVariant((int)paramValue.param_float); - } - else { - param = QVariant((qint32)paramValue.param_float); - } - parameters.value(compId)->insert(paramName, param); - // Emit change - emit parameterChanged(uasId, compId, paramName, param); - emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); -// qDebug() << "RECEIVED PARAM:" << param; - } - break; - default: - qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type; + switch (rawValue.param_type) { + case MAV_PARAM_TYPE_REAL32: + if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + param = QVariant(paramValue.param_float); + } else { + param = QVariant(paramValue.param_float); + } + break; + case MAV_PARAM_TYPE_UINT8: + if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + param = QVariant(QChar((unsigned char)paramValue.param_float)); + } else { + param = QVariant(QChar((quint8)paramValue.param_float)); + } + break; + case MAV_PARAM_TYPE_INT8: + if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + param = QVariant(QChar((char)paramValue.param_float)); + } else { + param = QVariant(QChar((qint8)paramValue.param_float)); + } + break; + case MAV_PARAM_TYPE_INT16: + if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + param = QVariant((short)paramValue.param_float); + } else { + param = QVariant((qint16)paramValue.param_float); + } + break; + case MAV_PARAM_TYPE_UINT16: + if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + param = QVariant((unsigned short)paramValue.param_float); + } else { + param = QVariant((quint16)paramValue.param_float); + } + break; + case MAV_PARAM_TYPE_UINT32: + if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + param = QVariant((unsigned int)paramValue.param_float); + } else { + param = QVariant((quint32)paramValue.param_float); + } + break; + case MAV_PARAM_TYPE_INT32: + if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + param = QVariant((int)paramValue.param_float); + } else { + param = QVariant((qint32)paramValue.param_float); + } + break; + default: + qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type; + return; } //switch (value.param_type) + + // We did not return a critical error, now insert parameter into registry + if (parameters.value(compId)->contains(paramName)) { + parameters.value(compId)->remove(paramName); + } + // add new values + parameters.value(compId)->insert(paramName, param); + // Emit change + emit parameterChanged(uasId, compId, paramName, param); + emit parameterChanged(uasId, compId, rawValue.param_count, rawValue.param_index, paramName, param); } /** -- 2.22.0