mirror of https://github.com/ArduPilot/ardupilot
AP_RangeFinder: Remove pre arm distance check
This commit is contained in:
parent
465d6b4dc7
commit
b31cd1adb5
|
@ -311,10 +311,6 @@ void RangeFinder::init(enum Rotation orientation_default)
|
|||
// present (although it may not be healthy)
|
||||
num_instances = i+1;
|
||||
}
|
||||
// initialise pre-arm check variables
|
||||
state[i].pre_arm_check = false;
|
||||
state[i].pre_arm_distance_min = 9999; // initialise to an arbitrary large value
|
||||
state[i].pre_arm_distance_max = 0;
|
||||
|
||||
// initialise status
|
||||
state[i].status = RangeFinder_NotConnected;
|
||||
|
@ -337,7 +333,6 @@ void RangeFinder::update(void)
|
|||
continue;
|
||||
}
|
||||
drivers[i]->update();
|
||||
drivers[i]->update_pre_arm_check();
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -631,22 +626,6 @@ uint8_t RangeFinder::range_valid_count_orient(enum Rotation orientation) const
|
|||
return backend->range_valid_count();
|
||||
}
|
||||
|
||||
/*
|
||||
returns true if pre-arm checks have passed for all range finders
|
||||
these checks involve the user lifting or rotating the vehicle so that sensor readings between
|
||||
the min and 2m can be captured
|
||||
*/
|
||||
bool RangeFinder::pre_arm_check() const
|
||||
{
|
||||
for (uint8_t i=0; i<num_instances; i++) {
|
||||
// if driver is valid but pre_arm_check is false, return false
|
||||
if ((drivers[i] != nullptr) && (params[i].type != RangeFinder_TYPE_NONE) && !state[i].pre_arm_check) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) const
|
||||
{
|
||||
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
||||
|
|
|
@ -91,9 +91,6 @@ public:
|
|||
uint16_t voltage_mv; // voltage in millivolts, if applicable, otherwise 0
|
||||
enum RangeFinder_Status status; // sensor status
|
||||
uint8_t range_valid_count; // number of consecutive valid readings (maxes out at 10)
|
||||
bool pre_arm_check; // true if sensor has passed pre-arm checks
|
||||
uint16_t pre_arm_distance_min; // min distance captured during pre-arm checks
|
||||
uint16_t pre_arm_distance_max; // max distance captured during pre-arm checks
|
||||
uint32_t last_reading_ms; // system time of last successful update from sensor
|
||||
|
||||
const struct AP_Param::GroupInfo *var_info;
|
||||
|
@ -154,13 +151,6 @@ public:
|
|||
estimated_terrain_height = height;
|
||||
}
|
||||
|
||||
/*
|
||||
returns true if pre-arm checks have passed for all range finders
|
||||
these checks involve the user lifting or rotating the vehicle so that sensor readings between
|
||||
the min and 2m can be captured
|
||||
*/
|
||||
bool pre_arm_check() const;
|
||||
|
||||
static RangeFinder *get_singleton(void) { return _singleton; }
|
||||
|
||||
protected:
|
||||
|
|
|
@ -79,28 +79,3 @@ void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_Status _status)
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
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(params.ground_clearance_cm,params.min_distance_cm) + 10)) &&
|
||||
((int16_t)state.pre_arm_distance_min > (MIN(params.ground_clearance_cm,params.min_distance_cm) - 10))) {
|
||||
state.pre_arm_check = true;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -33,8 +33,6 @@ public:
|
|||
|
||||
virtual void handle_msg(mavlink_message_t *msg) { return; }
|
||||
|
||||
void update_pre_arm_check();
|
||||
|
||||
enum Rotation orientation() const { return (Rotation)params.orientation.get(); }
|
||||
uint16_t distance_cm() const { return state.distance_cm; }
|
||||
uint16_t voltage_mv() const { return state.voltage_mv; }
|
||||
|
|
Loading…
Reference in New Issue