diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 753aebc40e40c7198ac90b55c4b389f7685242c7..3d5e341fa9ede791b0f600242bd61f22769e2029 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -211,7 +211,7 @@ int APMFirmwarePlugin::manualControlReservedButtonCount(void) return -1; } -void APMFirmwarePlugin::_handleParamValue(Vehicle* vehicle, mavlink_message_t* message) +void APMFirmwarePlugin::_handleIncomingParamValue(Vehicle* vehicle, mavlink_message_t* message) { Q_UNUSED(vehicle); @@ -301,7 +301,7 @@ void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* mavlink_msg_param_set_encode_chan(message->sysid, message->compid, outgoingLink->mavlinkChannel(), message, ¶mSet); } -bool APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* message) +bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message) { QString messageText; @@ -410,7 +410,7 @@ bool APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* m return true; } -void APMFirmwarePlugin::_handleHeartbeat(Vehicle* vehicle, mavlink_message_t* message) +void APMFirmwarePlugin::_handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message) { bool flying = false; @@ -439,12 +439,12 @@ bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_m switch (message->msgid) { case MAVLINK_MSG_ID_PARAM_VALUE: - _handleParamValue(vehicle, message); + _handleIncomingParamValue(vehicle, message); break; case MAVLINK_MSG_ID_STATUSTEXT: - return _handleStatusText(vehicle, message); + return _handleIncomingStatusText(vehicle, message); case MAVLINK_MSG_ID_HEARTBEAT: - _handleHeartbeat(vehicle, message); + _handleIncomingHeartbeat(vehicle, message); break; } diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 8f79d543f2d1e2a28772d354e0dceeebbbb38136..2c5150d40dfb279255a0bf8a1fb09eb6122c3530 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -114,10 +114,10 @@ private: static bool _isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion); void _setInfoSeverity(mavlink_message_t* message) const; QString _getMessageText(mavlink_message_t* message) const; - void _handleParamValue(Vehicle* vehicle, mavlink_message_t* message); + void _handleIncomingParamValue(Vehicle* vehicle, mavlink_message_t* message); + bool _handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message); + void _handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message); void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); - bool _handleStatusText(Vehicle* vehicle, mavlink_message_t* message); - void _handleHeartbeat(Vehicle* vehicle, mavlink_message_t* message); void _soloVideoHandshake(Vehicle* vehicle); bool _textSeverityAdjustmentNeeded; diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index b06c41ec071837daead25af7afaff7eb3b1d4958..fce51b800a7d265830cb3350b6c8cfa078055ddf 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -260,14 +260,15 @@ void MockLink::_sendHeartBeat(void) { mavlink_message_t msg; - mavlink_msg_heartbeat_pack(_vehicleSystemId, - _vehicleComponentId, - &msg, - _vehicleType, // MAV_TYPE - _firmwareType, // MAV_AUTOPILOT - _mavBaseMode, // MAV_MODE - _mavCustomMode, // custom mode - _mavState); // MAV_STATE + mavlink_msg_heartbeat_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + _vehicleType, // MAV_TYPE + _firmwareType, // MAV_AUTOPILOT + _mavBaseMode, // MAV_MODE + _mavCustomMode, // custom mode + _mavState); // MAV_STATE respondWithMavlinkMessage(msg); } @@ -276,16 +277,17 @@ void MockLink::_sendVibration(void) { mavlink_message_t msg; - mavlink_msg_vibration_pack(_vehicleSystemId, - _vehicleComponentId, - &msg, - 0, // time_usec - 50.5, // vibration_x, - 10.5, // vibration_y, - 60.0, // vibration_z, - 1, // clipping_0 - 2, // clipping_0 - 3); // clipping_0 + mavlink_msg_vibration_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + 0, // time_usec + 50.5, // vibration_x, + 10.5, // vibration_y, + 60.0, // vibration_z, + 1, // clipping_0 + 2, // clipping_0 + 3); // clipping_0 respondWithMavlinkMessage(msg); } @@ -603,14 +605,15 @@ void MockLink::_paramRequestListWorker(void) qCDebug(MockLinkLog) << "Sending msg_param_value" << componentId << paramId << paramType << _mapParamName2Value[componentId][paramId]; - mavlink_msg_param_value_pack(_vehicleSystemId, - componentId, // component id - &responseMsg, // Outgoing message - paramId, // Parameter name - _floatUnionForParam(componentId, paramName), // Parameter value - paramType, // MAV_PARAM_TYPE - cParameters, // Total number of parameters - _currentParamRequestListParamIndex); // Index of this parameter + mavlink_msg_param_value_pack_chan(_vehicleSystemId, + componentId, // component id + mavlinkChannel(), + &responseMsg, // Outgoing message + paramId, // Parameter name + _floatUnionForParam(componentId, paramName), // Parameter value + paramType, // MAV_PARAM_TYPE + cParameters, // Total number of parameters + _currentParamRequestListParamIndex); // Index of this parameter respondWithMavlinkMessage(responseMsg); } @@ -650,14 +653,15 @@ void MockLink::_handleParamSet(const mavlink_message_t& msg) // Respond with a param_value to ack mavlink_message_t responseMsg; - mavlink_msg_param_value_pack(_vehicleSystemId, - componentId, // component id - &responseMsg, // Outgoing message - paramId, // Parameter name - request.param_value, // Send same value back - request.param_type, // Send same type back - _mapParamName2Value[componentId].count(), // Total number of parameters - _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter + mavlink_msg_param_value_pack_chan(_vehicleSystemId, + componentId, // component id + mavlinkChannel(), + &responseMsg, // Outgoing message + paramId, // Parameter name + request.param_value, // Send same value back + request.param_type, // Send same type back + _mapParamName2Value[componentId].count(), // Total number of parameters + _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter respondWithMavlinkMessage(responseMsg); } @@ -676,14 +680,15 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) valueUnion.type = MAV_PARAM_TYPE_UINT32; valueUnion.param_uint32 = 0; // Special case of magic hash check value - mavlink_msg_param_value_pack(_vehicleSystemId, - componentId, - &responseMsg, - request.param_id, - valueUnion.param_float, - MAV_PARAM_TYPE_UINT32, - 0, - -1); + mavlink_msg_param_value_pack_chan(_vehicleSystemId, + componentId, + mavlinkChannel(), + &responseMsg, + request.param_id, + valueUnion.param_float, + MAV_PARAM_TYPE_UINT32, + 0, + -1); respondWithMavlinkMessage(responseMsg); return; } @@ -717,14 +722,15 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) return; } - mavlink_msg_param_value_pack(_vehicleSystemId, - componentId, // component id - &responseMsg, // Outgoing message - paramId, // Parameter name - _floatUnionForParam(componentId, paramId), // Parameter value - _mapParamName2MavParamType[paramId], // Parameter type - _mapParamName2Value[componentId].count(), // Total number of parameters - _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter + mavlink_msg_param_value_pack_chan(_vehicleSystemId, + componentId, // component id + mavlinkChannel(), + &responseMsg, // Outgoing message + paramId, // Parameter name + _floatUnionForParam(componentId, paramId), // Parameter value + _mapParamName2MavParamType[paramId], // Parameter type + _mapParamName2Value[componentId].count(), // Total number of parameters + _mapParamName2Value[componentId].keys().indexOf(paramId)); // Index of this parameter respondWithMavlinkMessage(responseMsg); } @@ -738,12 +744,13 @@ void MockLink::emitRemoteControlChannelRawChanged(int channel, uint16_t raw) chanRaw[channel] = raw; mavlink_message_t responseMsg; - mavlink_msg_rc_channels_pack(_vehicleSystemId, - _vehicleComponentId, - &responseMsg, // Outgoing message - 0, // time since boot, ignored - 18, // channel count - chanRaw[0], // channel raw value + mavlink_msg_rc_channels_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &responseMsg, // Outgoing message + 0, // time since boot, ignored + 18, // channel count + chanRaw[0], // channel raw value chanRaw[1], // channel raw value chanRaw[2], // channel raw value chanRaw[3], // channel raw value @@ -801,11 +808,12 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg) } mavlink_message_t commandAck; - mavlink_msg_command_ack_pack(_vehicleSystemId, - _vehicleComponentId, - &commandAck, - request.command, - commandResult); + mavlink_msg_command_ack_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &commandAck, + request.command, + commandResult); respondWithMavlinkMessage(commandAck); } @@ -827,20 +835,21 @@ void MockLink::_respondWithAutopilotVersion(void) flightVersion |= FIRMWARE_VERSION_TYPE_DEV << (8*0); } - mavlink_msg_autopilot_version_pack(_vehicleSystemId, - _vehicleComponentId, - &msg, - 0, // capabilities, - flightVersion, // flight_sw_version, - 0, // middleware_sw_version, - 0, // os_sw_version, - 0, // board_version, - (uint8_t *)&customVersion, // flight_custom_version, - (uint8_t *)&customVersion, // middleware_custom_version, - (uint8_t *)&customVersion, // os_custom_version, - 0, // vendor_id, - 0, // product_id, - 0); // uid + mavlink_msg_autopilot_version_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + 0, // capabilities, + flightVersion, // flight_sw_version, + 0, // middleware_sw_version, + 0, // os_sw_version, + 0, // board_version, + (uint8_t *)&customVersion, // flight_custom_version, + (uint8_t *)&customVersion, // middleware_custom_version, + (uint8_t *)&customVersion, // os_custom_version, + 0, // vendor_id, + 0, // product_id, + 0); // uid respondWithMavlinkMessage(msg); } @@ -859,14 +868,15 @@ void MockLink::_sendHomePosition(void) bogus[2] = 0.0f; bogus[3] = 0.0f; - mavlink_msg_home_position_pack(_vehicleSystemId, - _vehicleComponentId, - &msg, - (int32_t)(_vehicleLatitude * 1E7), - (int32_t)(_vehicleLongitude * 1E7), - (int32_t)(_vehicleAltitude * 1000), - 0.0f, 0.0f, 0.0f, - &bogus[0], + mavlink_msg_home_position_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + (int32_t)(_vehicleLatitude * 1E7), + (int32_t)(_vehicleLongitude * 1E7), + (int32_t)(_vehicleAltitude * 1000), + 0.0f, 0.0f, 0.0f, + &bogus[0], 0.0f, 0.0f, 0.0f); respondWithMavlinkMessage(msg); } @@ -876,18 +886,19 @@ void MockLink::_sendGpsRawInt(void) static uint64_t timeTick = 0; mavlink_message_t msg; - mavlink_msg_gps_raw_int_pack(_vehicleSystemId, - _vehicleComponentId, - &msg, - timeTick++, // time since boot - 3, // 3D fix - (int32_t)(_vehicleLatitude * 1E7), - (int32_t)(_vehicleLongitude * 1E7), - (int32_t)(_vehicleAltitude * 1000), - UINT16_MAX, UINT16_MAX, // HDOP/VDOP not known - UINT16_MAX, // velocity not known - UINT16_MAX, // course over ground not known - 8); // satellite count + mavlink_msg_gps_raw_int_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + timeTick++, // time since boot + 3, // 3D fix + (int32_t)(_vehicleLatitude * 1E7), + (int32_t)(_vehicleLongitude * 1E7), + (int32_t)(_vehicleAltitude * 1000), + UINT16_MAX, UINT16_MAX, // HDOP/VDOP not known + UINT16_MAX, // velocity not known + UINT16_MAX, // course over ground not known + 8); // satellite count respondWithMavlinkMessage(msg); } @@ -914,11 +925,12 @@ void MockLink::_sendStatusTextMessages(void) mavlink_message_t msg; const struct StatusMessage* status = &rgMessages[i]; - mavlink_msg_statustext_pack(_vehicleSystemId, - _vehicleComponentId, - &msg, - status->severity, - status->msg); + mavlink_msg_statustext_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + status->severity, + status->msg); respondWithMavlinkMessage(msg); } } @@ -1065,21 +1077,22 @@ void MockLink::_sendRCChannels(void) { mavlink_message_t msg; - mavlink_msg_rc_channels_pack(_vehicleSystemId, - _vehicleComponentId, - &msg, - 0, // time_boot_ms - 8, // chancount - 1500, // chan1_raw - 1500, // chan2_raw - 1500, // chan3_raw - 1500, // chan4_raw - 1500, // chan5_raw - 1500, // chan6_raw - 1500, // chan7_raw - 1500, // chan8_raw - UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, - 0); // rssi + mavlink_msg_rc_channels_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + 0, // time_boot_ms + 8, // chancount + 1500, // chan1_raw + 1500, // chan2_raw + 1500, // chan3_raw + 1500, // chan4_raw + 1500, // chan5_raw + 1500, // chan6_raw + 1500, // chan7_raw + 1500, // chan8_raw + UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, + 0); // rssi respondWithMavlinkMessage(msg); @@ -1106,11 +1119,12 @@ void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request } mavlink_message_t msg; - mavlink_msg_statustext_pack(_vehicleSystemId, - _vehicleComponentId, - &msg, - MAV_SEVERITY_INFO, - pCalMessage); + mavlink_msg_statustext_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &msg, + MAV_SEVERITY_INFO, + pCalMessage); respondWithMavlinkMessage(msg); } @@ -1126,14 +1140,15 @@ void MockLink::_handleLogRequestList(const mavlink_message_t& msg) } mavlink_message_t responseMsg; - mavlink_msg_log_entry_pack(_vehicleSystemId, - _vehicleComponentId, - &responseMsg, - _logDownloadLogId, // log id - 1, // num_logs - 1, // last_log_num - 0, // time_utc - _logDownloadFileSize); // size + mavlink_msg_log_entry_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &responseMsg, + _logDownloadLogId, // log id + 1, // num_logs + 1, // last_log_num + 0, // time_utc + _logDownloadFileSize); // size respondWithMavlinkMessage(responseMsg); } @@ -1181,13 +1196,14 @@ void MockLink::_logDownloadWorker(void) qDebug() << "MockLink::_logDownloadWorker" << _logDownloadCurrentOffset << _logDownloadBytesRemaining; mavlink_message_t responseMsg; - mavlink_msg_log_data_pack(_vehicleSystemId, - _vehicleComponentId, - &responseMsg, - _logDownloadLogId, - _logDownloadCurrentOffset, - bytesToRead, - &buffer[0]); + mavlink_msg_log_data_pack_chan(_vehicleSystemId, + _vehicleComponentId, + mavlinkChannel(), + &responseMsg, + _logDownloadLogId, + _logDownloadCurrentOffset, + bytesToRead, + &buffer[0]); respondWithMavlinkMessage(responseMsg); _logDownloadCurrentOffset += bytesToRead; diff --git a/src/comm/MockLinkFileServer.cc b/src/comm/MockLinkFileServer.cc index a952ae738b099e6560e20307eb6d108fc5af3fcb..238e6c9667d4949ec515956d6c266fb7ad7cb2d5 100644 --- a/src/comm/MockLinkFileServer.cc +++ b/src/comm/MockLinkFileServer.cc @@ -365,13 +365,14 @@ void MockLinkFileServer::_sendResponse(uint8_t targetSystemId, uint8_t targetCom request->hdr.seqNumber = seqNumber; - mavlink_msg_file_transfer_protocol_pack(_systemIdServer, // System ID - 0, // Component ID - &mavlinkMessage, // Mavlink Message to pack into - 0, // Target network - targetSystemId, - targetComponentId, - (uint8_t*)request); // Payload + mavlink_msg_file_transfer_protocol_pack_chan(_systemIdServer, // System ID + 0, // Component ID + _mockLink->mavlinkChannel(), + &mavlinkMessage, // Mavlink Message to pack into + 0, // Target network + targetSystemId, + targetComponentId, + (uint8_t*)request); // Payload _mockLink->respondWithMavlinkMessage(mavlinkMessage); } diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc index 8978207649fd1872ff1cea933ff7720f25610f74..146e5d7f31bb321401f383498f3c1117ecb103e1 100644 --- a/src/comm/MockLinkMissionItemHandler.cc +++ b/src/comm/MockLinkMissionItemHandler.cc @@ -96,12 +96,13 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message mavlink_message_t responseMsg; - mavlink_msg_mission_count_pack(_mockLink->vehicleId(), - MAV_COMP_ID_MISSIONPLANNER, - &responseMsg, // Outgoing message - msg.sysid, // Target is original sender - msg.compid, // Target is original sender - itemCount); // Number of mission items + mavlink_msg_mission_count_pack_chan(_mockLink->vehicleId(), + MAV_COMP_ID_MISSIONPLANNER, + _mockLink->mavlinkChannel(), + &responseMsg, // Outgoing message + msg.sysid, // Target is original sender + msg.compid, // Target is original sender + itemCount); // Number of mission items _mockLink->respondWithMavlinkMessage(responseMsg); } else { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode"; @@ -149,18 +150,19 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& item = _missionItems[request.seq]; } - mavlink_msg_mission_item_pack(_mockLink->vehicleId(), - MAV_COMP_ID_MISSIONPLANNER, - &responseMsg, // Outgoing message - msg.sysid, // Target is original sender - msg.compid, // Target is original sender - request.seq, // Index of mission item being sent - item.frame, - item.command, - item.current, - item.autocontinue, - item.param1, item.param2, item.param3, item.param4, - item.x, item.y, item.z); + mavlink_msg_mission_item_pack_chan(_mockLink->vehicleId(), + MAV_COMP_ID_MISSIONPLANNER, + _mockLink->mavlinkChannel(), + &responseMsg, // Outgoing message + msg.sysid, // Target is original sender + msg.compid, // Target is original sender + request.seq, // Index of mission item being sent + item.frame, + item.command, + item.current, + item.autocontinue, + item.param1, item.param2, item.param3, item.param4, + item.x, item.y, item.z); _mockLink->respondWithMavlinkMessage(responseMsg); } } @@ -218,7 +220,11 @@ void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber) missionRequest.target_component = _mavlinkProtocol->getComponentId(); missionRequest.seq = sequenceNumber; - mavlink_msg_mission_request_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionRequest); + mavlink_msg_mission_request_encode_chan(_mockLink->vehicleId(), + MAV_COMP_ID_MISSIONPLANNER, + _mockLink->mavlinkChannel(), + &message, + &missionRequest); _mockLink->respondWithMavlinkMessage(message); // If response with Mission Item doesn't come before timer fires it's an error @@ -238,7 +244,11 @@ void MockLinkMissionItemHandler::_sendAck(MAV_MISSION_RESULT ackType) missionAck.target_component = _mavlinkProtocol->getComponentId(); missionAck.type = ackType; - mavlink_msg_mission_ack_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionAck); + mavlink_msg_mission_ack_encode_chan(_mockLink->vehicleId(), + MAV_COMP_ID_MISSIONPLANNER, + _mockLink->mavlinkChannel(), + &message, + &missionAck); _mockLink->respondWithMavlinkMessage(message); }