Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: send OPTICAL_FLOW msg to GCS

Browse Source
This commit is contained in:
Randy Mackay 2014-12-08 14:07:49 +09:00
parent 67f48168a8
commit f4fc910fea
1 changed files with 34 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
ArduPlane/GCS_Mavlink.pde
View File

@ -478,6 +478,32 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
mavlink_msg_mission_current_send(chan, mission.get_current_nav_index());
}
#if OPTFLOW == ENABLED
static void NOINLINE send_opticalflow(mavlink_channel_t chan)
{
// exit immediately if no optical flow sensor or not healthy
if (!optflow.healthy()) {
return;
}
// get rates from sensor
const Vector2f &flowRate = optflow.flowRate();
const Vector2f &bodyRate = optflow.bodyRate();
// populate and send message
mavlink_msg_optical_flow_send(
chan,
millis(),
0, // sensor id is zero
flowRate.x,
flowRate.y,
bodyRate.x,
bodyRate.y,
optflow.quality(),
0); // ground distance (in meters) set to zero
}
#endif // OPTFLOW == ENABLED
static void NOINLINE send_statustext(mavlink_channel_t chan)
{
mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
@ -687,6 +713,13 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
#endif // MOUNT == ENABLED
break;
case MSG_OPTICAL_FLOW:
#if OPTFLOW == ENABLED
CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
send_opticalflow(chan);
#endif
break;
case MSG_RETRY_DEFERRED:
break; // just here to prevent a warning
@ -920,6 +953,7 @@ GCS_MAVLINK::data_stream_send(void)
#endif
send_message(MSG_BATTERY2);
send_message(MSG_MOUNT_STATUS);
send_message(MSG_OPTICAL_FLOW);
}
}