|
|
|
@ -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; |
|
|
|
|