From ff37590776ca488967de920d6948fe2d5672b1ba Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 29 May 2019 23:02:04 +1000 Subject: [PATCH] AC_Fence: move polygon points into AC_Fence_Polygon --- libraries/AC_Fence/AC_Fence.cpp | 70 +++++++--------------- libraries/AC_Fence/AC_Fence.h | 35 ++--------- libraries/AC_Fence/AC_PolyFence_loader.cpp | 62 ++++++++++++------- libraries/AC_Fence/AC_PolyFence_loader.h | 54 ++++++++++++++--- 4 files changed, 113 insertions(+), 108 deletions(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index 695831452d..3e2a58f250 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -116,7 +116,7 @@ bool AC_Fence::pre_arm_check_polygon(const char* &fail_msg) const return true; } - if (!_boundary_valid) { + if (!_poly_loader.valid()) { fail_msg = "Polygon boundary invalid"; return false; } @@ -258,28 +258,10 @@ bool AC_Fence::polygon_fence_is_breached() return false; } - // check consistency of number of points - if (_boundary_num_points != _total) { - // Fence is currently not completely loaded. Can't breach it?! - load_polygon_from_eeprom(); - return false; - } - if (!_boundary_valid) { - // fence isn't valid - can't breach it?! - return false; - } - - // check if vehicle is outside the polygon fence - Vector2f position; - if (!AP::ahrs().get_relative_position_NE_origin(position)) { - // we have no idea where we are; can't breach the fence - return false; - } - - position = position * 100.0f; // m to cm - return _poly_loader.boundary_breached(position, _boundary_num_points, _boundary); + return _poly_loader.breached(); } + bool AC_Fence::check_fence_circle() { if (!(_enabled_fences & AC_FENCE_TYPE_CIRCLE)) { @@ -385,13 +367,9 @@ bool AC_Fence::check_destination_within_fence(const Location& loc) } // polygon fence check - if ((get_enabled_fences() & AC_FENCE_TYPE_POLYGON) && _boundary_num_points > 0) { - // check ekf has a good location - Vector2f posNE; - if (loc.get_vector_xy_from_origin_NE(posNE)) { - if (_poly_loader.boundary_breached(posNE, _boundary_num_points, _boundary)) { - return false; - } + if ((get_enabled_fences() & AC_FENCE_TYPE_POLYGON)) { + if (_poly_loader.breached(loc)) { + return false; } } @@ -449,17 +427,17 @@ void AC_Fence::manual_recovery_start() } /// returns pointer to array of polygon points and num_points is filled in with the total number -Vector2f* AC_Fence::get_boundary_points(uint16_t& num_points) const +Vector2f* AC_PolyFence_loader::get_boundary_points(uint16_t& num_points) const { // return array minus the first point which holds the return location if (_boundary == nullptr) { return nullptr; } - if (!_boundary_valid) { + if (!valid()) { return nullptr; } // minus one for return point, minus one for closing point - // (_boundary_valid is not true unless we have a closing point AND + // (polygon().valid() is not true unless we have a closing point AND // we have a minumum number of points) if (_boundary_num_points < 2) { return nullptr; @@ -468,14 +446,8 @@ Vector2f* AC_Fence::get_boundary_points(uint16_t& num_points) const return &_boundary[1]; } -/// returns true if we've breached the polygon boundary. simple passthrough to underlying _poly_loader object -bool AC_Fence::boundary_breached(const Vector2f& location, uint16_t num_points, const Vector2f* points) const -{ - return _poly_loader.boundary_breached(location, num_points, points); -} - /// handler for polygon fence messages with GCS -void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg) +void AC_PolyFence_loader::handle_msg(GCS_MAVLINK &link, const mavlink_message_t& msg) { switch (msg.msgid) { // receive a fence point from GCS and store in EEPROM @@ -488,7 +460,7 @@ void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg) Vector2l point; point.x = packet.lat*1.0e7f; point.y = packet.lng*1.0e7f; - if (!_poly_loader.save_point_to_eeprom(packet.idx, point)) { + if (!save_point_to_eeprom(packet.idx, point)) { link.send_text(MAV_SEVERITY_WARNING, "Failed to save polygon point, too many points?"); } else { // trigger reload of points @@ -504,7 +476,7 @@ void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg) mavlink_msg_fence_fetch_point_decode(&msg, &packet); // attempt to retrieve from eeprom Vector2l point; - if (_poly_loader.load_point_from_eeprom(packet.idx, point)) { + if (load_point_from_eeprom(packet.idx, point)) { mavlink_msg_fence_point_send(link.get_chan(), msg.sysid, msg.compid, packet.idx, _total, point.x*1.0e-7f, point.y*1.0e-7f); } else { link.send_text(MAV_SEVERITY_WARNING, "Bad fence point"); @@ -519,12 +491,12 @@ void AC_Fence::handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg) } /// load polygon points stored in eeprom into boundary array and perform validation -bool AC_Fence::load_polygon_from_eeprom() +bool AC_PolyFence_loader::load_from_eeprom() { // check if we need to create array - if (!_boundary_create_attempted) { - _boundary = (Vector2f *)_poly_loader.create_point_array(sizeof(Vector2f)); - _boundary_create_attempted = true; + if (!_create_attempted) { + _boundary = (Vector2f *)create_point_array(sizeof(Vector2f)); + _create_attempted = true; } // exit if we could not allocate RAM for the boundary @@ -543,13 +515,13 @@ bool AC_Fence::load_polygon_from_eeprom() } // sanity check total - _total = constrain_int16(_total, 0, _poly_loader.max_points()); + _total = constrain_int16(_total, 0, max_points()); // load each point from eeprom Vector2l temp_latlon; for (uint16_t index=0; index<_total; index++) { // load boundary point as lat/lon point - if (!_poly_loader.load_point_from_eeprom(index, temp_latlon)) { + if (!load_point_from_eeprom(index, temp_latlon)) { return false; } // move into location structure and convert to offset from ekf origin @@ -558,10 +530,10 @@ bool AC_Fence::load_polygon_from_eeprom() _boundary[index] = ekf_origin.get_distance_NE(temp_loc) * 100.0f; } _boundary_num_points = _total; - _boundary_update_ms = AP_HAL::millis(); + _update_ms = AP_HAL::millis(); // update validity of polygon - _boundary_valid = _poly_loader.boundary_valid(_boundary_num_points, _boundary); + _valid = calculate_boundary_valid(); return true; } @@ -593,7 +565,7 @@ bool AC_Fence::sys_status_failed() const return true; } if (_enabled_fences & AC_FENCE_TYPE_POLYGON) { - if (!_boundary_valid) { + if (!_poly_loader.valid()) { return true; } } diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 7f055e40ef..2f2e7ebbcd 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -96,26 +96,6 @@ public: /// has no effect if no breaches have occurred void manual_recovery_start(); - /// - /// polygon related methods - /// - - /// returns true if polygon fence is valid (i.e. has at least 3 sides) - bool is_polygon_valid() const { return _boundary_valid; } - - /// returns pointer to array of polygon points and num_points is filled in with the total number - /// points are offsets from EKF origin in NE frame - Vector2f* get_boundary_points(uint16_t& num_points) const; - - /// returns true if we've breached the polygon boundary. simple passthrough to underlying _poly_loader object - bool boundary_breached(const Vector2f& location, uint16_t num_points, const Vector2f* points) const; - - /// handler for polygon fence messages with GCS - void handle_msg(GCS_MAVLINK &link, const mavlink_message_t &msg); - - /// return system time of last update to the boundary (allows external detection of boundary changes) - uint32_t get_boundary_update_ms() const { return _boundary_update_ms; } - static const struct AP_Param::GroupInfo var_info[]; // methods for mavlink SYS_STATUS message (send_sys_status) @@ -126,6 +106,9 @@ public: // get singleton instance static AC_Fence *get_singleton() { return _singleton; } + AC_PolyFence_loader &polyfence() { return _poly_loader; } + const AC_PolyFence_loader &polyfence_const() const { return _poly_loader; } + private: static AC_Fence *_singleton; @@ -149,9 +132,6 @@ private: bool pre_arm_check_circle(const char* &fail_msg) const; bool pre_arm_check_alt(const char* &fail_msg) const; - /// load polygon points stored in eeprom into boundary array and perform validation. returns true if load successfully completed - bool load_polygon_from_eeprom(); - // returns true if we have breached the fence: bool polygon_fence_is_breached(); @@ -185,13 +165,8 @@ private: uint32_t _manual_recovery_start_ms; // system time in milliseconds that pilot re-took manual control - // polygon fence variables - AC_PolyFence_loader _poly_loader; // helper for loading/saving polygon points - Vector2f *_boundary = nullptr; // array of boundary points. Note: point 0 is the return point - uint8_t _boundary_num_points = 0; // number of points in the boundary array (should equal _total parameter after load has completed) - bool _boundary_create_attempted = false; // true if we have attempted to create the boundary array - bool _boundary_valid = false; // true if boundary forms a closed polygon - uint32_t _boundary_update_ms; // system time of last update to the boundary + AC_PolyFence_loader _poly_loader{_total}; // polygon fence + }; namespace AP { diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp index 018efd04c4..2e7981d920 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.cpp +++ b/libraries/AC_Fence/AC_PolyFence_loader.cpp @@ -53,13 +53,12 @@ bool AC_PolyFence_loader::save_point_to_eeprom(uint16_t i, const Vector2l& point return true; } -// validate array of boundary points (expressed as either floats or long ints) +// validate array of boundary points // returns true if boundary is valid -template -bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2* points) const +bool AC_PolyFence_loader::calculate_boundary_valid() const { // exit immediate if no points - if (points == nullptr) { + if (_boundary == nullptr) { return false; } @@ -67,30 +66,61 @@ bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2* uint8_t start_num = 1; // a boundary requires at least 4 point (a triangle and last point equals first) - if (num_points < start_num + 4) { + if (_boundary_num_points < start_num + 4) { return false; } // point 1 and last point must be the same. Note: 0th point is reserved as the return point - if (!Polygon_complete(&points[start_num], num_points-start_num)) { + if (!Polygon_complete(&_boundary[start_num], _boundary_num_points-start_num)) { return false; } // check return point is within the fence - if (Polygon_outside(points[0], &points[1], num_points-start_num)) { + if (Polygon_outside(_boundary[0], &_boundary[1], _boundary_num_points-start_num)) { return false; } return true; } -// check if a location (expressed as either floats or long ints) is within the boundary +bool AC_PolyFence_loader::breached() +{ + // check if vehicle is outside the polygon fence + Vector2f position; + if (!AP::ahrs().get_relative_position_NE_origin(position)) { + // we have no idea where we are; can't breach the fence + return false; + } + + position = position * 100.0f; // m to cm + return breached(position); +} + +bool AC_PolyFence_loader::breached(const Location& loc) +{ + Vector2f posNE; + if (!loc.get_vector_xy_from_origin_NE(posNE)) { + // not breached if we don't now where we are + return false; + } + return breached(posNE); +} + // returns true if location is outside the boundary -template -bool AC_PolyFence_loader::boundary_breached(const Vector2& location, uint16_t num_points, const Vector2* points) const +bool AC_PolyFence_loader::breached(const Vector2f& location) { + // check consistency of number of points + if (_boundary_num_points != _total) { + // Fence is currently not completely loaded. Can't breach it?! + load_from_eeprom(); + return false; + } // exit immediate if no points - if (points == nullptr) { + if (_boundary == nullptr) { + return false; + } + if (!_valid) { + // fence isn't valid - can't breach it?! return false; } @@ -98,13 +128,5 @@ bool AC_PolyFence_loader::boundary_breached(const Vector2& location, uint16_t uint8_t start_num = 1; // check location is within the fence - return Polygon_outside(location, &points[start_num], num_points-start_num); + return Polygon_outside(location, &_boundary[start_num], _boundary_num_points-start_num); } - -// declare type specific methods -template bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2l* points) const; -template bool AC_PolyFence_loader::boundary_valid(uint16_t num_points, const Vector2f* points) const; -template bool AC_PolyFence_loader::boundary_breached(const Vector2l& location, uint16_t num_points, - const Vector2l* points) const; -template bool AC_PolyFence_loader::boundary_breached(const Vector2f& location, uint16_t num_points, - const Vector2f* points) const; diff --git a/libraries/AC_Fence/AC_PolyFence_loader.h b/libraries/AC_Fence/AC_PolyFence_loader.h index be15a3c8f5..925afae337 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.h +++ b/libraries/AC_Fence/AC_PolyFence_loader.h @@ -2,12 +2,49 @@ #include #include +#include class AC_PolyFence_loader { public: + AC_PolyFence_loader(AP_Int8 &total) : + _total(total) {} + + AC_PolyFence_loader(const AC_PolyFence_loader &other) = delete; + AC_PolyFence_loader &operator=(const AC_PolyFence_loader&) = delete; + + /// returns pointer to array of polygon points and num_points is + /// filled in with the total number. This does not include the + /// return point or the closing point. + Vector2f* get_boundary_points(uint16_t& num_points) const; + + /// handler for polygon fence messages with GCS + void handle_msg(GCS_MAVLINK &link, mavlink_message_t* msg); + + bool breached(); + // returns true if location is outside the boundary + bool breached(const Location& loc); + + // returns true if boundary is valid + bool valid() const { + return _valid; + } + + // returns the system in in millis when the boundary was last updated + uint32_t update_ms() const { + return _update_ms; + } + +private: + + bool breached(const Vector2f& location); + /// load polygon points stored in eeprom into boundary array and + /// perform validation. returns true if load successfully + /// completed + bool load_from_eeprom(); + // maximum number of fence points we can store in eeprom uint8_t max_points() const; @@ -21,16 +58,15 @@ public: // save a fence point to eeprom, returns true on successful save bool save_point_to_eeprom(uint16_t i, const Vector2l& point); + // update the validity flag: + bool calculate_boundary_valid() const; - // validate array of boundary points (expressed as either floats or long ints) - // returns true if boundary is valid - template - bool boundary_valid(uint16_t num_points, const Vector2* points) const; - - // check if a location (expressed as either floats or long ints) is within the boundary - // returns true if location is outside the boundary - template - bool boundary_breached(const Vector2& location, uint16_t num_points, const Vector2* points) const; + Vector2f *_boundary; // array of boundary points. Note: point 0 is the return point + uint8_t _boundary_num_points; // number of points in the boundary array (should equal _total parameter after load has completed) + bool _create_attempted; // true if we have attempted to create the boundary array + bool _valid; // true if boundary forms a closed polygon + uint32_t _update_ms; // system time of last update to the boundary + AP_Int8 &_total; };