diff --git a/QGCExternalLibs.pri b/QGCExternalLibs.pri index 06cb2e1fb9dbc95f093276ffb4d7d49351acdaf9..506884f6f095b169d8b685a827bb78a8eea397c9 100644 --- a/QGCExternalLibs.pri +++ b/QGCExternalLibs.pri @@ -7,20 +7,35 @@ WindowsBuild { # # [REQUIRED] Add support for the MAVLink communications protocol. -# Mavlink dialect is hardwired to arudpilotmega for now. The reason being +# +# By default MAVLink dialect is hardwired to arudpilotmega. The reason being # the current codebase supports both PX4 and APM flight stack. PX4 flight stack -# only usese common mavlink specifications, whereas APM flight stack uses custom -# mavlink specifications which add to common. So by using the adupilotmega dialect +# only uses common MAVLink specifications, whereas APM flight stack uses custom +# MAVLink specifications which adds to common. So by using the adupilotmega dialect # QGC can support both in the same codebase. -# + # Once the mavlink helper routines include support for multiple dialects within # a single compiled codebase this hardwiring of dialect can go away. But until then # this "workaround" is needed. -MAVLINKPATH_REL = libs/mavlink/include/mavlink/v2.0 -MAVLINKPATH = $$BASEDIR/$$MAVLINKPATH_REL -MAVLINK_CONF = ardupilotmega -DEFINES += MAVLINK_NO_DATA +# In the mean time, it’s possible to define a completely different dialect by defining the +# location and name below. + +isEmpty(MAVLINKPATH_REL) { + MAVLINKPATH_REL = libs/mavlink/include/mavlink/v2.0 +} +isEmpty(MAVLINKPATH) { + MAVLINKPATH = $$BASEDIR/$$MAVLINKPATH_REL +} +isEmpty(MAVLINK_CONF) { + MAVLINK_CONF = ardupilotmega +} + +# If defined, all APM specific MAVLink messages are disabled +contains (CONFIG, QGC_DISABLE_APM_MAVLINK) { + message("Disable APM MAVLink support") + DEFINES += NO_ARDUPILOT_DIALECT +} # First we select the dialect, checking for valid user selection # Users can override all other settings by specifying MAVLINK_CONF as an argument to qmake diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 7aed3cdd836e6576de4bdf0f568f182fae0055cd..dd0ccd8aeff9cbb39b5d5fae692b181fd614e608 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -674,9 +674,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_SCALED_PRESSURE3: _handleScaledPressure3(message); break; - case MAVLINK_MSG_ID_CAMERA_FEEDBACK: - _handleCameraFeedback(message); - break; case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED: _handleCameraImageCaptured(message); break; @@ -691,11 +688,16 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes emit mavlinkSerialControl(ser.device, ser.flags, ser.timeout, ser.baudrate, QByteArray(reinterpret_cast(ser.data), ser.count)); } break; - // Following are ArduPilot dialect messages + // Following are ArduPilot dialect messages +#if !defined(NO_ARDUPILOT_DIALECT) + case MAVLINK_MSG_ID_CAMERA_FEEDBACK: + _handleCameraFeedback(message); + break; case MAVLINK_MSG_ID_WIND: _handleWind(message); break; +#endif } // This must be emitted after the vehicle processes the message. This way the vehicle state is up to date when anyone else @@ -706,6 +708,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes } +#if !defined(NO_ARDUPILOT_DIALECT) void Vehicle::_handleCameraFeedback(const mavlink_message_t& message) { mavlink_camera_feedback_t feedback; @@ -716,6 +719,7 @@ void Vehicle::_handleCameraFeedback(const mavlink_message_t& message) qCDebug(VehicleLog) << "_handleCameraFeedback coord:index" << imageCoordinate << feedback.img_idx; _cameraTriggerPoints.append(new QGCQGeoCoordinate(imageCoordinate, this)); } +#endif void Vehicle::_handleCameraImageCaptured(const mavlink_message_t& message) { @@ -1014,6 +1018,7 @@ void Vehicle::_handleWindCov(mavlink_message_t& message) _windFactGroup.verticalSpeed()->setRawValue(0); } +#if !defined(NO_ARDUPILOT_DIALECT) void Vehicle::_handleWind(mavlink_message_t& message) { mavlink_wind_t wind; @@ -1023,6 +1028,7 @@ void Vehicle::_handleWind(mavlink_message_t& message) _windFactGroup.speed()->setRawValue(wind.speed); _windFactGroup.verticalSpeed()->setRawValue(wind.speed_z); } +#endif void Vehicle::_handleSysStatus(mavlink_message_t& message) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 5c68214cb92d658a3be543c3cdaf61a848ac3746..75bcac998a5d2522c27dbd528b2d76bac091f18f 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -891,7 +891,6 @@ private: void _handleBatteryStatus(mavlink_message_t& message); void _handleSysStatus(mavlink_message_t& message); void _handleWindCov(mavlink_message_t& message); - void _handleWind(mavlink_message_t& message); void _handleVibration(mavlink_message_t& message); void _handleExtendedSysState(mavlink_message_t& message); void _handleCommandAck(mavlink_message_t& message); @@ -906,7 +905,11 @@ private: void _handleScaledPressure(mavlink_message_t& message); void _handleScaledPressure2(mavlink_message_t& message); void _handleScaledPressure3(mavlink_message_t& message); + // ArduPilot dialect messages +#if !defined(NO_ARDUPILOT_DIALECT) void _handleCameraFeedback(const mavlink_message_t& message); + void _handleWind(mavlink_message_t& message); +#endif void _handleCameraImageCaptured(const mavlink_message_t& message); void _handleADSBVehicle(const mavlink_message_t& message); void _missionManagerError(int errorCode, const QString& errorMsg); diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index c9872fef87e8c3693fd4202c3bfe97c81da61c9e..0ae7f78fe74a00f7c8e5a7e200bcc1ba5ef7d4d7 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -883,18 +883,21 @@ void MockLink::_respondWithAutopilotVersion(void) uint8_t customVersion[8] = { }; uint32_t flightVersion = 0; +#if !defined(NO_ARDUPILOT_DIALECT) if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { flightVersion |= 3 << (8*3); flightVersion |= 5 << (8*2); flightVersion |= 0 << (8*1); flightVersion |= FIRMWARE_VERSION_TYPE_DEV << (8*0); } else if (_firmwareType == MAV_AUTOPILOT_PX4) { +#endif flightVersion |= 1 << (8*3); flightVersion |= 4 << (8*2); flightVersion |= 1 << (8*1); flightVersion |= FIRMWARE_VERSION_TYPE_DEV << (8*0); +#if !defined(NO_ARDUPILOT_DIALECT) } - +#endif mavlink_msg_autopilot_version_pack_chan(_vehicleSystemId, _vehicleComponentId, _mavlinkChannel, @@ -909,7 +912,12 @@ void MockLink::_respondWithAutopilotVersion(void) (uint8_t *)&customVersion, // os_custom_version, 0, // vendor_id, 0, // product_id, - 0); // uid + 0 // uid +#if defined(NO_ARDUPILOT_DIALECT) + //-- Once the MAVLink module is updated, this should show up. In the mean time, it's disabled. + ,0 // uid2 +#endif + ); respondWithMavlinkMessage(msg); }