From c079532a7a54550b1aa3b306a65920b65a661f51 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Mon, 30 Jan 2017 12:06:48 -0700 Subject: [PATCH] AP_Avoidance: Remove unutilized get_destination_perpendicular --- libraries/AP_Avoidance/AP_Avoidance.cpp | 66 ------------------------- 1 file changed, 66 deletions(-) diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp index d78201de25..bdb6cc1dae 100644 --- a/libraries/AP_Avoidance/AP_Avoidance.cpp +++ b/libraries/AP_Avoidance/AP_Avoidance.cpp @@ -573,72 +573,6 @@ void AP_Avoidance::handle_msg(const mavlink_message_t &msg) 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 bool AP_Avoidance::get_vector_perpendicular(const AP_Avoidance::Obstacle *obstacle, Vector3f &vec_neu) {