mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-13 20:48:42 +01:00
1126 lines
32 KiB
C++
1126 lines
32 KiB
C++
/**
|
|
******************************************************************************
|
|
*
|
|
* @file uavobjectfield.cpp
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @see The GNU Public License (GPL) Version 3
|
|
* @addtogroup GCSPlugins GCS Plugins
|
|
* @{
|
|
* @addtogroup UAVObjectsPlugin UAVObjects Plugin
|
|
* @{
|
|
* @brief The UAVUObjects GCS plugin
|
|
*****************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
#include "uavobjectfield.h"
|
|
#include <QtEndian>
|
|
#include <QDebug>
|
|
|
|
UAVObjectField::UAVObjectField(const QString & name, const QString & units, FieldType type, quint32 numElements, const QStringList & options, const QString &limits)
|
|
{
|
|
QStringList elementNames;
|
|
|
|
// Set element names
|
|
for (quint32 n = 0; n < numElements; ++n) {
|
|
elementNames.append(QString("%1").arg(n));
|
|
}
|
|
// Initialize
|
|
constructorInitialize(name, units, type, elementNames, options, limits);
|
|
}
|
|
|
|
UAVObjectField::UAVObjectField(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits)
|
|
{
|
|
constructorInitialize(name, units, type, elementNames, options, limits);
|
|
}
|
|
|
|
void UAVObjectField::constructorInitialize(const QString & name, const QString & units, FieldType type, const QStringList & elementNames, const QStringList & options, const QString &limits)
|
|
{
|
|
// Copy params
|
|
this->name = name;
|
|
this->units = units;
|
|
this->type = type;
|
|
this->options = options;
|
|
this->numElements = elementNames.length();
|
|
this->offset = 0;
|
|
this->data = NULL;
|
|
this->obj = NULL;
|
|
this->elementNames = elementNames;
|
|
// Set field size
|
|
switch (type) {
|
|
case INT8:
|
|
numBytesPerElement = sizeof(qint8);
|
|
break;
|
|
case INT16:
|
|
numBytesPerElement = sizeof(qint16);
|
|
break;
|
|
case INT32:
|
|
numBytesPerElement = sizeof(qint32);
|
|
break;
|
|
case UINT8:
|
|
numBytesPerElement = sizeof(quint8);
|
|
break;
|
|
case UINT16:
|
|
numBytesPerElement = sizeof(quint16);
|
|
break;
|
|
case UINT32:
|
|
numBytesPerElement = sizeof(quint32);
|
|
break;
|
|
case FLOAT32:
|
|
numBytesPerElement = sizeof(quint32);
|
|
break;
|
|
case ENUM:
|
|
numBytesPerElement = sizeof(quint8);
|
|
break;
|
|
case BITFIELD:
|
|
numBytesPerElement = sizeof(quint8);
|
|
this->options = QStringList() << tr("0") << tr("1");
|
|
break;
|
|
case STRING:
|
|
numBytesPerElement = sizeof(quint8);
|
|
break;
|
|
default:
|
|
numBytesPerElement = 0;
|
|
}
|
|
limitsInitialize(limits);
|
|
}
|
|
|
|
void UAVObjectField::limitsInitialize(const QString &limits)
|
|
{
|
|
// Limit string format:
|
|
// % - start char
|
|
// XXXX - optional BOARD_TYPE and BOARD_REVISION (4 hex digits)
|
|
// TY - rule type (EQ-equal, NE-not equal, BE-between, BI-bigger, SM-smaller)
|
|
// VAL - values for TY separated by colon
|
|
// , - rule separator (may have leading or trailing spaces)
|
|
// ; - element separator (may have leading or trailing spaces)
|
|
//
|
|
// Examples:
|
|
// Disable few flight modes for Revo (00903):
|
|
// "%0903NE:Autotune:VelocityControl:PositionHold:ReturnToBase:Land:PathPlanner"
|
|
// Original CC board (rev 1), first element bigger than 3 and second element inside [2.3-5.0]:
|
|
// "%0401BI:3; %BE:2.3:5"
|
|
// Set applicable range [0-500] for 3 elements of array for all boards:
|
|
// "%BE:0:500; %BE:0:500; %BE:0:500"
|
|
if (limits.isEmpty()) {
|
|
return;
|
|
}
|
|
QStringList stringPerElement = limits.split(";");
|
|
quint32 index = 0;
|
|
foreach(QString str, stringPerElement) {
|
|
QStringList ruleList = str.split(",");
|
|
|
|
QList<LimitStruct> limitList;
|
|
foreach(QString rule, ruleList) {
|
|
QString _str = rule.trimmed();
|
|
|
|
if (_str.isEmpty()) {
|
|
continue;
|
|
}
|
|
QStringList valuesPerElement = _str.split(":");
|
|
LimitStruct lstruc;
|
|
bool startFlag = valuesPerElement.at(0).startsWith("%");
|
|
bool maxIndexFlag = (int)(index) < (int)numElements;
|
|
bool elemNumberSizeFlag = valuesPerElement.at(0).size() == 3;
|
|
bool aux;
|
|
valuesPerElement.at(0).mid(1, 4).toInt(&aux, 16);
|
|
bool b4 = ((valuesPerElement.at(0).size()) == 7 && aux);
|
|
if (startFlag && maxIndexFlag && (elemNumberSizeFlag || b4)) {
|
|
if (b4) {
|
|
lstruc.board = valuesPerElement.at(0).mid(1, 4).toInt(&aux, 16);
|
|
} else {
|
|
lstruc.board = 0;
|
|
}
|
|
if (valuesPerElement.at(0).right(2) == "EQ") {
|
|
lstruc.type = EQUAL;
|
|
} else if (valuesPerElement.at(0).right(2) == "NE") {
|
|
lstruc.type = NOT_EQUAL;
|
|
} else if (valuesPerElement.at(0).right(2) == "BE") {
|
|
lstruc.type = BETWEEN;
|
|
} else if (valuesPerElement.at(0).right(2) == "BI") {
|
|
lstruc.type = BIGGER;
|
|
} else if (valuesPerElement.at(0).right(2) == "SM") {
|
|
lstruc.type = SMALLER;
|
|
} else {
|
|
qDebug() << "limits parsing failed (invalid property) on UAVObjectField" << name;
|
|
}
|
|
valuesPerElement.removeAt(0);
|
|
foreach(QString _value, valuesPerElement) {
|
|
QString value = _value.trimmed();
|
|
|
|
switch (type) {
|
|
case UINT8:
|
|
case UINT16:
|
|
case UINT32:
|
|
case BITFIELD:
|
|
lstruc.values.append((quint32)value.toULong());
|
|
break;
|
|
case INT8:
|
|
case INT16:
|
|
case INT32:
|
|
lstruc.values.append((qint32)value.toLong());
|
|
break;
|
|
case FLOAT32:
|
|
lstruc.values.append((float)value.toFloat());
|
|
break;
|
|
case ENUM:
|
|
lstruc.values.append((QString)value);
|
|
break;
|
|
case STRING:
|
|
lstruc.values.append((QString)value);
|
|
break;
|
|
default:
|
|
lstruc.values.append(QVariant());
|
|
}
|
|
}
|
|
limitList.append(lstruc);
|
|
} else {
|
|
if (!valuesPerElement.at(0).isEmpty() && !startFlag) {
|
|
qDebug() << "limits parsing failed (property doesn't start with %) on UAVObjectField" << name;
|
|
} else if (!maxIndexFlag) {
|
|
qDebug() << "limits parsing failed (index>numelements) on UAVObjectField" << name << "index" << index << "numElements" << numElements;
|
|
} else if (!elemNumberSizeFlag || !b4) {
|
|
qDebug() << "limits parsing failed limit not starting with %XX or %YYYYXX where XX is the limit type and YYYY is the board type on UAVObjectField" << name;
|
|
}
|
|
}
|
|
}
|
|
elementLimits.insert(index, limitList);
|
|
++index;
|
|
}
|
|
foreach(QList<LimitStruct> limitList, elementLimits) {
|
|
foreach(LimitStruct limit, limitList) {
|
|
qDebug() << "Limit type" << limit.type << "for board" << limit.board << "for field" << getName();
|
|
foreach(QVariant var, limit.values) {
|
|
qDebug() << "value" << var;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
bool UAVObjectField::isWithinLimits(QVariant var, quint32 index, int board)
|
|
{
|
|
if (!elementLimits.keys().contains(index)) {
|
|
return true;
|
|
}
|
|
|
|
foreach(LimitStruct struc, elementLimits.value(index)) {
|
|
if ((struc.board != board) && board != 0 && struc.board != 0) {
|
|
continue;
|
|
}
|
|
switch (struc.type) {
|
|
case EQUAL:
|
|
switch (type) {
|
|
case INT8:
|
|
case INT16:
|
|
case INT32:
|
|
foreach(QVariant vars, struc.values) {
|
|
if (var.toInt() == vars.toInt()) {
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
|
|
break;
|
|
case UINT8:
|
|
case UINT16:
|
|
case UINT32:
|
|
case BITFIELD:
|
|
foreach(QVariant vars, struc.values) {
|
|
if (var.toUInt() == vars.toUInt()) {
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
|
|
break;
|
|
case ENUM:
|
|
case STRING:
|
|
foreach(QVariant vars, struc.values) {
|
|
if (var.toString() == vars.toString()) {
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
|
|
break;
|
|
case FLOAT32:
|
|
foreach(QVariant vars, struc.values) {
|
|
if (var.toFloat() == vars.toFloat()) {
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
|
|
break;
|
|
default:
|
|
return true;
|
|
}
|
|
break;
|
|
case NOT_EQUAL:
|
|
switch (type) {
|
|
case INT8:
|
|
case INT16:
|
|
case INT32:
|
|
foreach(QVariant vars, struc.values) {
|
|
if (var.toInt() == vars.toInt()) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case UINT8:
|
|
case UINT16:
|
|
case UINT32:
|
|
case BITFIELD:
|
|
foreach(QVariant vars, struc.values) {
|
|
if (var.toUInt() == vars.toUInt()) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case ENUM:
|
|
case STRING:
|
|
foreach(QVariant vars, struc.values) {
|
|
if (var.toString() == vars.toString()) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case FLOAT32:
|
|
foreach(QVariant vars, struc.values) {
|
|
if (var.toFloat() == vars.toFloat()) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
default:
|
|
return true;
|
|
}
|
|
break;
|
|
case BETWEEN:
|
|
if (struc.values.length() < 2) {
|
|
qDebug() << __FUNCTION__ << "between limit with less than 1 pair, aborting; field:" << name;
|
|
return true;
|
|
}
|
|
if (struc.values.length() > 2) {
|
|
qDebug() << __FUNCTION__ << "between limit with more than 1 pair, using first; field" << name;
|
|
}
|
|
switch (type) {
|
|
case INT8:
|
|
case INT16:
|
|
case INT32:
|
|
if (!(var.toInt() >= struc.values.at(0).toInt() && var.toInt() <= struc.values.at(1).toInt())) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case UINT8:
|
|
case UINT16:
|
|
case UINT32:
|
|
case BITFIELD:
|
|
if (!(var.toUInt() >= struc.values.at(0).toUInt() && var.toUInt() <= struc.values.at(1).toUInt())) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case ENUM:
|
|
if (!(options.indexOf(var.toString()) >= options.indexOf(struc.values.at(0).toString()) && options.indexOf(var.toString()) <= options.indexOf(struc.values.at(1).toString()))) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case STRING:
|
|
return true;
|
|
|
|
break;
|
|
case FLOAT32:
|
|
if (!(var.toFloat() >= struc.values.at(0).toFloat() && var.toFloat() <= struc.values.at(1).toFloat())) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
default:
|
|
return true;
|
|
}
|
|
break;
|
|
case BIGGER:
|
|
if (struc.values.length() < 1) {
|
|
qDebug() << __FUNCTION__ << "BIGGER limit with less than 1 value, aborting; field:" << name;
|
|
return true;
|
|
}
|
|
if (struc.values.length() > 1) {
|
|
qDebug() << __FUNCTION__ << "BIGGER limit with more than 1 value, using first; field" << name;
|
|
}
|
|
switch (type) {
|
|
case INT8:
|
|
case INT16:
|
|
case INT32:
|
|
if (!(var.toInt() >= struc.values.at(0).toInt())) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case UINT8:
|
|
case UINT16:
|
|
case UINT32:
|
|
case BITFIELD:
|
|
if (!(var.toUInt() >= struc.values.at(0).toUInt())) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case ENUM:
|
|
if (!(options.indexOf(var.toString()) >= options.indexOf(struc.values.at(0).toString()))) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case STRING:
|
|
return true;
|
|
|
|
break;
|
|
case FLOAT32:
|
|
if (!(var.toFloat() >= struc.values.at(0).toFloat())) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
default:
|
|
return true;
|
|
}
|
|
break;
|
|
case SMALLER:
|
|
switch (type) {
|
|
case INT8:
|
|
case INT16:
|
|
case INT32:
|
|
if (!(var.toInt() <= struc.values.at(0).toInt())) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case UINT8:
|
|
case UINT16:
|
|
case UINT32:
|
|
case BITFIELD:
|
|
if (!(var.toUInt() <= struc.values.at(0).toUInt())) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case ENUM:
|
|
if (!(options.indexOf(var.toString()) <= options.indexOf(struc.values.at(0).toString()))) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
case STRING:
|
|
return true;
|
|
|
|
break;
|
|
case FLOAT32:
|
|
if (!(var.toFloat() <= struc.values.at(0).toFloat())) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
break;
|
|
default:
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
QVariant UAVObjectField::getMaxLimit(quint32 index, int board)
|
|
{
|
|
if (!elementLimits.keys().contains(index)) {
|
|
return QVariant();
|
|
}
|
|
foreach(LimitStruct struc, elementLimits.value(index)) {
|
|
if ((struc.board != board) && board != 0 && struc.board != 0) {
|
|
continue;
|
|
}
|
|
switch (struc.type) {
|
|
case EQUAL:
|
|
case NOT_EQUAL:
|
|
case BIGGER:
|
|
return QVariant();
|
|
|
|
break;
|
|
break;
|
|
case BETWEEN:
|
|
return struc.values.at(1);
|
|
|
|
break;
|
|
case SMALLER:
|
|
return struc.values.at(0);
|
|
|
|
break;
|
|
default:
|
|
return QVariant();
|
|
|
|
break;
|
|
}
|
|
}
|
|
return QVariant();
|
|
}
|
|
QVariant UAVObjectField::getMinLimit(quint32 index, int board)
|
|
{
|
|
if (!elementLimits.keys().contains(index)) {
|
|
return QVariant();
|
|
}
|
|
foreach(LimitStruct struc, elementLimits.value(index)) {
|
|
if ((struc.board != board) && board != 0 && struc.board != 0) {
|
|
return QVariant();
|
|
}
|
|
switch (struc.type) {
|
|
case EQUAL:
|
|
case NOT_EQUAL:
|
|
case SMALLER:
|
|
return QVariant();
|
|
|
|
break;
|
|
break;
|
|
case BETWEEN:
|
|
return struc.values.at(0);
|
|
|
|
break;
|
|
case BIGGER:
|
|
return struc.values.at(0);
|
|
|
|
break;
|
|
default:
|
|
return QVariant();
|
|
|
|
break;
|
|
}
|
|
}
|
|
return QVariant();
|
|
}
|
|
void UAVObjectField::initialize(quint8 *data, quint32 dataOffset, UAVObject *obj)
|
|
{
|
|
this->data = data;
|
|
this->offset = dataOffset;
|
|
this->obj = obj;
|
|
clear();
|
|
}
|
|
|
|
UAVObjectField::FieldType UAVObjectField::getType()
|
|
{
|
|
return type;
|
|
}
|
|
|
|
QString UAVObjectField::getTypeAsString()
|
|
{
|
|
switch (type) {
|
|
case UAVObjectField::INT8:
|
|
return "int8";
|
|
|
|
case UAVObjectField::INT16:
|
|
return "int16";
|
|
|
|
case UAVObjectField::INT32:
|
|
return "int32";
|
|
|
|
case UAVObjectField::UINT8:
|
|
return "uint8";
|
|
|
|
case UAVObjectField::UINT16:
|
|
return "uint16";
|
|
|
|
case UAVObjectField::UINT32:
|
|
return "uint32";
|
|
|
|
case UAVObjectField::FLOAT32:
|
|
return "float32";
|
|
|
|
case UAVObjectField::ENUM:
|
|
return "enum";
|
|
|
|
case UAVObjectField::BITFIELD:
|
|
return "bitfield";
|
|
|
|
case UAVObjectField::STRING:
|
|
return "string";
|
|
|
|
default:
|
|
return "";
|
|
}
|
|
}
|
|
|
|
QStringList UAVObjectField::getElementNames()
|
|
{
|
|
return elementNames;
|
|
}
|
|
|
|
UAVObject *UAVObjectField::getObject()
|
|
{
|
|
return obj;
|
|
}
|
|
|
|
void UAVObjectField::clear()
|
|
{
|
|
QMutexLocker locker(obj->getMutex());
|
|
|
|
switch (type) {
|
|
case BITFIELD:
|
|
memset(&data[offset], 0, numBytesPerElement * ((quint32)(1 + (numElements - 1) / 8)));
|
|
break;
|
|
default:
|
|
memset(&data[offset], 0, numBytesPerElement * numElements);
|
|
break;
|
|
}
|
|
}
|
|
|
|
QString UAVObjectField::getName()
|
|
{
|
|
return name;
|
|
}
|
|
|
|
QString UAVObjectField::getUnits()
|
|
{
|
|
return units;
|
|
}
|
|
|
|
QStringList UAVObjectField::getOptions()
|
|
{
|
|
return options;
|
|
}
|
|
|
|
quint32 UAVObjectField::getNumElements()
|
|
{
|
|
return numElements;
|
|
}
|
|
|
|
quint32 UAVObjectField::getDataOffset()
|
|
{
|
|
return offset;
|
|
}
|
|
|
|
quint32 UAVObjectField::getNumBytes()
|
|
{
|
|
switch (type) {
|
|
case BITFIELD:
|
|
return numBytesPerElement * ((quint32)(1 + (numElements - 1) / 8));
|
|
|
|
break;
|
|
default:
|
|
return numBytesPerElement * numElements;
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
QString UAVObjectField::toString()
|
|
{
|
|
QString sout;
|
|
|
|
sout.append(QString("%1: [ ").arg(name));
|
|
for (unsigned int n = 0; n < numElements; ++n) {
|
|
sout.append(QString("%1 ").arg(getDouble(n)));
|
|
}
|
|
sout.append(QString("] %1\n").arg(units));
|
|
return sout;
|
|
}
|
|
|
|
|
|
qint32 UAVObjectField::pack(quint8 *dataOut)
|
|
{
|
|
QMutexLocker locker(obj->getMutex());
|
|
|
|
// Pack each element in output buffer
|
|
switch (type) {
|
|
case INT8:
|
|
memcpy(dataOut, &data[offset], numElements);
|
|
break;
|
|
case INT16:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
qint16 value;
|
|
memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
qToLittleEndian<qint16>(value, &dataOut[numBytesPerElement * index]);
|
|
}
|
|
break;
|
|
case INT32:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
qint32 value;
|
|
memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
qToLittleEndian<qint32>(value, &dataOut[numBytesPerElement * index]);
|
|
}
|
|
break;
|
|
case UINT8:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
dataOut[numBytesPerElement * index] = data[offset + numBytesPerElement * index];
|
|
}
|
|
break;
|
|
case UINT16:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
quint16 value;
|
|
memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
qToLittleEndian<quint16>(value, &dataOut[numBytesPerElement * index]);
|
|
}
|
|
break;
|
|
case UINT32:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
quint32 value;
|
|
memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
qToLittleEndian<quint32>(value, &dataOut[numBytesPerElement * index]);
|
|
}
|
|
break;
|
|
case FLOAT32:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
quint32 value;
|
|
memcpy(&value, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
qToLittleEndian<quint32>(value, &dataOut[numBytesPerElement * index]);
|
|
}
|
|
break;
|
|
case ENUM:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
dataOut[numBytesPerElement * index] = data[offset + numBytesPerElement * index];
|
|
}
|
|
break;
|
|
case BITFIELD:
|
|
for (quint32 index = 0; index < (quint32)(1 + (numElements - 1) / 8); ++index) {
|
|
dataOut[numBytesPerElement * index] = data[offset + numBytesPerElement * index];
|
|
}
|
|
break;
|
|
case STRING:
|
|
memcpy(dataOut, &data[offset], numElements);
|
|
break;
|
|
}
|
|
// Done
|
|
return getNumBytes();
|
|
}
|
|
|
|
qint32 UAVObjectField::unpack(const quint8 *dataIn)
|
|
{
|
|
QMutexLocker locker(obj->getMutex());
|
|
|
|
// Unpack each element from input buffer
|
|
switch (type) {
|
|
case INT8:
|
|
memcpy(&data[offset], dataIn, numElements);
|
|
break;
|
|
case INT16:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
qint16 value;
|
|
value = qFromLittleEndian<qint16>(&dataIn[numBytesPerElement * index]);
|
|
memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement);
|
|
}
|
|
break;
|
|
case INT32:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
qint32 value;
|
|
value = qFromLittleEndian<qint32>(&dataIn[numBytesPerElement * index]);
|
|
memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement);
|
|
}
|
|
break;
|
|
case UINT8:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
data[offset + numBytesPerElement * index] = dataIn[numBytesPerElement * index];
|
|
}
|
|
break;
|
|
case UINT16:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
quint16 value;
|
|
value = qFromLittleEndian<quint16>(&dataIn[numBytesPerElement * index]);
|
|
memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement);
|
|
}
|
|
break;
|
|
case UINT32:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
quint32 value;
|
|
value = qFromLittleEndian<quint32>(&dataIn[numBytesPerElement * index]);
|
|
memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement);
|
|
}
|
|
break;
|
|
case FLOAT32:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
quint32 value;
|
|
value = qFromLittleEndian<quint32>(&dataIn[numBytesPerElement * index]);
|
|
memcpy(&data[offset + numBytesPerElement * index], &value, numBytesPerElement);
|
|
}
|
|
break;
|
|
case ENUM:
|
|
for (quint32 index = 0; index < numElements; ++index) {
|
|
data[offset + numBytesPerElement * index] = dataIn[numBytesPerElement * index];
|
|
}
|
|
break;
|
|
case BITFIELD:
|
|
for (quint32 index = 0; index < (quint32)(1 + (numElements - 1) / 8); ++index) {
|
|
data[offset + numBytesPerElement * index] = dataIn[numBytesPerElement * index];
|
|
}
|
|
break;
|
|
case STRING:
|
|
memcpy(&data[offset], dataIn, numElements);
|
|
break;
|
|
}
|
|
// Done
|
|
return getNumBytes();
|
|
}
|
|
|
|
bool UAVObjectField::isNumeric()
|
|
{
|
|
switch (type) {
|
|
case INT8:
|
|
return true;
|
|
|
|
break;
|
|
case INT16:
|
|
return true;
|
|
|
|
break;
|
|
case INT32:
|
|
return true;
|
|
|
|
break;
|
|
case UINT8:
|
|
return true;
|
|
|
|
break;
|
|
case UINT16:
|
|
return true;
|
|
|
|
break;
|
|
case UINT32:
|
|
return true;
|
|
|
|
break;
|
|
case FLOAT32:
|
|
return true;
|
|
|
|
break;
|
|
case ENUM:
|
|
return false;
|
|
|
|
break;
|
|
case BITFIELD:
|
|
return true;
|
|
|
|
break;
|
|
case STRING:
|
|
return false;
|
|
|
|
break;
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool UAVObjectField::isText()
|
|
{
|
|
switch (type) {
|
|
case INT8:
|
|
return false;
|
|
|
|
break;
|
|
case INT16:
|
|
return false;
|
|
|
|
break;
|
|
case INT32:
|
|
return false;
|
|
|
|
break;
|
|
case UINT8:
|
|
return false;
|
|
|
|
break;
|
|
case UINT16:
|
|
return false;
|
|
|
|
break;
|
|
case UINT32:
|
|
return false;
|
|
|
|
break;
|
|
case FLOAT32:
|
|
return false;
|
|
|
|
break;
|
|
case ENUM:
|
|
return true;
|
|
|
|
break;
|
|
case BITFIELD:
|
|
return false;
|
|
|
|
break;
|
|
case STRING:
|
|
return true;
|
|
|
|
break;
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
QVariant UAVObjectField::getValue(quint32 index)
|
|
{
|
|
QMutexLocker locker(obj->getMutex());
|
|
|
|
// Check that index is not out of bounds
|
|
if (index >= numElements) {
|
|
return QVariant();
|
|
}
|
|
// Get value
|
|
switch (type) {
|
|
case INT8:
|
|
{
|
|
qint8 tmpint8;
|
|
memcpy(&tmpint8, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
return QVariant(tmpint8);
|
|
|
|
break;
|
|
}
|
|
case INT16:
|
|
{
|
|
qint16 tmpint16;
|
|
memcpy(&tmpint16, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
return QVariant(tmpint16);
|
|
|
|
break;
|
|
}
|
|
case INT32:
|
|
{
|
|
qint32 tmpint32;
|
|
memcpy(&tmpint32, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
return QVariant(tmpint32);
|
|
|
|
break;
|
|
}
|
|
case UINT8:
|
|
{
|
|
quint8 tmpuint8;
|
|
memcpy(&tmpuint8, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
return QVariant(tmpuint8);
|
|
|
|
break;
|
|
}
|
|
case UINT16:
|
|
{
|
|
quint16 tmpuint16;
|
|
memcpy(&tmpuint16, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
return QVariant(tmpuint16);
|
|
|
|
break;
|
|
}
|
|
case UINT32:
|
|
{
|
|
quint32 tmpuint32;
|
|
memcpy(&tmpuint32, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
return QVariant(tmpuint32);
|
|
|
|
break;
|
|
}
|
|
case FLOAT32:
|
|
{
|
|
float tmpfloat;
|
|
memcpy(&tmpfloat, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
return QVariant(tmpfloat);
|
|
|
|
break;
|
|
}
|
|
case ENUM:
|
|
{
|
|
quint8 tmpenum;
|
|
memcpy(&tmpenum, &data[offset + numBytesPerElement * index], numBytesPerElement);
|
|
if (tmpenum >= options.length()) {
|
|
qDebug() << "Invalid value for" << name;
|
|
tmpenum = 0;
|
|
}
|
|
return QVariant(options[tmpenum]);
|
|
|
|
break;
|
|
}
|
|
case BITFIELD:
|
|
{
|
|
quint8 tmpbitfield;
|
|
memcpy(&tmpbitfield, &data[offset + numBytesPerElement * ((quint32)(index / 8))], numBytesPerElement);
|
|
tmpbitfield = (tmpbitfield >> (index % 8)) & 1;
|
|
return QVariant(tmpbitfield);
|
|
|
|
break;
|
|
}
|
|
case STRING:
|
|
{
|
|
data[offset + numElements - 1] = '\0';
|
|
QString str((char *)&data[offset]);
|
|
return QVariant(str);
|
|
|
|
break;
|
|
}
|
|
}
|
|
// If this point is reached then we got an invalid type
|
|
return QVariant();
|
|
}
|
|
|
|
bool UAVObjectField::checkValue(const QVariant & value, quint32 index)
|
|
{
|
|
QMutexLocker locker(obj->getMutex());
|
|
|
|
// Check that index is not out of bounds
|
|
if (index >= numElements) {
|
|
return false;
|
|
}
|
|
// Get metadata
|
|
UAVObject::Metadata mdata = obj->getMetadata();
|
|
// Update value if the access mode permits
|
|
if (UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READWRITE) {
|
|
switch (type) {
|
|
case INT8:
|
|
case INT16:
|
|
case INT32:
|
|
case UINT8:
|
|
case UINT16:
|
|
case UINT32:
|
|
case FLOAT32:
|
|
case STRING:
|
|
case BITFIELD:
|
|
return true;
|
|
|
|
break;
|
|
case ENUM:
|
|
{
|
|
qint8 tmpenum = options.indexOf(value.toString());
|
|
return (tmpenum < 0) ? false : true;
|
|
|
|
break;
|
|
}
|
|
default:
|
|
qDebug() << "checkValue: other types" << type;
|
|
Q_ASSERT(0); // To catch any programming errors where we tried to test invalid values
|
|
break;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void UAVObjectField::setValue(const QVariant & value, quint32 index)
|
|
{
|
|
QMutexLocker locker(obj->getMutex());
|
|
|
|
// Check that index is not out of bounds
|
|
if (index >= numElements) {
|
|
return;
|
|
}
|
|
// Get metadata
|
|
UAVObject::Metadata mdata = obj->getMetadata();
|
|
// Update value if the access mode permits
|
|
if (UAVObject::GetGcsAccess(mdata) == UAVObject::ACCESS_READWRITE) {
|
|
switch (type) {
|
|
case INT8:
|
|
{
|
|
qint8 tmpint8 = value.toInt();
|
|
memcpy(&data[offset + numBytesPerElement * index], &tmpint8, numBytesPerElement);
|
|
break;
|
|
}
|
|
case INT16:
|
|
{
|
|
qint16 tmpint16 = value.toInt();
|
|
memcpy(&data[offset + numBytesPerElement * index], &tmpint16, numBytesPerElement);
|
|
break;
|
|
}
|
|
case INT32:
|
|
{
|
|
qint32 tmpint32 = value.toInt();
|
|
memcpy(&data[offset + numBytesPerElement * index], &tmpint32, numBytesPerElement);
|
|
break;
|
|
}
|
|
case UINT8:
|
|
{
|
|
quint8 tmpuint8 = value.toUInt();
|
|
memcpy(&data[offset + numBytesPerElement * index], &tmpuint8, numBytesPerElement);
|
|
break;
|
|
}
|
|
case UINT16:
|
|
{
|
|
quint16 tmpuint16 = value.toUInt();
|
|
memcpy(&data[offset + numBytesPerElement * index], &tmpuint16, numBytesPerElement);
|
|
break;
|
|
}
|
|
case UINT32:
|
|
{
|
|
quint32 tmpuint32 = value.toUInt();
|
|
memcpy(&data[offset + numBytesPerElement * index], &tmpuint32, numBytesPerElement);
|
|
break;
|
|
}
|
|
case FLOAT32:
|
|
{
|
|
float tmpfloat = value.toFloat();
|
|
memcpy(&data[offset + numBytesPerElement * index], &tmpfloat, numBytesPerElement);
|
|
break;
|
|
}
|
|
case ENUM:
|
|
{
|
|
qint8 tmpenum = options.indexOf(value.toString());
|
|
// Default to 0 on invalid values.
|
|
if (tmpenum < 0) {
|
|
tmpenum = 0;
|
|
}
|
|
memcpy(&data[offset + numBytesPerElement * index], &tmpenum, numBytesPerElement);
|
|
break;
|
|
}
|
|
case BITFIELD:
|
|
{
|
|
quint8 tmpbitfield;
|
|
memcpy(&tmpbitfield, &data[offset + numBytesPerElement * ((quint32)(index / 8))], numBytesPerElement);
|
|
tmpbitfield = (tmpbitfield & ~(1 << (index % 8))) | ((value.toUInt() != 0 ? 1 : 0) << (index % 8));
|
|
memcpy(&data[offset + numBytesPerElement * ((quint32)(index / 8))], &tmpbitfield, numBytesPerElement);
|
|
break;
|
|
}
|
|
case STRING:
|
|
{
|
|
QString str = value.toString();
|
|
QByteArray barray = str.toAscii();
|
|
quint32 index;
|
|
for (index = 0; index < (quint32)barray.length() && index < (numElements - 1); ++index) {
|
|
data[offset + index] = barray[index];
|
|
}
|
|
barray[index] = '\0';
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
double UAVObjectField::getDouble(quint32 index)
|
|
{
|
|
return getValue(index).toDouble();
|
|
}
|
|
|
|
void UAVObjectField::setDouble(double value, quint32 index)
|
|
{
|
|
setValue(QVariant(value), index);
|
|
}
|