diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 5bb172898f..6951f6d8c4 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -582,7 +582,7 @@ void RangeFinder::update(void) continue; } drivers[i]->update(); - update_pre_arm_check(i); + drivers[i]->update_pre_arm_check(); } } } @@ -843,32 +843,6 @@ bool RangeFinder::pre_arm_check() const return true; } -/* - set pre-arm checks to passed if the range finder has been exercised through a reasonable range of movement - max distance sensed is at least 50cm > min distance sensed - max distance < 200cm - min distance sensed is within 10cm of ground clearance or sensor's minimum distance - */ -void RangeFinder::update_pre_arm_check(uint8_t instance) -{ - // return immediately if already passed or no sensor data - if (state[instance].pre_arm_check || state[instance].status == RangeFinder_NotConnected || state[instance].status == RangeFinder_NoData) { - return; - } - - // update min, max captured distances - state[instance].pre_arm_distance_min = MIN(state[instance].distance_cm, state[instance].pre_arm_distance_min); - state[instance].pre_arm_distance_max = MAX(state[instance].distance_cm, state[instance].pre_arm_distance_max); - - // Check that the range finder has been exercised through a realistic range of movement - if (((state[instance].pre_arm_distance_max - state[instance].pre_arm_distance_min) > RANGEFINDER_PREARM_REQUIRED_CHANGE_CM) && - (state[instance].pre_arm_distance_max < RANGEFINDER_PREARM_ALT_MAX_CM) && - ((int16_t)state[instance].pre_arm_distance_min < (MAX(state[instance].ground_clearance_cm,min_distance_cm(instance)) + 10)) && - ((int16_t)state[instance].pre_arm_distance_min > (MIN(state[instance].ground_clearance_cm,min_distance_cm(instance)) - 10))) { - state[instance].pre_arm_check = true; - } -} - const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) const { uint8_t i=0; diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 8903428eb6..56d9cf81f3 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -209,6 +209,5 @@ private: void detect_instance(uint8_t instance); void update_instance(uint8_t instance); - void update_pre_arm_check(uint8_t instance); bool _add_backend(AP_RangeFinder_Backend *driver); }; diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp index f116a78bda..8de913780e 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.cpp @@ -58,3 +58,29 @@ void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_Status status) state.range_valid_count = 0; } } + +/* + set pre-arm checks to passed if the range finder has been exercised through a reasonable range of movement + max distance sensed is at least 50cm > min distance sensed + max distance < 200cm + min distance sensed is within 10cm of ground clearance or sensor's minimum distance + */ +void AP_RangeFinder_Backend::update_pre_arm_check() +{ + // return immediately if already passed or no sensor data + if (state.pre_arm_check || state.status == RangeFinder::RangeFinder_NotConnected || state.status == RangeFinder::RangeFinder_NoData) { + return; + } + + // update min, max captured distances + state.pre_arm_distance_min = MIN(state.distance_cm, state.pre_arm_distance_min); + 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) && + (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))) { + state.pre_arm_check = true; + } +} diff --git a/libraries/AP_RangeFinder/RangeFinder_Backend.h b/libraries/AP_RangeFinder/RangeFinder_Backend.h index 95b7ebc078..1b99b8c1d8 100644 --- a/libraries/AP_RangeFinder/RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/RangeFinder_Backend.h @@ -38,6 +38,8 @@ public: virtual void handle_msg(mavlink_message_t *msg) { return; } + void update_pre_arm_check(); + protected: // update status based on distance measurement