Browse Source

MavlinkConsoleController: set correct data size

Do not include the null bytes
QGC4.4
Beat Küng 3 years ago
parent
commit
550b6c3cfa
  1. 3
      src/AnalyzeView/MavlinkConsoleController.cc

3
src/AnalyzeView/MavlinkConsoleController.cc

@ -143,6 +143,7 @@ MavlinkConsoleController::_sendSerialData(QByteArray data, bool close) @@ -143,6 +143,7 @@ MavlinkConsoleController::_sendSerialData(QByteArray data, bool close)
// Send maximum sized chunks until the complete buffer is transmitted
while(data.size()) {
QByteArray chunk{data.left(MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN)};
int dataSize = chunk.size();
// Ensure the buffer is large enough, as the MAVLink parser expects MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN bytes
chunk.append(MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN - chunk.size(), '\0');
uint8_t flags = SERIAL_CONTROL_FLAG_EXCLUSIVE | SERIAL_CONTROL_FLAG_RESPOND | SERIAL_CONTROL_FLAG_MULTI;
@ -159,7 +160,7 @@ MavlinkConsoleController::_sendSerialData(QByteArray data, bool close) @@ -159,7 +160,7 @@ MavlinkConsoleController::_sendSerialData(QByteArray data, bool close)
flags,
0,
0,
chunk.size(),
dataSize,
reinterpret_cast<uint8_t*>(chunk.data()),
_vehicle->id(), _vehicle->defaultComponentId());
_vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);

Loading…
Cancel
Save