diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 3d5e341fa9ede791b0f600242bd61f22769e2029..526df49bc42b1fbe43418c2cb7a0e9dd6c91a6f7 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -251,9 +251,12 @@ void APMFirmwarePlugin::_handleIncomingParamValue(Vehicle* vehicle, mavlink_mess paramValue.param_value = paramUnion.param_float; + // Re-Encoding is always done using mavlink 1.0 + mavlink_status_t* mavlinkStatusReEncode = mavlink_get_channel_status(0); + mavlinkStatusReEncode->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1; mavlink_msg_param_value_encode_chan(message->sysid, message->compid, - vehicle->priorityLink()->mavlinkChannel(), + 0, // Re-encoding uses reserved channel 0 message, ¶mValue); } @@ -522,10 +525,14 @@ void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const break; } - mavlink_msg_statustext_encode(message->sysid, - message->compid, - message, - &statusText); + // Re-Encoding is always done using mavlink 1.0 + mavlink_status_t* mavlinkStatusReEncode = mavlink_get_channel_status(0); + mavlinkStatusReEncode->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1; + mavlink_msg_statustext_encode_chan(message->sysid, + message->compid, + 0, // Re-encoding uses reserved channel 0 + message, + &statusText); } void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const @@ -533,19 +540,26 @@ void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const mavlink_statustext_t statusText; mavlink_msg_statustext_decode(message, &statusText); + // Re-Encoding is always done using mavlink 1.0 + mavlink_status_t* mavlinkStatusReEncode = mavlink_get_channel_status(0); + mavlinkStatusReEncode->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1; statusText.severity = MAV_SEVERITY_INFO; - mavlink_msg_statustext_encode(message->sysid, - message->compid, - message, - &statusText); + mavlink_msg_statustext_encode_chan(message->sysid, + message->compid, + 0, // Re-encoding uses reserved channel 0 + message, + &statusText); } void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const { mavlink_statustext_t statusText; mavlink_msg_statustext_decode(message, &statusText); + // Re-Encoding is always done using mavlink 1.0 + mavlink_status_t* mavlinkStatusReEncode = mavlink_get_channel_status(0); + mavlinkStatusReEncode->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1; statusText.severity = MAV_SEVERITY_INFO; - mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText); + mavlink_msg_statustext_encode_chan(message->sysid, message->compid, 0, message, &statusText); } void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle) diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 85aa877a5cd1a9dcdc0e6dda64465325f6e50b44..7aef4cad9f358d5a47461b18ead2c9dfda92ec7f 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -186,8 +186,8 @@ void LinkManager::_addLink(LinkInterface* link) if (!_links.contains(link)) { bool channelSet = false; - // Find a mavlink channel to use for this link - for (int i=0; i<32; i++) { + // Find a mavlink channel to use for this link, Channel 0 is reserved for internal use. + for (int i=1; i<32; i++) { if (!(_mavlinkChannelsUsedBitMask & 1 << i)) { mavlink_reset_channel_status(i); link->_setMavlinkChannel(i);