diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index c453595a7cdc2d662d646b47a9a7740781a39ca8..9239b431b4974762f0cf221b1a17316abe1adbcd 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -620,6 +620,14 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) { onboardParams.remove(key); onboardParams.insert(key, set.param_value); + + // Pack message and get size of encoded byte string + mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value); + // Allocate buffer with packet data + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer+=bufferlength; } } } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 6ae1fd6858d7e9b03ab1bb881a5bc74541362923..9e12210e6bfc85d4fc1599889f4872035fc93aac 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -631,8 +631,8 @@ void UAS::setParameter(int component, QString id, float value) mavlink_message_t msg; mavlink_param_set_t p; p.param_value = value; - p.target_system = uasId; - p.target_component = component; + p.target_system = (uint8_t)uasId; + p.target_component = (uint8_t)component; // Copy string into buffer, ensuring not to exceed the buffer size char* s = (char*)id.toStdString().c_str(); diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index c194fe912357c1cb4b66ccf8a56d65566e48717a..d31af497a739cb94217c6418fabd075fa60bb52e 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -153,8 +153,9 @@ void QGCParamWidget::setParameters() } } } + clear(); + //mav->requestParameters(); qDebug() << __FILE__ << __LINE__ << "SETTING ALL PARAMETERS"; - requestParameterList(); } void QGCParamWidget::writeParameters()