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)
|
// present (although it may not be healthy)
|
||||||
num_instances = i+1;
|
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
|
// initialise status
|
||||||
state[i].status = RangeFinder_NotConnected;
|
state[i].status = RangeFinder_NotConnected;
|
||||||
|
@ -337,7 +333,6 @@ void RangeFinder::update(void)
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
drivers[i]->update();
|
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();
|
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
|
const Vector3f &RangeFinder::get_pos_offset_orient(enum Rotation orientation) const
|
||||||
{
|
{
|
||||||
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
AP_RangeFinder_Backend *backend = find_instance(orientation);
|
||||||
|
|
|
@ -91,9 +91,6 @@ public:
|
||||||
uint16_t voltage_mv; // voltage in millivolts, if applicable, otherwise 0
|
uint16_t voltage_mv; // voltage in millivolts, if applicable, otherwise 0
|
||||||
enum RangeFinder_Status status; // sensor status
|
enum RangeFinder_Status status; // sensor status
|
||||||
uint8_t range_valid_count; // number of consecutive valid readings (maxes out at 10)
|
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
|
uint32_t last_reading_ms; // system time of last successful update from sensor
|
||||||
|
|
||||||
const struct AP_Param::GroupInfo *var_info;
|
const struct AP_Param::GroupInfo *var_info;
|
||||||
|
@ -154,13 +151,6 @@ public:
|
||||||
estimated_terrain_height = height;
|
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; }
|
static RangeFinder *get_singleton(void) { return _singleton; }
|
||||||
|
|
||||||
protected:
|
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; }
|
virtual void handle_msg(mavlink_message_t *msg) { return; }
|
||||||
|
|
||||||
void update_pre_arm_check();
|
|
||||||
|
|
||||||
enum Rotation orientation() const { return (Rotation)params.orientation.get(); }
|
enum Rotation orientation() const { return (Rotation)params.orientation.get(); }
|
||||||
uint16_t distance_cm() const { return state.distance_cm; }
|
uint16_t distance_cm() const { return state.distance_cm; }
|
||||||
uint16_t voltage_mv() const { return state.voltage_mv; }
|
uint16_t voltage_mv() const { return state.voltage_mv; }
|
||||||
|
|
Loading…
Reference in New Issue