Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: add polygon fence check to check_destination_within_fence

Browse Source
This commit is contained in:
Randy Mackay 2016-07-02 17:14:17 +09:00
parent a9d1dcd6ac
commit e820506c5a
2 changed files with 32 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AC_Fence/AC_Fence.cpp
View File

@ -65,7 +65,8 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
}; };
/// Default constructor. /// Default constructor.
AC_Fence::AC_Fence(const AP_InertialNav& inav) : AC_Fence::AC_Fence(const AP_AHRS& ahrs, const AP_InertialNav& inav) :
_ahrs(ahrs),
_inav(inav), _inav(inav),
_alt_max_backup(0), _alt_max_backup(0),
_circle_radius_backup(0), _circle_radius_backup(0),
@ -233,17 +234,37 @@ uint8_t AC_Fence::check_fence(float curr_alt)
} }
// returns true if the destination is within fence (used to reject waypoints outside the fence) // returns true if the destination is within fence (used to reject waypoints outside the fence)
bool AC_Fence::check_destination_within_fence(float dest_alt, float dest_distance_to_home) bool AC_Fence::check_destination_within_fence(const Location_Class& loc)
{ {
// Altitude fence check // Altitude fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) && (dest_alt >= _alt_max)) { if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) {
int32_t alt_above_home_cm;
if (loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_above_home_cm)) {
if ((alt_above_home_cm * 0.01f) > _alt_max) {
return false; return false;
} }
}
}
// Circular fence check // Circular fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) && (dest_distance_to_home >= _circle_radius)) { if ((get_enabled_fences() & AC_FENCE_TYPE_CIRCLE)) {
if ((get_distance_cm(_ahrs.get_home(), loc) * 0.01f) > _circle_radius) {
return false; return false;
} }
}
// polygon fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_POLYGON) && _boundary_num_points > 0) {
// check ekf has a good location
Location temp_loc;
if (_inav.get_location(temp_loc)) {
const struct Location &ekf_origin = _inav.get_origin();
Vector2f position = location_diff(ekf_origin, loc) * 100.0f;
if (_poly_loader.boundary_breached(position, _boundary_num_points, _boundary, true)) {
return false;
}
}
}
return true; return true;
} }

7
libraries/AC_Fence/AC_Fence.h
View File

@ -6,8 +6,10 @@
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library #include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library
#include <AC_Fence/AC_PolyFence_loader.h> #include <AC_Fence/AC_PolyFence_loader.h>
#include <AP_Common/Location.h>
// bit masks for enabled fence types. Used for TYPE parameter // bit masks for enabled fence types. Used for TYPE parameter
#define AC_FENCE_TYPE_NONE 0 // fence disabled #define AC_FENCE_TYPE_NONE 0 // fence disabled
@ -35,7 +37,7 @@ class AC_Fence
public: public:
/// Constructor /// Constructor
AC_Fence(const AP_InertialNav& inav); AC_Fence(const AP_AHRS& ahrs, const AP_InertialNav& inav);
/// enable - allows fence to be enabled/disabled. Note: this does not update the eeprom saved value /// enable - allows fence to be enabled/disabled. Note: this does not update the eeprom saved value
void enable(bool true_false) { _enabled = true_false; } void enable(bool true_false) { _enabled = true_false; }
@ -58,7 +60,7 @@ public:
uint8_t check_fence(float curr_alt); uint8_t check_fence(float curr_alt);
// returns true if the destination is within fence (used to reject waypoints outside the fence) // returns true if the destination is within fence (used to reject waypoints outside the fence)
bool check_destination_within_fence(float dest_alt, float dest_distance_to_home); bool check_destination_within_fence(const Location_Class& loc);
/// get_breaches - returns bit mask of the fence types that have been breached /// get_breaches - returns bit mask of the fence types that have been breached
uint8_t get_breaches() const { return _breached_fences; } uint8_t get_breaches() const { return _breached_fences; }
@ -123,6 +125,7 @@ private:
bool load_polygon_from_eeprom(bool force_reload = false); bool load_polygon_from_eeprom(bool force_reload = false);
// pointers to other objects we depend upon // pointers to other objects we depend upon
const AP_AHRS& _ahrs;
const AP_InertialNav& _inav; const AP_InertialNav& _inav;
// parameters // parameters