Commit 221a43af authored by lm's avatar lm

Minor fix to simulation link

parent 53c54b15
......@@ -845,11 +845,14 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
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)
// {
// Drop on even milliseconds
if (QGC::groundTimeMilliseconds() % 2 == 0)
{
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))
{
......@@ -864,8 +867,10 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
memcpy(stream+streampointer,buffer, bufferlength);
streampointer+=bufferlength;
}
// }
// }
}
}
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
qDebug() << "SIMULATION RECEIVED COMMAND TO SEND PARAMETER";
......
......@@ -282,7 +282,7 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa
else if (justWritten && writeMismatch)
{
// Mismatch, tell user
statusLabel->setText(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(parameterName).arg(map->value(parameterName), value));
statusLabel->setText(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(parameterName).arg(map->value(parameterName)).arg(value));
}
else
{
......@@ -704,7 +704,7 @@ void QGCParamWidget::retransmissionGuardTick()
{
// Re-request write operation
emit parameterChanged(component, key, missingParams->value(key));
statusLabel->setText(tr("Re-requested write: %1: %2").arg(key).arg(missingParams->value(key)));
statusLabel->setText(tr("Requested rewrite of: %1: %2").arg(key).arg(missingParams->value(key)));
count++;
}
else
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment