AP_Avoidance: Remove unutilized get_destination_perpendicular

Yukong · Feb 12, 2017 · c079532 · c079532
1 parent 97c5776
commit c079532
Showing 1 changed file with 0 additions and 66 deletions.
diff --git 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)
 {