diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp index d16179e31a..7c1cf4fd5d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp @@ -63,6 +63,12 @@ const AP_Param::GroupInfo AP_RangeFinder_analog::var_info[] PROGMEM = { // @Increment: 1 AP_GROUPINFO("MAX_CM", 5, AP_RangeFinder_analog, _max_distance_cm, 700), + // @Param: ENABLE + // @DisplayName: Sonar enabled + // @Description: set to 1 to enable this sonar + // @Values: 0:Disabled,1:Enabled + AP_GROUPINFO("ENABLE", 6, AP_RangeFinder_analog, _enabled, 0), + AP_GROUPEND }; @@ -81,6 +87,9 @@ AP_RangeFinder_analog::AP_RangeFinder_analog(void) */ void AP_RangeFinder_analog::Init(void *adc) { + if (!_enabled) { + return; + } if (_source != NULL) { return; } @@ -100,8 +109,11 @@ void AP_RangeFinder_analog::Init(void *adc) */ float AP_RangeFinder_analog::voltage(void) { + if (!_enabled) { + return 0.0f; + } if (_source == NULL) { - return 0.0; + return 0.0f; } // check for pin changes if (_last_pin != 127 && _last_pin != _pin) { @@ -116,6 +128,10 @@ float AP_RangeFinder_analog::voltage(void) */ float AP_RangeFinder_analog::distance_cm(void) { + if (!_enabled) { + return 0.0f; + } + /* first convert to volts */ float v = voltage(); float dist_m = 0; @@ -150,6 +166,9 @@ float AP_RangeFinder_analog::distance_cm(void) */ bool AP_RangeFinder_analog::in_range(void) { + if (!_enabled) { + return false; + } float dist_cm = distance_cm(); if (dist_cm >= _max_distance_cm) { return false; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.h b/libraries/AP_RangeFinder/AP_RangeFinder_analog.h index df94dc8f55..15c07080c9 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_analog.h @@ -24,6 +24,9 @@ public: // return true if the sonar is in the configured range bool in_range(void); + // return true if enabled + bool enabled(void) { return (bool)_enabled.get(); } + enum RangeFinder_Function { FUNCTION_LINEAR = 0, FUNCTION_INVERTED = 1, @@ -41,6 +44,7 @@ private: AP_Int16 _min_distance_cm; AP_Int16 _max_distance_cm; int8_t _last_pin; + AP_Int8 _enabled; }; #endif // __AP_RangeFinder_analog_H__