From 0bc9d294fbbbb697dc0d2cf9a04bc05f90c447d4 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Tue, 30 May 2017 12:31:50 +0200 Subject: [PATCH] ArduPlane: change rangefinder msg for common one --- ArduPlane/GCS_Mavlink.cpp | 14 +------------- ArduPlane/Plane.h | 1 - 2 files changed, 1 insertion(+), 14 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index da00ff54c3..e2e4c9c7d8 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -414,18 +414,6 @@ void Plane::send_pid_tuning(mavlink_channel_t chan) } } -void Plane::send_rangefinder(mavlink_channel_t chan) -{ - if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) { - // no sonar to report - return; - } - mavlink_msg_rangefinder_send( - chan, - rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f, - rangefinder.voltage_mv_orient(ROTATION_PITCH_270) * 0.001f); -} - void Plane::send_current_waypoint(mavlink_channel_t chan) { mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); @@ -597,7 +585,7 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) case MSG_RANGEFINDER: CHECK_PAYLOAD_SIZE(RANGEFINDER); - plane.send_rangefinder(chan); + send_rangefinder(plane.rangefinder); CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR); send_distance_sensor_downward(plane.rangefinder); break; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 4c23186b91..392a9f0bf0 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -814,7 +814,6 @@ private: void send_wind(mavlink_channel_t chan); void send_pid_tuning(mavlink_channel_t chan); void send_rpm(mavlink_channel_t chan); - void send_rangefinder(mavlink_channel_t chan); void send_current_waypoint(mavlink_channel_t chan); void send_aoa_ssa(mavlink_channel_t chan);