Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: add support for polygon fences

This commit is contained in:
Randy Mackay 2016-06-23 15:24:48 +09:00
parent f0bb1ac1d2
commit d91805edbf
2 changed files with 173 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

150
libraries/AC_Fence/AC_Fence.cpp
View File

@ -1,6 +1,8 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_HAL/AP_HAL.h>
#include "AC_Fence.h"
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
@ -15,9 +17,10 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
// @Param: TYPE
// @DisplayName: Fence Type
// @Description: Enabled fence types held as bitmask
// @Values: 0:None,1:Altitude,2:Circle,3:Altitude and Circle
// @Values: 0:None,1:Altitude,2:Circle,3:Altitude and Circle,4:Polygon,5:Altitude and Polygon,6:Circle and Polygon,7:All
// @Bitmask: 0:Altitude,1:Circle,2:Polygon
// @User: Standard
AP_GROUPINFO("TYPE", 1, AC_Fence, _enabled_fences, AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE),
AP_GROUPINFO("TYPE", 1, AC_Fence, _enabled_fences, AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON),
// @Param: ACTION
// @DisplayName: Fence Action
@ -50,7 +53,14 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
// @Range: 1 10
// @User: Standard
AP_GROUPINFO("MARGIN", 5, AC_Fence, _margin, AC_FENCE_MARGIN_DEFAULT),
// @Param: TOTAL
// @DisplayName: Fence polygon point total
// @Description: Number of polygon points saved in eeprom (do not update manually)
// @Range: 1 20
// @User: Standard
AP_GROUPINFO("TOTAL", 6, AC_Fence, _total, 0),
AP_GROUPEND
};
@ -190,11 +200,32 @@ uint8_t AC_Fence::check_fence(float curr_alt)
}
}
// polygon fence check
if ((_enabled_fences & AC_FENCE_TYPE_POLYGON) != 0 ) {
// load fence if necessary
if (!_boundary_loaded) {
load_polygon_from_eeprom();
} else if (_boundary_valid) {
// check if vehicle is outside the polygon fence
const Vector3f& position = _inav.get_position();
if (_poly_loader.boundary_breached(Vector2f(position.x, position.y), _boundary_num_points, _boundary, true)) {
// check if this is a new breach
if ((_breached_fences & AC_FENCE_TYPE_POLYGON) == 0) {
// record that we have breached the polygon
record_breach(AC_FENCE_TYPE_POLYGON);
ret = ret | AC_FENCE_TYPE_POLYGON;
}
} else {
// clear breach if present
if ((_breached_fences & AC_FENCE_TYPE_POLYGON) != 0) {
clear_breach(AC_FENCE_TYPE_POLYGON);
}
}
}
}
// return any new breaches that have occurred
return ret;
// To-Do: add min alt and polygon check
//outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
}
// returns true if the destination is within fence (used to reject waypoints outside the fence)
@ -272,3 +303,110 @@ void AC_Fence::manual_recovery_start()
// record time pilot began manual recovery
_manual_recovery_start_ms = AP_HAL::millis();
}
/// returns pointer to array of polygon points and num_points is filled in with the total number
Vector2f* AC_Fence::get_polygon_points(uint16_t& num_points) const
{
num_points = _boundary_num_points;
return _boundary;
}
/// 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, true);
}
/// handler for polygon fence messages with GCS
void AC_Fence::handle_msg(mavlink_channel_t chan, mavlink_message_t* msg)
{
// exit immediately if null message
if (msg == NULL) {
return;
}
switch (msg->msgid) {
// receive a fence point from GCS and store in EEPROM
case MAVLINK_MSG_ID_FENCE_POINT: {
mavlink_fence_point_t packet;
mavlink_msg_fence_point_decode(msg, &packet);
if (!check_latlng(packet.lat,packet.lng)) {
GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_WARNING, chan, "Invalid fence point, lat or lng too large");
} else {
Vector2l point;
point.x = packet.lat*1.0e7f;
point.y = packet.lng*1.0e7f;
if (!_poly_loader.save_point_to_eeprom(packet.idx, point)) {
GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_WARNING, chan, "Failed to save polygon point, too many points?");
}
}
break;
}
// send a fence point to GCS
case MAVLINK_MSG_ID_FENCE_FETCH_POINT: {
mavlink_fence_fetch_point_t packet;
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)) {
mavlink_msg_fence_point_send_buf(msg, chan, msg->sysid, msg->compid, packet.idx, _total, point.x*1.0e-7f, point.y*1.0e-7f);
} else {
GCS_MAVLINK::send_statustext_chan(MAV_SEVERITY_WARNING, chan, "Bad fence point");
}
break;
}
default:
// do nothing
break;
}
}
/// load polygon points stored in eeprom into boundary array and perform validation
bool AC_Fence::load_polygon_from_eeprom(bool force_reload)
{
// exit immediately if already loaded
if (_boundary_loaded && !force_reload) {
return true;
}
// check if we need to create array
if (!_boundary_create_attempted) {
_boundary = (Vector2f *)_poly_loader.create_point_array(sizeof(Vector2f));
_boundary_create_attempted = true;
}
// exit if we could not allocate RAM for the boundary
if (_boundary == NULL) {
return false;
}
// get current location from EKF
Location temp_loc;
if (!_inav.get_location(temp_loc)) {
return false;
}
const struct Location &ekf_origin = _inav.get_origin();
// sanity check total
_total = constrain_int16(_total, 0, _poly_loader.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
_poly_loader.load_point_from_eeprom(index, temp_latlon);
// move into location structure and convert to offset from ekf origin
temp_loc.lat = temp_latlon.x;
temp_loc.lng = temp_latlon.y;
_boundary[index] = location_diff(ekf_origin, temp_loc) * 100.0f;
}
_boundary_num_points = _total;
_boundary_loaded = true;
// update validity of polygon
_boundary_valid = _poly_loader.boundary_valid(_boundary_num_points, _boundary, true);
return true;
}

29
libraries/AC_Fence/AC_Fence.h
View File

@ -5,12 +5,15 @@
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library
#include <AP_Fence/AP_PolyFence_loader.h>
// bit masks for enabled fence types. Used for TYPE parameter
#define AC_FENCE_TYPE_NONE 0 // fence disabled
#define AC_FENCE_TYPE_ALT_MAX 1 // high alt fence which usually initiates an RTL
#define AC_FENCE_TYPE_CIRCLE 2 // circular horizontal fence (usually initiates an RTL)
#define AC_FENCE_TYPE_POLYGON 4 // polygon horizontal fence
// valid actions should a fence be breached
#define AC_FENCE_ACTION_REPORT_ONLY 0 // report to GCS that boundary has been breached but take no further action
@ -93,6 +96,19 @@ public:
/// set_home_distance - update vehicle's distance from home in meters - required for circular horizontal fence monitoring
void set_home_distance(float distance) { _home_distance = distance; }
///
/// polygon related methods
///
/// returns pointer to array of polygon points and num_points is filled in with the total number
Vector2f* get_polygon_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(mavlink_channel_t chan, mavlink_message_t* msg);
static const struct AP_Param::GroupInfo var_info[];
private:
@ -103,6 +119,9 @@ private:
/// clear_breach - update breach bitmask, time and count
void clear_breach(uint8_t fence_type);
/// load polygon points stored in eeprom into boundary array and perform validation. returns true if load successfully completed
bool load_polygon_from_eeprom(bool force_reload = false);
// pointers to other objects we depend upon
const AP_InertialNav& _inav;
@ -113,6 +132,7 @@ private:
AP_Float _alt_max; // altitude upper limit in meters
AP_Float _circle_radius; // circle fence radius in meters
AP_Float _margin; // distance in meters that autopilot's should maintain from the fence to avoid a breach
AP_Int8 _total; // number of polygon points saved in eeprom
// backup fences
float _alt_max_backup; // backup altitude upper limit in meters used to refire the breach if the vehicle continues to move further away
@ -131,4 +151,13 @@ private:
uint16_t _breach_count; // number of times we have breached the fence
uint32_t _manual_recovery_start_ms; // system time in milliseconds that pilot re-took manual control
// polygon fence variables
AP_PolyFence_loader _poly_loader; // helper for loading/saving polygon points
Vector2f *_boundary = NULL; // 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_loaded = false; // true if boundary array has been loaded from eeprom
bool _boundary_valid = false; // true if boundary forms a closed polygon
bool _boundary_revalidate = false; // set to true when we need to revalidate the boundary (required after a point is changed)
};