From 261eb387ebccc70067c0daa5b42102e500bc3a8a Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Tue, 30 May 2017 16:15:32 +0200 Subject: [PATCH] ArduCopter: change rangefinder msg for common one --- ArduCopter/Copter.h | 1 - ArduCopter/GCS_Mavlink.cpp | 16 +--------------- 2 files changed, 1 insertion(+), 16 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index e25c50901e..0fdf847bb2 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -703,7 +703,6 @@ private: void send_hwstatus(mavlink_channel_t chan); void send_vfr_hud(mavlink_channel_t chan); void send_current_waypoint(mavlink_channel_t chan); - void send_rangefinder(mavlink_channel_t chan); void send_proximity(mavlink_channel_t chan, uint16_t count_max); void send_rpm(mavlink_channel_t chan); void rpm_update(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index c927e1867b..7331c0596d 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -207,20 +207,6 @@ void NOINLINE Copter::send_current_waypoint(mavlink_channel_t chan) mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); } -#if RANGEFINDER_ENABLED == ENABLED -void NOINLINE Copter::send_rangefinder(mavlink_channel_t chan) -{ - // exit immediately if rangefinder is disabled - if (!rangefinder.has_data_orient(ROTATION_PITCH_270)) { - return; - } - mavlink_msg_rangefinder_send( - chan, - rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f, - rangefinder.voltage_mv_orient(ROTATION_PITCH_270) * 0.001f); -} -#endif - void NOINLINE Copter::send_proximity(mavlink_channel_t chan, uint16_t count_max) { #if PROXIMITY_ENABLED == ENABLED @@ -472,7 +458,7 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) case MSG_RANGEFINDER: #if RANGEFINDER_ENABLED == ENABLED CHECK_PAYLOAD_SIZE(RANGEFINDER); - copter.send_rangefinder(chan); + send_rangefinder(copter.rangefinder); CHECK_PAYLOAD_SIZE(DISTANCE_SENSOR); send_distance_sensor_downward(copter.rangefinder); #endif