Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: report smaller of two sonar distances

This commit is contained in:
Andrew Tridgell 2013-03-29 11:00:41 +11:00
parent ad7e8bd9c4
commit 9eaa764f42
1 changed files with 25 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
APMrover2/GCS_Mavlink.pde
View File

@ -418,10 +418,33 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
static void NOINLINE send_rangefinder(mavlink_channel_t chan)
{
if (!sonar.enabled()) {
// no sonar to report
return;
}
/*
report smaller distance of two sonars if more than one enabled
*/
float distance_cm, voltage;
if (!sonar2.enabled()) {
distance_cm = sonar.distance_cm();
voltage = sonar.voltage();
} else {
float dist1 = sonar.distance_cm();
float dist2 = sonar2.distance_cm();
if (dist1 <= dist2) {
distance_cm = dist1;
voltage = sonar.voltage();
} else {
distance_cm = dist2;
voltage = sonar2.voltage();
}
}
mavlink_msg_rangefinder_send(
chan,
sonar.distance_cm() * 0.01,
sonar.voltage());
distance_cm * 0.01f,
voltage);
}
static void NOINLINE send_current_waypoint(mavlink_channel_t chan)