diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 91d37466e1..26d60b08d0 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -65,6 +65,14 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = { // 19 was _MIN // 20 was _MAX + // @Param{Copter}: _ALT_MIN + // @DisplayName: Proximity lowest altitude. + // @Description: Minimum altitude below which proximity should not work. + // @Units: m + // @Range: 0 10 + // @User: Advanced + AP_GROUPINFO_FRAME("_ALT_MIN", 25, AP_Proximity, _alt_min, 1.0f, AP_PARAM_FRAME_COPTER | AP_PARAM_FRAME_HELI | AP_PARAM_FRAME_TRICOPTER), + // @Group: 1 // @Path: AP_Proximity_Params.cpp AP_SUBGROUPINFO(params[0], "1", 21, AP_Proximity, AP_Proximity_Params), diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 8dd2f2393a..acea4e5a3e 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -173,9 +173,10 @@ private: bool valid_instance(uint8_t i) const; // parameters for all instances - AP_Int8 _raw_log_enable; // enable logging raw distances + AP_Int8 _raw_log_enable; // enable logging raw distances AP_Int8 _ign_gnd_enable; // true if land detection should be enabled AP_Float _filt_freq; // cutoff frequency for low pass filter + AP_Float _alt_min; // Minimum altitude -in meters- below which proximity should not work. // get alt from rangefinder in meters. This reading is corrected for vehicle tilt bool get_rangefinder_alt(float &alt_m) const; diff --git a/libraries/AP_Proximity/AP_Proximity_Utils.cpp b/libraries/AP_Proximity/AP_Proximity_Utils.cpp index 966f868a79..9db47c8f73 100644 --- a/libraries/AP_Proximity/AP_Proximity_Utils.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Utils.cpp @@ -20,7 +20,6 @@ #if HAL_PROXIMITY_ENABLED -#define PROXIMITY_GND_DETECT_THRESHOLD 1.0f // set ground detection threshold to be 1 meters #define PROXIMITY_ALT_DETECT_TIMEOUT_MS 500 // alt readings should arrive within this much time extern const AP_HAL::HAL& hal; @@ -79,7 +78,7 @@ bool AP_Proximity::check_obstacle_near_ground(float pitch, float yaw, float dist if (rotated_object_3D.z > -0.5f) { // obstacle is at the most 0.5 meters above vehicle - if ((alt - PROXIMITY_GND_DETECT_THRESHOLD) < rotated_object_3D.z) { + if ((alt - _alt_min) < rotated_object_3D.z) { // obstacle is near or below ground return true; }