Rover: use new AP_RangeFinder_analog class

APMrover2/APMrover2.pde

@@ -268,9 +268,7 @@ AP_HAL::AnalogSource * batt_curr_pin;
 // SONAR selection
 ////////////////////////////////////////////////////////////////////////////////
 //
-ModeFilterInt16_Size5 sonar_mode_filter(2);
-AP_HAL::AnalogSource *sonar_analog_source;
-AP_RangeFinder_MaxsonarXL *sonar;
+static AP_RangeFinder_analog sonar;
 
 // relay support
 AP_Relay relay;
@@ -401,8 +399,7 @@ static uint8_t receiver_rssi;
 // the time when the last HEARTBEAT message arrived from a GCS
 static uint32_t last_heartbeat_ms;
 
-// The distance as reported by Sonar in cm – Values are 20 to 700 generally.
-static int16_t sonar_dist_cm;
+// Set to true when an obstacle is detected
 static bool obstacle = false;
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -580,16 +577,6 @@ void setup() {
     batt_volt_pin = hal.analogin->channel(g.battery_volt_pin);
     batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
 
- #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
-    sonar_analog_source = new AP_ADC_AnalogSource(
-        &adc, CONFIG_SONAR_SOURCE_ADC_CHANNEL, 0.25);
- #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN
-    sonar_analog_source = hal.analogin->channel(
-        CONFIG_SONAR_SOURCE_ANALOG_PIN);
-    sonar = new AP_RangeFinder_MaxsonarXL(sonar_analog_source,
-                                          &sonar_mode_filter);
- #endif
-
 	init_ardupilot();
 }
 
@@ -668,7 +655,7 @@ static void fast_loop()
 	// Read Sonar
 	// ----------
 	if (g.sonar_enabled) {
-		sonar_dist_cm = sonar->read();
+		float sonar_dist_cm = sonar.distance_cm();
         if (sonar_dist_cm <= g.sonar_trigger_cm)  {
             // obstacle detected in front 
             obstacle = true;

APMrover2/GCS_Mavlink.pde

@@ -420,6 +420,14 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
         hal.i2c->lockup_count());
 }
 
+static void NOINLINE send_rangefinder(mavlink_channel_t chan)
+{
+    mavlink_msg_rangefinder_send(
+        chan,
+        sonar.distance_cm() * 0.01,
+        sonar.voltage());
+}
+
 static void NOINLINE send_current_waypoint(mavlink_channel_t chan)
 {
     mavlink_msg_mission_current_send(
@@ -584,6 +592,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
         send_hwstatus(chan);
         break;
 
+    case MSG_RANGEFINDER:
+        CHECK_PAYLOAD_SIZE(RANGEFINDER);
+        send_rangefinder(chan);
+        break;
+
     case MSG_RETRY_DEFERRED:
         break; // just here to prevent a warning
 	}
@@ -862,6 +875,7 @@ GCS_MAVLINK::data_stream_send(void)
     if (stream_trigger(STREAM_EXTRA3)) {
         send_message(MSG_AHRS);
         send_message(MSG_HWSTATUS);
+        send_message(MSG_RANGEFINDER);
     }
 }
 

APMrover2/Parameters.h

@@ -98,6 +98,7 @@ public:
 
         // obstacle control
         k_param_sonar_enabled = 190,
+        k_param_sonar, // sonar object
         k_param_sonar_type,
         k_param_sonar_trigger_cm,
         k_param_sonar_turn_angle,

APMrover2/Parameters.pde

@@ -356,6 +356,10 @@ const AP_Param::Info var_info[] PROGMEM = {
 	GOBJECT(gcs0,					"SR0_",     GCS_MAVLINK),
 	GOBJECT(gcs3,					"SR3_",     GCS_MAVLINK),
 
+    // @Group: SONAR_
+    // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
+    GOBJECT(sonar,                  "SONAR_", AP_RangeFinder_analog),
+
 #if HIL_MODE == HIL_MODE_DISABLED
     // @Group: INS_
     // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp

APMrover2/defines.h

@@ -122,6 +122,7 @@ enum ap_message {
     MSG_AHRS,
     MSG_SIMSTATE,
     MSG_HWSTATUS,
+    MSG_RANGEFINDER,
     MSG_RETRY_DEFERRED // this must be last
 };
 

APMrover2/sensors.pde

@@ -1,13 +1,12 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
 static void init_sonar(void)
 {
-  /*
-    #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
-	    sonar.calculate_scaler(g.sonar_type, 3.3);
-	#else
-        sonar.calculate_scaler(g.sonar_type, 5.0);
-	#endif
-*/
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+    sonar.Init(&adc);
+#else
+    sonar.Init(NULL);
+#endif
 }
 
 // Sensors are not available in HIL_MODE_ATTITUDE

APMrover2/system.pde

@@ -180,8 +180,10 @@ static void init_ardupilot()
             //compass.get_offsets();						// load offsets to account for airframe magnetic interference
         }
 	}
+
 	// initialise sonar
-	init_sonar();
+    init_sonar();
+
 #endif
 	// Do GPS init
 	g_gps = &g_gps_driver;

APMrover2/test.pde

@@ -551,14 +551,14 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
     }
 
     print_hit_enter();
-	delay(1000);
-	init_sonar();
-	delay(1000);
+    init_sonar();
 
 	while (true) {
-        delay(20);
-        sonar_dist_cm = sonar->read();
-        cliSerial->printf_P(PSTR("sonar distance = %u cm\n"), (unsigned)sonar_dist_cm);
+        delay(200);
+        float sonar_dist_cm = sonar.distance_cm();
+        float voltage = sonar.voltage();
+        cliSerial->printf_P(PSTR("sonar distance=%5.1fcm  voltage=%5.2f\n"), 
+                            sonar_dist_cm, voltage);
         if (cliSerial->available() > 0) {
             break;
 	    }