Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: move prearm checks into backend

This commit is contained in:
Peter Barker 2017-08-07 14:20:46 +10:00 committed by Francisco Ferreira
parent 0b1c67d170
commit 127edce39e
4 changed files with 29 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -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;

1
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -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);
};

26
libraries/AP_RangeFinder/RangeFinder_Backend.cpp
View File

@ -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;
}
}

2
libraries/AP_RangeFinder/RangeFinder_Backend.h
View File

@ -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