From 3be9f352640cc936da1f7a5a2e8b27708353e78b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 3 Oct 2018 09:43:50 +1000 Subject: [PATCH] AP_RangeFinder: remove rangefinder prearm requirements for SITL Getting the virtual rangefinder to move in prearm would be better, much much more involved. --- libraries/AP_RangeFinder/RangeFinder.h | 4 ++++ libraries/AP_RangeFinder/RangeFinder_Backend.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 391c66b846..c56adfd7f1 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -24,7 +24,11 @@ #define RANGEFINDER_MAX_INSTANCES 2 #define RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT 10 #define RANGEFINDER_PREARM_ALT_MAX_CM 200 +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM 0 +#else #define RANGEFINDER_PREARM_REQUIRED_CHANGE_CM 50 +#endif class AP_RangeFinder_Backend; diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp index 2ad29b9849..8cd27ac94e 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp @@ -97,7 +97,7 @@ void AP_RangeFinder_Backend::update_pre_arm_check() state.pre_arm_distance_max = MAX(state.distance_cm, state.pre_arm_distance_max); // Check that the range finder has been exercised through a realistic range of movement - if (((state.pre_arm_distance_max - state.pre_arm_distance_min) > RANGEFINDER_PREARM_REQUIRED_CHANGE_CM) && + if (((state.pre_arm_distance_max - state.pre_arm_distance_min) >= RANGEFINDER_PREARM_REQUIRED_CHANGE_CM) && (state.pre_arm_distance_max < RANGEFINDER_PREARM_ALT_MAX_CM) && ((int16_t)state.pre_arm_distance_min < (MAX(state.ground_clearance_cm,state.min_distance_cm) + 10)) && ((int16_t)state.pre_arm_distance_min > (MIN(state.ground_clearance_cm,state.min_distance_cm) - 10))) {