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

AC_Fence: replace AP_InertialNav by AHRS

Browse Source
This commit is contained in:
khancyr 2017-08-08 11:54:33 +02:00 committed by Randy Mackay
parent d09b0696db
commit 65373b85f0
2 changed files with 33 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/AC_Fence/AC_Fence.cpp
View File

@ -73,9 +73,8 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = {
};
/// Default constructor.
AC_Fence::AC_Fence(const AP_AHRS& ahrs, const AP_InertialNav& inav) :
AC_Fence::AC_Fence(const AP_AHRS_NavEKF& ahrs) :
_ahrs(ahrs),
_inav(inav),
_alt_max_backup(0),
_circle_radius_backup(0),
_alt_max_breach_distance(0),
@ -132,7 +131,12 @@ bool AC_Fence::pre_arm_check(const char* &fail_msg) const
}
// if we have horizontal limits enabled, check inertial nav position is ok
if ((_enabled_fences & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON))>0 && !_inav.get_filter_status().flags.horiz_pos_abs && !_inav.get_filter_status().flags.pred_horiz_pos_abs) {
nav_filter_status filt_status;
if (!_ahrs.get_filter_status(filt_status)) {
fail_msg = "AHRS status not available";
return false;
}
if ((_enabled_fences & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON))>0 && !filt_status.flags.horiz_pos_abs && !filt_status.flags.pred_horiz_pos_abs) {
fail_msg = "fence requires position";
return false;
}
@ -232,18 +236,21 @@ uint8_t AC_Fence::check_fence(float curr_alt)
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 |= AC_FENCE_TYPE_POLYGON;
}
} else {
// clear breach if present
if ((_breached_fences & AC_FENCE_TYPE_POLYGON) != 0) {
clear_breach(AC_FENCE_TYPE_POLYGON);
Vector2f position;
if (_ahrs.get_relative_position_NE_origin(position)) { // don't check polyfence in case of bad position
position = position * 100.0f; // m to cm
if (_poly_loader.boundary_breached(position, _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 |= AC_FENCE_TYPE_POLYGON;
}
} else {
// clear breach if present
if ((_breached_fences & AC_FENCE_TYPE_POLYGON) != 0) {
clear_breach(AC_FENCE_TYPE_POLYGON);
}
}
}
}
@ -276,11 +283,10 @@ bool AC_Fence::check_destination_within_fence(const Location_Class& loc)
// 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)) {
Vector2f posNE;
if (_ahrs.get_relative_position_NE_origin(posNE)) {
posNE = posNE * 100.0f; // m to cm
if (_poly_loader.boundary_breached(posNE, _boundary_num_points, _boundary, true)) {
return false;
}
}
@ -432,10 +438,11 @@ bool AC_Fence::load_polygon_from_eeprom(bool force_reload)
// get current location from EKF
Location temp_loc;
if (!_inav.get_location(temp_loc)) {
if (!_ahrs.get_location(temp_loc)) {
return false;
}
const struct Location &ekf_origin = _inav.get_origin();
struct Location ekf_origin {};
_ahrs.get_origin(ekf_origin);
// sanity check total
_total = constrain_int16(_total, 0, _poly_loader.max_points());

10
libraries/AC_Fence/AC_Fence.h
View File

@ -6,7 +6,6 @@
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library
#include <AC_Fence/AC_PolyFence_loader.h>
#include <AP_Common/Location.h>
@ -35,8 +34,8 @@
class AC_Fence
{
public:
static AC_Fence create(const AP_AHRS &ahrs, const AP_InertialNav &inav) {
return AC_Fence{ahrs, inav};
static AC_Fence create(const AP_AHRS_NavEKF &ahrs) {
return AC_Fence{ahrs};
}
constexpr AC_Fence(AC_Fence &&other) = default;
@ -123,7 +122,7 @@ public:
static const struct AP_Param::GroupInfo var_info[];
private:
AC_Fence(const AP_AHRS &ahrs, const AP_InertialNav &inav);
AC_Fence(const AP_AHRS_NavEKF &ahrs);
/// record_breach - update breach bitmask, time and count
void record_breach(uint8_t fence_type);
@ -135,8 +134,7 @@ private:
bool load_polygon_from_eeprom(bool force_reload = false);
// pointers to other objects we depend upon
const AP_AHRS& _ahrs;
const AP_InertialNav& _inav;
const AP_AHRS_NavEKF& _ahrs;
// parameters
AP_Int8 _enabled; // top level enable/disable control