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: Change division to multiplication

Browse Source
This commit is contained in:
muramura 2023-09-15 01:19:28 +09:00 committed by Peter Barker
parent 695831628a
commit f19439e769
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Avoidance/AP_Avoidance.cpp
View File

@ -325,11 +325,11 @@ float closest_approach_z(const Location &my_loc,
} }
debug(" time_horizon: (%d)", time_horizon); 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(" 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, 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) Vector3f AP_Avoidance::perpendicular_xyz(const Location &p1, const Vector3f &v1, const Location &p2)
{ {
const Vector2f delta_p_2d = p1.get_distance_NE(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 v1_xyz = Vector3f(v1[0], v1[1], -v1[2]);
Vector3f ret = Vector3f::perpendicular(delta_p_xyz, v1_xyz); Vector3f ret = Vector3f::perpendicular(delta_p_xyz, v1_xyz);
return ret; return ret;