From 8c7b4feac11189c3113fd23936fc7555b6daef44 Mon Sep 17 00:00:00 2001 From: Ben Nizette <bennizette@gmail.com> Date: Fri, 16 May 2014 12:54:24 +0800 Subject: [PATCH] Copter: send sonar distance to GCS via MAVLink At the moment, the copter sonar set up does not have access to the raw sonar readings so this code sets the voltage field to zero. --- ArduCopter/GCS_Mavlink.pde | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 8ca14ff77..7b665eb4d 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -578,6 +578,13 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan) mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); } +#if CONFIG_SONAR == ENABLED +static void NOINLINE send_rangefinder(mavlink_channel_t chan) +{ + mavlink_msg_rangefinder_send(chan, sonar_alt * 0.01f, 0); +} +#endif + static void NOINLINE send_statustext(mavlink_channel_t chan) { mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status; @@ -719,6 +726,13 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) gcs[chan-MAVLINK_COMM_0].queued_waypoint_send(); break; +#if CONFIG_SONAR == ENABLED + case MSG_RANGEFINDER: + CHECK_PAYLOAD_SIZE(RANGEFINDER); + send_rangefinder(chan); + break; +#endif + case MSG_STATUSTEXT: CHECK_PAYLOAD_SIZE(STATUSTEXT); send_statustext(chan); @@ -754,7 +768,6 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) case MSG_FENCE_STATUS: case MSG_WIND: - case MSG_RANGEFINDER: // unused break; @@ -1029,6 +1042,7 @@ GCS_MAVLINK::data_stream_send(void) send_message(MSG_AHRS); send_message(MSG_HWSTATUS); send_message(MSG_SYSTEM_TIME); + send_message(MSG_RANGEFINDER); } } -- GitLab