Browse Source

Used NO_ARDUPILOT_DIALECT for RangeFinder Message Callback

QGC4.4
Harsh Mittal 2 years ago committed by Philipp Borgers
parent
commit
569e36e755
  1. 5
      QGCExternalLibs.pri
  2. 26
      src/Vehicle/Vehicle.cc
  3. 2
      src/Vehicle/Vehicle.h

5
QGCExternalLibs.pri

@ -58,11 +58,6 @@ isEmpty(MAVLINK_CONF) { @@ -58,11 +58,6 @@ isEmpty(MAVLINK_CONF) {
}
}
equals (MAVLINK_CONF, ardupilotmega) {
MAVLINK_CONF_ARDUPILOTMEGA = 1
DEFINES += MAVLINK_CONF_ARDUPILOTMEGA=$$MAVLINK_CONF_ARDUPILOTMEGA
}
# If defined, all APM specific MAVLink messages are disabled
contains (CONFIG, QGC_DISABLE_APM_MAVLINK) {
message("Disable APM MAVLink support")

26
src/Vehicle/Vehicle.cc

@ -729,11 +729,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes @@ -729,11 +729,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_VFR_HUD:
_handleVfrHud(message);
break;
#ifdef MAVLINK_CONF_ARDUPILOTMEGA
case MAVLINK_MSG_ID_RANGEFINDER:
_handleRangefinder(message);
break;
#endif
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
_handleNavControllerOutput(message);
break;
@ -795,6 +790,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes @@ -795,6 +790,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_CAMERA_FEEDBACK:
_handleCameraFeedback(message);
break;
case MAVLINK_MSG_ID_RANGEFINDER:
_handleRangefinder(message);
break;
#endif
}
@ -816,6 +814,14 @@ void Vehicle::_handleCameraFeedback(const mavlink_message_t& message) @@ -816,6 +814,14 @@ void Vehicle::_handleCameraFeedback(const mavlink_message_t& message)
qCDebug(VehicleLog) << "_handleCameraFeedback coord:index" << imageCoordinate << feedback.img_idx;
_cameraTriggerPoints.append(new QGCQGeoCoordinate(imageCoordinate, this));
}
void Vehicle::_handleRangefinder(mavlink_message_t& message)
{
mavlink_rangefinder_t rangefinder;
mavlink_msg_rangefinder_decode(&message, &rangefinder);
_rangeFinderDistFact.setRawValue(qIsNaN(rangefinder.distance) ? 0 : rangefinder.distance);
}
#endif
void Vehicle::_handleOrbitExecutionStatus(const mavlink_message_t& message)
@ -1015,16 +1021,6 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message) @@ -1015,16 +1021,6 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message)
_altitudeTuningFact.setRawValue(vfrHud.alt - _altitudeTuningOffset);
}
void Vehicle::_handleRangefinder(mavlink_message_t& message)
{
#ifdef MAVLINK_CONF_ARDUPILOTMEGA
mavlink_rangefinder_t rangefinder;
mavlink_msg_rangefinder_decode(&message, &rangefinder);
_rangeFinderDistFact.setRawValue(qIsNaN(rangefinder.distance) ? 0 : rangefinder.distance);
#endif
}
void Vehicle::_handleNavControllerOutput(mavlink_message_t& message)
{
mavlink_nav_controller_output_t navControllerOutput;

2
src/Vehicle/Vehicle.h

@ -1036,7 +1036,6 @@ private: @@ -1036,7 +1036,6 @@ private:
void _handleGlobalPositionInt (mavlink_message_t& message);
void _handleAltitude (mavlink_message_t& message);
void _handleVfrHud (mavlink_message_t& message);
void _handleRangefinder (mavlink_message_t& message);
void _handleNavControllerOutput (mavlink_message_t& message);
void _handleHighLatency (mavlink_message_t& message);
void _handleHighLatency2 (mavlink_message_t& message);
@ -1051,6 +1050,7 @@ private: @@ -1051,6 +1050,7 @@ private:
// ArduPilot dialect messages
#if !defined(NO_ARDUPILOT_DIALECT)
void _handleCameraFeedback (const mavlink_message_t& message);
void _handleRangefinder (mavlink_message_t& message);
#endif
void _handleCameraImageCaptured (const mavlink_message_t& message);
void _handleADSBVehicle (const mavlink_message_t& message);

Loading…
Cancel
Save