diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp index dfa1c6bf4f..45f28874fc 100644 --- a/libraries/AP_Avoidance/AP_Avoidance.cpp +++ b/libraries/AP_Avoidance/AP_Avoidance.cpp @@ -325,11 +325,11 @@ float closest_approach_z(const Location &my_loc, } debug(" time_horizon: (%d)", time_horizon); - debug(" delta pos: (%f) metres", delta_pos_d/100.0f); + debug(" delta pos: (%f) metres", delta_pos_d*0.01f); debug(" delta vel: (%f) m/s", delta_vel_d); - debug(" closest: (%f) metres", ret/100.0f); + debug(" closest: (%f) metres", ret*0.01f); - return ret/100.0f; + return ret*0.01f; } void AP_Avoidance::update_threat_level(const Location &my_loc, @@ -657,7 +657,7 @@ bool AP_Avoidance::get_vector_perpendicular(const AP_Avoidance::Obstacle *obstac Vector3f AP_Avoidance::perpendicular_xyz(const Location &p1, const Vector3f &v1, const Location &p2) { const Vector2f delta_p_2d = p1.get_distance_NE(p2); - Vector3f delta_p_xyz = Vector3f(delta_p_2d[0],delta_p_2d[1],(p2.alt-p1.alt)/100.0f); //check this line + Vector3f delta_p_xyz = Vector3f(delta_p_2d[0],delta_p_2d[1],(p2.alt-p1.alt)*0.01f); //check this line Vector3f v1_xyz = Vector3f(v1[0], v1[1], -v1[2]); Vector3f ret = Vector3f::perpendicular(delta_p_xyz, v1_xyz); return ret;