Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AVOIDANCE: Minimum altitude for avoidance action

Browse Source
This commit is contained in:
Matt 2017-12-27 21:44:34 -05:00 committed by Tom Pittenger
parent 0fb679b2b2
commit b5bdaa18ac
2 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Avoidance/AP_Avoidance.cpp
View File

@ -115,6 +115,13 @@ const AP_Param::GroupInfo AP_Avoidance::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("F_DIST_Z", 11, AP_Avoidance, _fail_distance_z, AP_AVOIDANCE_FAIL_DISTANCE_Z_DEFAULT), AP_GROUPINFO("F_DIST_Z", 11, AP_Avoidance, _fail_distance_z, AP_AVOIDANCE_FAIL_DISTANCE_Z_DEFAULT),
// @Param: F_ALT_MIN
// @DisplayName: ADS-B avoidance minimum altitude
// @Description: Minimum altitude for ADS-B avoidance. If the vehicle is below this altitude, no avoidance action will take place. Useful to prevent ADS-B avoidance from activating while below the tree line or around structures. Default of 0 is no minimum.
// @Units: m
// @User: Advanced
AP_GROUPINFO("F_ALT_MIN", 12, AP_Avoidance, _fail_altitude_minimum, 0),
AP_GROUPEND AP_GROUPEND
}; };
@ -511,7 +518,13 @@ void AP_Avoidance::handle_avoidance_local(AP_Avoidance::Obstacle *threat)
new_threat_level = threat->threat_level; new_threat_level = threat->threat_level;
if (new_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) { if (new_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) {
action = (MAV_COLLISION_ACTION)_fail_action.get(); action = (MAV_COLLISION_ACTION)_fail_action.get();
} Location my_loc;
if (action != MAV_COLLISION_ACTION_NONE && _fail_altitude_minimum > 0 &&
_ahrs.get_position(my_loc) && ((my_loc.alt*0.01f) < _fail_altitude_minimum)) {
// disable avoidance when close to ground, report only
action = MAV_COLLISION_ACTION_REPORT;
}
}
} }
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();

1
libraries/AP_Avoidance/AP_Avoidance.h
View File

@ -189,6 +189,7 @@ private:
AP_Int8 _fail_time_horizon; AP_Int8 _fail_time_horizon;
AP_Int16 _fail_distance_xy; AP_Int16 _fail_distance_xy;
AP_Int16 _fail_distance_z; AP_Int16 _fail_distance_z;
AP_Int16 _fail_altitude_minimum;
AP_Int8 _warn_action; AP_Int8 _warn_action;
AP_Int8 _warn_time_horizon; AP_Int8 _warn_time_horizon;