Browse Source

fix value reading, only real32 was correct.

QGC4.4
treymarc 11 years ago
parent
commit
ec799d44bd
  1. 25
      src/uas/UAS.cc

25
src/uas/UAS.cc

@ -2583,7 +2583,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, @@ -2583,7 +2583,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
param = QVariant(QChar((unsigned char)paramValue.param_float));
}
else {
param = QVariant(QChar((unsigned char)paramValue.param_uint8));
param = QVariant(QChar((uint8_t)paramValue.param_float));
}
parameters.value(compId)->insert(paramName, param);
// Emit change
@ -2598,7 +2598,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, @@ -2598,7 +2598,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
param = QVariant(QChar((char)paramValue.param_float));
}
else {
param = QVariant(QChar((char)paramValue.param_int8));
param = QVariant(QChar((int8_t)paramValue.param_float));
}
parameters.value(compId)->insert(paramName, param);
// Emit change
@ -2613,7 +2613,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, @@ -2613,7 +2613,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
param = QVariant((short)paramValue.param_float);
}
else {
param = QVariant(paramValue.param_int16);
param = QVariant((int16_t)paramValue.param_float);
}
parameters.value(compId)->insert(paramName, param);
// Emit change
@ -2622,13 +2622,28 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, @@ -2622,13 +2622,28 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
//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((uint16_t)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(paramValue.param_uint32);
param = QVariant((uint32_t)paramValue.param_float);
}
parameters.value(compId)->insert(paramName, param);
// Emit change
@ -2642,7 +2657,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, @@ -2642,7 +2657,7 @@ void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName,
param = QVariant((int)paramValue.param_float);
}
else {
param = QVariant(paramValue.param_int32);
param = QVariant((int32_t)paramValue.param_float);
}
parameters.value(compId)->insert(paramName, param);
// Emit change

Loading…
Cancel
Save