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

Copter: send OPTICAL_FLOW msg to GCS

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

34
ArduCopter/GCS_Mavlink.pde
View File

@ -441,6 +441,32 @@ static void NOINLINE send_rangefinder(mavlink_channel_t chan)
}
#endif
#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;
@ -645,6 +671,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_FENCE_STATUS:
case MSG_WIND:
// unused
@ -871,6 +904,7 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_TERRAIN);
#endif
send_message(MSG_MOUNT_STATUS);
send_message(MSG_OPTICAL_FLOW);
}
}