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 sonar distance to GCS via MAVLink

Browse Source 
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.
This commit is contained in:
Ben Nizette 2014-05-16 12:54:24 +08:00 committed by Randy Mackay
parent 9e7e2eb106
commit 8c7b4feac1
1 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/GCS_Mavlink.pde
View File

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