Browse Source

AP_Rally: add is_valid method

method will be used in Copter to check if a rally point is inside fence
master
Francisco Ferreira 9 years ago committed by Randy Mackay
parent
commit
ef28be9ce8
  1. 2
      libraries/AP_Rally/AP_Rally.cpp
  2. 2
      libraries/AP_Rally/AP_Rally.h

2
libraries/AP_Rally/AP_Rally.cpp

@ -132,7 +132,7 @@ bool AP_Rally::find_nearest_rally_point(const Location &current_loc, RallyLocati
Location rally_loc = rally_location_to_location(next_rally); Location rally_loc = rally_location_to_location(next_rally);
float dis = get_distance(current_loc, rally_loc); 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; min_dis = dis;
return_loc = next_rally; return_loc = next_rally;
} }

2
libraries/AP_Rally/AP_Rally.h

@ -62,6 +62,8 @@ public:
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private:
virtual bool is_valid(const Location &rally_point) const { return true; }
static StorageAccess _storage; static StorageAccess _storage;
// internal variables // internal variables

Loading…
Cancel
Save