mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-11 17:13:56 -03:00
AP_RangeFinder: move prearm checks into backend
This commit is contained in:
parent
0b1c67d170
commit
127edce39e
@ -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;
|
||||
|
@ -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);
|
||||
};
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user