From 28e1449e8d8027499009b1bbdb30c531fabe4e2b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 27 Jun 2014 13:05:14 +1000
Subject: [PATCH] Plane: updates for new AP_RangeFinder API

---
 ArduPlane/ArduPlane.pde   |  2 +-
 ArduPlane/GCS_Mavlink.pde |  4 ++--
 ArduPlane/Log.pde         |  4 ++--
 ArduPlane/Parameters.h    |  3 ++-
 ArduPlane/Parameters.pde  |  6 +++---
 ArduPlane/sensors.pde     | 11 ++---------
 6 files changed, 12 insertions(+), 18 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index a542953a94..1013a30eb7 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -308,7 +308,7 @@ static AP_HAL::AnalogSource *rssi_analog_source;
 ////////////////////////////////////////////////////////////////////////////////
 // Sonar
 ////////////////////////////////////////////////////////////////////////////////
-static AP_RangeFinder_analog sonar;
+static RangeFinder sonar;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Relay
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 9c5cc935d7..aacc69ae49 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -520,14 +520,14 @@ static void NOINLINE send_wind(mavlink_channel_t chan)
 
 static void NOINLINE send_rangefinder(mavlink_channel_t chan)
 {
-    if (!sonar.enabled()) {
+    if (!sonar.healthy()) {
         // no sonar to report
         return;
     }
     mavlink_msg_rangefinder_send(
         chan,
         sonar.distance_cm() * 0.01f,
-        sonar.voltage());
+        sonar.voltage_mv()*0.001f);
 }
 
 static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde
index 1b001f0864..a1ec1b356c 100644
--- a/ArduPlane/Log.pde
+++ b/ArduPlane/Log.pde
@@ -361,8 +361,8 @@ static void Log_Write_Sonar()
     struct log_Sonar pkt = {
         LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
         timestamp   : hal.scheduler->millis(),
-        distance    : sonar.distance_cm(),
-        voltage     : sonar.voltage(),
+        distance    : (float)sonar.distance_cm(),
+        voltage     : sonar.voltage_mv()*0.001f,
         baro_alt    : barometer.get_altitude(),
         groundspeed : gps.ground_speed(),
         throttle    : (uint8_t)(100 * channel_throttle->norm_output())
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 0ac23a2126..976ac3e09f 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -98,7 +98,7 @@ public:
         k_param_ground_steer_dps,
         k_param_rally_limit_km_old, //unused anymore -- just holding this index
         k_param_hil_err_limit,
-        k_param_sonar,
+        k_param_sonar_old, // unused
         k_param_log_bitmask,
         k_param_BoardConfig,
         k_param_rssi_range,
@@ -115,6 +115,7 @@ public:
         k_param_takeoff_rotate_speed,
         k_param_takeoff_throttle_slewrate,
         k_param_takeoff_throttle_max,
+        k_param_sonar,
 
         // 100: Arming parameters
         k_param_arming = 100,
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index 0aa29d27fa..ccafaf3c2e 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -863,9 +863,9 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_Relay/AP_Relay.cpp
     GOBJECT(relay,                  "RELAY_", AP_Relay),
 
-    // @Group: SONAR_
-    // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
-    GOBJECT(sonar,                  "SONAR_", AP_RangeFinder_analog),
+    // @Group: SONAR
+    // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
+    GOBJECT(sonar,                  "SONAR", RangeFinder),
 
     // RC channel
     //-----------
diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde
index 89e9c63966..369e9490d8 100644
--- a/ArduPlane/sensors.pde
+++ b/ArduPlane/sensors.pde
@@ -10,20 +10,13 @@ static void init_barometer(void)
 
 static void init_sonar(void)
 {
-#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
-    sonar.Init(&apm1_adc);
-#else
-    sonar.Init(NULL);
-#endif
+    sonar.init();
 }
 
 // read the sonars
 static void read_sonars(void)
 {
-    if (!sonar.enabled()) {
-        // this makes it possible to disable sonar at runtime
-        return;
-    }
+    sonar.update();
 
     if (should_log(MASK_LOG_SONAR))
         Log_Write_Sonar();