From ef28be9ce8c3706e352fd3570ad3f88b0d46ffe1 Mon Sep 17 00:00:00 2001 From: Francisco Ferreira Date: Tue, 19 Jul 2016 23:40:22 +0100 Subject: [PATCH] AP_Rally: add is_valid method method will be used in Copter to check if a rally point is inside fence --- libraries/AP_Rally/AP_Rally.cpp | 2 +- libraries/AP_Rally/AP_Rally.h | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Rally/AP_Rally.cpp b/libraries/AP_Rally/AP_Rally.cpp index 8b20f69c24..a810ab9348 100644 --- a/libraries/AP_Rally/AP_Rally.cpp +++ b/libraries/AP_Rally/AP_Rally.cpp @@ -132,7 +132,7 @@ bool AP_Rally::find_nearest_rally_point(const Location ¤t_loc, RallyLocati Location rally_loc = rally_location_to_location(next_rally); float dis = get_distance(current_loc, rally_loc); - if (dis < min_dis || min_dis < 0) { + if (is_valid(rally_loc) && (dis < min_dis || min_dis < 0)) { min_dis = dis; return_loc = next_rally; } diff --git a/libraries/AP_Rally/AP_Rally.h b/libraries/AP_Rally/AP_Rally.h index adf9b2900c..20b6f4a002 100644 --- a/libraries/AP_Rally/AP_Rally.h +++ b/libraries/AP_Rally/AP_Rally.h @@ -62,6 +62,8 @@ public: static const struct AP_Param::GroupInfo var_info[]; private: + virtual bool is_valid(const Location &rally_point) const { return true; } + static StorageAccess _storage; // internal variables