Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: add internal error if invalid destination received

Browse Source
This commit is contained in:
Randy Mackay 2022-01-03 10:06:42 +09:00
parent 461c5196e7
commit 1fd9da710d
1 changed files with 2 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -207,6 +207,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location
Vector2f destination_NE; Vector2f destination_NE;
if (!_origin.get_vector_xy_from_origin_NE(origin_NE) || if (!_origin.get_vector_xy_from_origin_NE(origin_NE) ||
!_destination.get_vector_xy_from_origin_NE(destination_NE)) { !_destination.get_vector_xy_from_origin_NE(destination_NE)) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return false; return false;
} }
origin_NE *= 0.01f; origin_NE *= 0.01f;
@ -240,6 +241,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location
// convert next_destination to offset from EKF origin // convert next_destination to offset from EKF origin
Vector2f next_destination_NE; Vector2f next_destination_NE;
if (!next_destination.get_vector_xy_from_origin_NE(next_destination_NE)) { if (!next_destination.get_vector_xy_from_origin_NE(next_destination_NE)) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return false; return false;
} }
next_destination_NE *= 0.01f; next_destination_NE *= 0.01f;