Rover: convert to new AP_RangeFinder API

APMrover2/APMrover2.pde

@@ -298,11 +298,8 @@ static GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
 AP_HAL::AnalogSource *rssi_analog_source;
 
 ////////////////////////////////////////////////////////////////////////////////
-// SONAR selection
-////////////////////////////////////////////////////////////////////////////////
-//
-static AP_RangeFinder_analog sonar;
-static AP_RangeFinder_analog sonar2;
+// SONAR
+static RangeFinder sonar;
 
 // relay support
 AP_Relay relay;

APMrover2/GCS_Mavlink.pde

@@ -389,7 +389,7 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
 
 static void NOINLINE send_rangefinder(mavlink_channel_t chan)
 {
-    if (!sonar.enabled()) {
+    if (!sonar.healthy()) {
         // no sonar to report
         return;
     }
@@ -398,18 +398,18 @@ static void NOINLINE send_rangefinder(mavlink_channel_t chan)
       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();
+    if (!sonar.healthy(1)) {
+        distance_cm = sonar.distance_cm(0);
+        voltage = sonar.voltage_mv(0) * 0.001f;
     } else {
-        float dist1 = sonar.distance_cm();
-        float dist2 = sonar2.distance_cm();
+        float dist1 = sonar.distance_cm(0);
+        float dist2 = sonar.distance_cm(1);
         if (dist1 <= dist2) {
             distance_cm = dist1;
-            voltage = sonar.voltage();
+            voltage = sonar.voltage_mv(0) * 0.001f;
         } else {
             distance_cm = dist2;
-            voltage = sonar2.voltage();
+            voltage = sonar.voltage_mv(1) * 0.001f;
         }
     }
     mavlink_msg_rangefinder_send(

APMrover2/Log.pde

@@ -368,8 +368,8 @@ static void Log_Write_Sonar()
         LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
         time_ms         : millis(),
         lateral_accel   : lateral_acceleration,
-        sonar1_distance : (uint16_t)sonar.distance_cm(),
-        sonar2_distance : (uint16_t)sonar2.distance_cm(),
+        sonar1_distance : (uint16_t)sonar.distance_cm(0),
+        sonar2_distance : (uint16_t)sonar.distance_cm(1),
         detected_count  : obstacle.detected_count,
         turn_angle      : (int8_t)obstacle.turn_angle,
         turn_time       : turn_time,

APMrover2/Parameters.h

@@ -129,12 +129,13 @@ public:
 
         // obstacle control
         k_param_sonar_enabled = 190, // deprecated, can be removed
-        k_param_sonar, // sonar object
+        k_param_sonar_old, // unused
         k_param_sonar_trigger_cm,
         k_param_sonar_turn_angle,
         k_param_sonar_turn_time,
-        k_param_sonar2, // sonar2 object
+        k_param_sonar2_old, // unused
         k_param_sonar_debounce,
+        k_param_sonar, // sonar object
         
         //
         // 210: driving modes

APMrover2/Parameters.pde

@@ -482,13 +482,9 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
     GOBJECT(L1_controller,         "NAVL1_",   AP_L1_Control),
 
-    // @Group: SONAR_
-    // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
-    GOBJECT(sonar,                  "SONAR_", AP_RangeFinder_analog),
-
-    // @Group: SONAR2_
-    // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
-    GOBJECT(sonar2,                 "SONAR2_", AP_RangeFinder_analog),
+    // @Group: SONAR
+    // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
+    GOBJECT(sonar,                 "SONAR", RangeFinder),
 
     // @Group: INS_
     // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp

APMrover2/defines.h

@@ -71,7 +71,6 @@ enum mode {
 #define LOG_ATTITUDE_MSG        0x08
 #define LOG_MODE_MSG            0x09
 #define LOG_COMPASS_MSG         0x0A
-#define LOG_CAMERA_MSG          0x0B    // deprecated
 #define LOG_COMPASS2_MSG        0x0C
 #define LOG_STEERING_MSG        0x0D
 #define LOG_COMPASS3_MSG        0x0E

APMrover2/sensors.pde

@@ -9,13 +9,7 @@ static void init_barometer(void)
 
 static void init_sonar(void)
 {
-#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
-    sonar.Init(&adc);
-    sonar2.Init(&adc);
-#else
-    sonar.Init(NULL);
-    sonar2.Init(NULL);
-#endif
+    sonar.init();
 }
 
 // read_battery - reads battery voltage and current and invokes failsafe
@@ -37,15 +31,17 @@ void read_receiver_rssi(void)
 // read the sonars
 static void read_sonars(void)
 {
-    if (!sonar.enabled()) {
+    sonar.update();
+
+    if (!sonar.healthy()) {
         // this makes it possible to disable sonar at runtime
         return;
     }
 
-    if (sonar2.enabled()) {
+    if (sonar.healthy(1)) {
         // we have two sonars
-        obstacle.sonar1_distance_cm = sonar.distance_cm();
-        obstacle.sonar2_distance_cm = sonar2.distance_cm();
+        obstacle.sonar1_distance_cm = sonar.distance_cm(0);
+        obstacle.sonar2_distance_cm = sonar.distance_cm(1);
         if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm &&
             obstacle.sonar2_distance_cm <= (uint16_t)obstacle.sonar2_distance_cm)  {
             // we have an object on the left
@@ -72,7 +68,7 @@ static void read_sonars(void)
         }
     } else {
         // we have a single sonar
-        obstacle.sonar1_distance_cm = sonar.distance_cm();
+        obstacle.sonar1_distance_cm = sonar.distance_cm(0);
         obstacle.sonar2_distance_cm = 0;
         if (obstacle.sonar1_distance_cm <= (uint16_t)g.sonar_trigger_cm)  {
             // obstacle detected in front 

APMrover2/test.pde

@@ -455,7 +455,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_sonar(uint8_t argc, const Menu::arg *argv)
 {
-    if (!sonar.enabled()) {
+    if (!sonar.healthy()) {
         cliSerial->println_P(PSTR("WARNING: Sonar is not enabled"));
     }
 
@@ -474,8 +474,8 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
         delay(20);
         uint32_t now = millis();
 
-        float dist_cm = sonar.distance_cm();
-        float voltage = sonar.voltage();
+        float dist_cm = sonar.distance_cm(0);
+        float voltage = sonar.voltage_mv(0);
         if (sonar_dist_cm_min == 0.0f) {
             sonar_dist_cm_min = dist_cm;
             voltage_min = voltage;
@@ -485,8 +485,8 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
         voltage_min = min(voltage_min, voltage);
         voltage_max = max(voltage_max, voltage);
 
-        dist_cm = sonar2.distance_cm();
-        voltage = sonar2.voltage();
+        dist_cm = sonar.distance_cm(1);
+        voltage = sonar.voltage_mv(1);
         if (sonar2_dist_cm_min == 0.0f) {
             sonar2_dist_cm_min = dist_cm;
             voltage2_min = voltage;