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

AP_Avoidance: Remove unutilized get_destination_perpendicular

Browse Source
This commit is contained in:
Michael du Breuil 2017-01-30 12:06:48 -07:00 committed by Andrew Tridgell
parent 97c57764c4
commit c079532a7a
1 changed files with 0 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
libraries/AP_Avoidance/AP_Avoidance.cpp
View File

@ -573,72 +573,6 @@ void AP_Avoidance::handle_msg(const mavlink_message_t &msg)
vel); vel);
} }
// wp_speeds in cm/s
bool AP_Avoidance::get_destination_perpendicular(const AP_Avoidance::Obstacle *obstacle, Vector3f &newdest_neu, const float wp_speed_xy, const float wp_speed_z, const uint8_t _minimum_avoid_height)
{
if (obstacle == nullptr) {
// why where we called?!
return false;
}
Location my_abs_pos;
if (! _ahrs.get_position(my_abs_pos)) {
// we should not get to here! If we don't know our position
// we can't know if there are any threats, for starters!
return false;
}
Vector3f my_pos_ned;
if (! _ahrs.get_relative_position_NED(my_pos_ned)) {
// we should not get to here! If we don't know our position
// we know if there are any threats, for starters!
return false;
}
// if their velocity is moving around close to zero then flying
// perpendicular to that velocity may mean we do weird things.
// Instead, we will fly directly away from them:
if (obstacle->_velocity.length() < _low_velocity_threshold) {
const Vector2f delta_pos_xy = location_diff(obstacle->_location, my_abs_pos);
const float delta_pos_z = my_abs_pos.alt - obstacle->_location.alt;
Vector3f delta_pos_xyz = Vector3f(delta_pos_xy[0],delta_pos_xy[1],delta_pos_z);
// avoid divide by zero
if (delta_pos_xyz.is_zero()) {
return false;
}
delta_pos_xyz.normalize();
newdest_neu[0] = my_pos_ned[0]*100 + delta_pos_xyz[0] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC;
newdest_neu[1] = my_pos_ned[1]*100 + delta_pos_xyz[1] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC;
newdest_neu[2] = -my_pos_ned[2]*100 + delta_pos_xyz[2] * wp_speed_z * AP_AVOIDANCE_ESCAPE_TIME_SEC;
if(newdest_neu[2] < _minimum_avoid_height*100) {
newdest_neu[0] = my_pos_ned[0]*100 + delta_pos_xy[0] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC;
newdest_neu[1] = my_pos_ned[1]*100 + delta_pos_xy[1] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC;
newdest_neu[2] = -my_pos_ned[2]*100;
}
return true;
}
{
Vector3f perp_xyz = perpendicular_xyz(obstacle->_location, obstacle->_velocity, my_abs_pos);
perp_xyz.normalize();
newdest_neu[0] = my_pos_ned[0]*100 + perp_xyz[0] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC;
newdest_neu[1] = my_pos_ned[1]*100 + perp_xyz[1] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC;
newdest_neu[2] = -my_pos_ned[2]*100 + perp_xyz[2] * wp_speed_z * AP_AVOIDANCE_ESCAPE_TIME_SEC;
}
if (newdest_neu[2] < _minimum_avoid_height*100) {
// too close to the ground to do 3D avoidance
// GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "AVOID: PERPENDICULAR: 2D");
Vector2f perp_xy = perpendicular_xy(obstacle->_location, obstacle->_velocity, my_abs_pos);
perp_xy.normalize();
newdest_neu[0] = my_pos_ned[0]*100 + perp_xy[0] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC;
newdest_neu[1] = my_pos_ned[1]*100 + perp_xy[1] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC;
newdest_neu[2] = -my_pos_ned[2]*100;
}
return true;
}
// get unit vector away from the nearest obstacle // get unit vector away from the nearest obstacle
bool AP_Avoidance::get_vector_perpendicular(const AP_Avoidance::Obstacle *obstacle, Vector3f &vec_neu) bool AP_Avoidance::get_vector_perpendicular(const AP_Avoidance::Obstacle *obstacle, Vector3f &vec_neu)
{ {