From 09ca9d42834ce34bb74ff9c81224254e4bcb2799 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 26 Apr 2013 14:53:15 +1000
Subject: [PATCH] AP_Rangefinder_analog: implement settle time and stop_pin

---
 .../AP_RangeFinder/AP_RangeFinder_analog.cpp  | 23 +++++++++++++------
 .../AP_RangeFinder/AP_RangeFinder_analog.h    |  3 ++-
 2 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
index 17075f924c..199f8f5e52 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp
@@ -69,6 +69,16 @@ const AP_Param::GroupInfo AP_RangeFinder_analog::var_info[] PROGMEM = {
 	// @Values: 0:Disabled,1:Enabled
     AP_GROUPINFO("ENABLE",  6, AP_RangeFinder_analog, _enabled, 0),
 
+    // @Param: STOP_PIN
+    // @DisplayName: Sonar stop pin
+    // @Description: Digital pin that enables/disables sonar measurement. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the sonar and set to 0 to disable it. This can be used to ensure that multiple sonars don't interfere with each other.
+    AP_GROUPINFO("STOP_PIN", 7, AP_RangeFinder_analog, _stop_pin, -1),
+
+    // @Param: SETTLE_MS
+    // @DisplayName: Sonar settle time
+    // @Description: The time in milliseconds that the sonar reading takes to settle. This is only used when a STOP_PIN is specified. It determines how long we have to wait for the sonar to give a reading after we set the STOP_PIN high. For a sonar with a range of around 7m this would need to be around 50 milliseconds to allow for the sonar pulse to travel to the target and back again.
+    AP_GROUPINFO("SETTLE_MS", 8, AP_RangeFinder_analog, _settle_time_ms, 0),
+
     AP_GROUPEND
 };
 
@@ -96,12 +106,12 @@ void AP_RangeFinder_analog::Init(void *adc)
 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
    if (_pin == 127) {
 	  _source = new AP_ADC_AnalogSource((AP_ADC*)adc, 7, 1.0);
-	  _last_pin = 127;
 	  return;
    }
 #endif
    _source = hal.analogin->channel(_pin);
-   _last_pin = _pin;
+   _source->set_stop_pin((uint8_t)_stop_pin);
+   _source->set_settle_time((uint16_t)_settle_time_ms);
 }
 
 /*
@@ -115,11 +125,10 @@ float AP_RangeFinder_analog::voltage(void)
    if (_source == NULL) {
 	  return 0.0f;
    }
-   // check for pin changes
-   if (_last_pin != 127 && _last_pin != _pin) {
-	  _source->set_pin(_pin);
-	  _last_pin = _pin;
-   }
+   // cope with changed settings
+   _source->set_pin(_pin);
+   _source->set_stop_pin((uint8_t)_stop_pin);
+   _source->set_settle_time((uint16_t)_settle_time_ms);
    return _source->voltage_average();
 }
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.h b/libraries/AP_RangeFinder/AP_RangeFinder_analog.h
index 15c07080c9..9eafbcea9f 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.h
@@ -38,12 +38,13 @@ public:
 private:
     AP_HAL::AnalogSource *_source;
     AP_Int8  _pin;
+    AP_Int8  _stop_pin;
+    AP_Int16 _settle_time_ms;
     AP_Float _scaling;
     AP_Float _offset;
     AP_Int8  _function;
     AP_Int16 _min_distance_cm;
     AP_Int16 _max_distance_cm;
-    int8_t _last_pin;
     AP_Int8 _enabled;
 };
 #endif // __AP_RangeFinder_analog_H__