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

ArduPlane: Remove all unused geofence capability 

ArduPlane: Remove missed geofence logic
This commit is contained in:
James O'Shannessy 2020-09-10 18:05:24 +10:00 committed by Peter Barker
parent a23e8d46da
commit aae092b88f
14 changed files with 4 additions and 850 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/AP_Arming.cpp
View File

@ -174,20 +174,6 @@ bool AP_Arming_Plane::arm_checks(AP_Arming::Method method)
return true;
}
#if GEOFENCE_ENABLED == ENABLED
if (plane.g.fence_autoenable == FenceAutoEnable::WhenArmed) {
if (!plane.geofence_set_enabled(true)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Fence: cannot enable for arming");
return false;
} else if (!plane.geofence_prearm_check()) {
plane.geofence_set_enabled(false);
return false;
} else {
gcs().send_text(MAV_SEVERITY_WARNING, "Fence: auto-enabled for arming");
}
}
#endif
// call parent class checks
return AP_Arming::arm_checks(method);
}
@ -276,12 +262,6 @@ bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_chec
gcs().send_text(MAV_SEVERITY_INFO, "Throttle disarmed");
#if GEOFENCE_ENABLED == ENABLED
if (plane.g.fence_autoenable == FenceAutoEnable::WhenArmed) {
plane.geofence_set_enabled(false);
}
#endif
return true;
}

8
ArduPlane/ArduPlane.cpp
View File

@ -407,11 +407,6 @@ void Plane::update_GPS_10Hz(void)
}
}
#if GEOFENCE_ENABLED == ENABLED
// see if we've breached the geo-fence
geofence_check(false);
#endif
// update wind estimate
ahrs.estimate_wind();
} else if (gps.status() < AP_GPS::GPS_OK_FIX_3D && ground_start_count != 0) {
@ -496,9 +491,6 @@ void Plane::update_alt()
#if PARACHUTE == ENABLED
parachute.set_sink_rate(auto_state.sink_rate);
#endif
#if GEOFENCE_ENABLED == ENABLED
geofence_check(true);
#endif
update_flight_stage();

3
ArduPlane/Attitude.cpp
View File

@ -58,9 +58,6 @@ bool Plane::stick_mixing_enabled(void)
// we're in an auto mode. Check the stick mixing flag
if (g.stick_mixing != STICK_MIXING_DISABLED &&
g.stick_mixing != STICK_MIXING_VTOL_YAW &&
#if GEOFENCE_ENABLED == ENABLED
geofence_stickmixing() &&
#endif
#if AC_FENCE == ENABLED
fence_stickmixing() &&
#endif

78
ArduPlane/GCS_Mavlink.cpp
View File

@ -152,14 +152,6 @@ void GCS_MAVLINK_Plane::send_aoa_ssa()
ahrs.getSSA());
}
#if GEOFENCE_ENABLED == ENABLED
void Plane::send_fence_status(mavlink_channel_t chan)
{
geofence_send_status(chan);
}
#endif
void GCS_MAVLINK_Plane::send_nav_controller_output() const
{
if (plane.control_mode == &plane.mode_manual) {
@ -415,13 +407,6 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
#endif
break;
#if GEOFENCE_ENABLED == ENABLED
case MSG_FENCE_STATUS:
CHECK_PAYLOAD_SIZE(FENCE_STATUS);
plane.send_fence_status(chan);
break;
#endif
case MSG_TERRAIN:
#if AP_TERRAIN_AVAILABLE
CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST);
@ -1050,36 +1035,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
}
return MAV_RESULT_FAILED;
#if GEOFENCE_ENABLED == ENABLED
case MAV_CMD_DO_FENCE_ENABLE:
if (!plane.geofence_present()) {
gcs().send_text(MAV_SEVERITY_NOTICE,"Fence not configured");
return MAV_RESULT_FAILED;
}
switch((uint16_t)packet.param1) {
case 0:
if (! plane.geofence_set_enabled(false)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
case 1:
if (! plane.geofence_set_enabled(true)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
case 2: //disable fence floor only
if (! plane.geofence_set_floor_enabled(false)) {
return MAV_RESULT_FAILED;
}
gcs().send_text(MAV_SEVERITY_NOTICE,"Fence floor disabled");
return MAV_RESULT_ACCEPTED;
default:
break;
}
return MAV_RESULT_FAILED;
#endif
case MAV_CMD_DO_SET_HOME: {
// param1 : use current (1=use current location, 0=use specified location)
// param5 : latitude
@ -1177,39 +1132,6 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg)
{
switch (msg.msgid) {
#if GEOFENCE_ENABLED == ENABLED
// 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 (plane.g.fence_action != FENCE_ACTION_NONE) {
send_text(MAV_SEVERITY_WARNING,"Fencing must be disabled");
} else if (packet.count != plane.g.fence_total) {
send_text(MAV_SEVERITY_WARNING,"Bad fence point");
} else if (!check_latlng(packet.lat,packet.lng)) {
send_text(MAV_SEVERITY_WARNING,"Invalid fence point, lat or lng too large");
} else {
plane.set_fence_point_with_index(Vector2l(packet.lat*1.0e7f, packet.lng*1.0e7f), packet.idx);
}
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);
if (packet.idx >= plane.g.fence_total) {
send_text(MAV_SEVERITY_WARNING,"Bad fence point");
} else {
Vector2l point = plane.get_fence_point_with_index(packet.idx);
mavlink_msg_fence_point_send(chan, msg.sysid, msg.compid, packet.idx, plane.g.fence_total,
point.x*1.0e-7f, point.y*1.0e-7f);
}
break;
}
#endif // GEOFENCE_ENABLED
case MAVLINK_MSG_ID_MANUAL_CONTROL:
{
if (msg.sysid != plane.g.sysid_my_gcs) {

17
ArduPlane/GCS_Plane.cpp
View File

@ -21,12 +21,6 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
}
#endif
#if GEOFENCE_ENABLED == ENABLED
if (plane.geofence_present()) {
control_sensors_present |= MAV_SYS_STATUS_GEOFENCE;
}
#endif
if (plane.have_reverse_thrust()) {
control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
@ -35,12 +29,6 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
#if GEOFENCE_ENABLED == ENABLED
if (plane.geofence_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE;
}
#endif
control_sensors_present |=
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
@ -120,11 +108,6 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
if (airspeed && airspeed->all_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
#if GEOFENCE_ENABLED
if (!plane.geofence_breached()) {
control_sensors_health |= MAV_SYS_STATUS_GEOFENCE;
}
#endif
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;

62
ArduPlane/Parameters.cpp
View File

@ -279,68 +279,6 @@ const AP_Param::Info Plane::var_info[] = {
// @User: Standard
GSCALAR(rtl_radius, "RTL_RADIUS", 0),
#if GEOFENCE_ENABLED == ENABLED
// @Param: FENCE_ACTION
// @DisplayName: Action on geofence breach
// @Description: What to do on fence breach. If this is set to 0 then no action is taken, and geofencing is disabled. If this is set to 1 then the plane will enter GUIDED mode, with the target waypoint as the fence return point. If this is set to 2 then the fence breach is reported to the ground station, but no other action is taken. If set to 3 then the plane enters guided mode but the pilot retains manual throttle control. If set to 4 the plane enters RTL mode, with the target waypoint as the closest rally point (or home point if there are no rally points).
// @Values: 0:None,1:GuidedMode,2:ReportOnly,3:GuidedModeThrPass,4:RTL_Mode
// @User: Standard
GSCALAR(fence_action, "FENCE_ACTION", 0),
// @Param: FENCE_TOTAL
// @DisplayName: Fence Total
// @Description: Number of geofence points currently loaded
// @User: Advanced
GSCALAR(fence_total, "FENCE_TOTAL", 0),
// @Param: FENCE_CHANNEL
// @DisplayName: Fence Channel
// @Description: RC Channel to use to enable geofence. PWM input above 1750 enables the geofence
// @User: Standard
GSCALAR(fence_channel, "FENCE_CHANNEL", 0),
// @Param: FENCE_MINALT
// @DisplayName: Fence Minimum Altitude
// @Description: Minimum altitude allowed before geofence triggers
// @Units: m
// @Range: 0 32767
// @Increment: 1
// @User: Standard
GSCALAR(fence_minalt, "FENCE_MINALT", 0),
// @Param: FENCE_MAXALT
// @DisplayName: Fence Maximum Altitude
// @Description: Maximum altitude allowed before geofence triggers
// @Units: m
// @Range: 0 32767
// @Increment: 1
// @User: Standard
GSCALAR(fence_maxalt, "FENCE_MAXALT", 0),
// @Param: FENCE_RETALT
// @DisplayName: Fence Return Altitude
// @Description: Altitude the aircraft will transit to when a fence breach occurs. If FENCE_RETALT is <= 0 then the midpoint between FENCE_MAXALT and FENCE_MINALT is used, unless FENCE_MAXALT < FENCE_MINALT. If FENCE_MAXALT < FENCE_MINALT AND FENCE_RETALT is <= 0 then ALT_HOLD_RTL is the altitude used on a fence breach.
// @Units: m
// @Range: 0 32767
// @Increment: 1
// @User: Standard
GSCALAR(fence_retalt, "FENCE_RETALT", 0),
// @Param: FENCE_AUTOENABLE
// @DisplayName: Fence automatic enable
// @Description: When set to 1, geofence automatically enables after an auto takeoff and automatically disables at the beginning of an auto landing. When on the ground before takeoff the fence is disabled. When set to 2, the fence autoenables after an auto takeoff, but only disables the fence floor during landing. It is highly recommended to not use this option for line of sight flying and use a fence enable channel instead. When set to 3 the fence auto-enables when the vehicle is armed and disables when disarmed and arming will fail if the fence cannot be enabled or is outside the fence. Option 3 cannot be used with a non-zero FENCE_MINALT
// @Values: 0:NoAutoEnable,1:AutoEnable,2:AutoEnableDisableFloorOnly,3:EnableWhenArmed
// @User: Standard
GSCALAR(fence_autoenable, "FENCE_AUTOENABLE", 0),
// @Param: FENCE_RET_RALLY
// @DisplayName: Fence Return to Rally
// @Description: When set to 1: on fence breach the plane will return to the nearest rally point rather than the fence return point. If no rally points have been defined the plane will return to the home point.
// @Values: 0:FenceReturnPoint,1:NearestRallyPoint
// @User: Standard
GSCALAR(fence_ret_rally, "FENCE_RET_RALLY", 0),
#endif
// @Param: STALL_PREVENTION
// @DisplayName: Enable stall prevention
// @Description: Enables roll limits at low airspeed in roll limiting flight modes. Roll limits based on aerodynamic load factor in turns and scale on ARSPD_FBW_MIN that must be set correctly. Without airspeed sensor, uses synthetic airspeed from wind speed estimate that may both be inaccurate.

11
ArduPlane/Parameters.h
View File

@ -398,17 +398,6 @@ public:
AP_Int16 waypoint_max_radius;
AP_Int16 rtl_radius;
#if GEOFENCE_ENABLED == ENABLED
AP_Int8 fence_action;
AP_Int8 fence_total;
AP_Int8 fence_channel;
AP_Int16 fence_minalt; // meters
AP_Int16 fence_maxalt; // meters
AP_Int16 fence_retalt; // meters
AP_Enum<FenceAutoEnable> fence_autoenable;
AP_Int8 fence_ret_rally;
#endif
// Fly-by-wire
//
AP_Int8 flybywire_elev_reverse;

23
ArduPlane/Plane.h
View File

@ -834,7 +834,6 @@ private:
void calc_nav_yaw_ground(void);
// GCS_Mavlink.cpp
void send_fence_status(mavlink_channel_t chan);
void send_servo_out(mavlink_channel_t chan);
// Log.cpp
@ -932,28 +931,6 @@ private:
bool fence_stickmixing() const;
#endif
#if GEOFENCE_ENABLED == ENABLED
// geofence.cpp
uint8_t max_fencepoints(void) const;
Vector2l get_fence_point_with_index(uint8_t i) const;
void set_fence_point_with_index(const Vector2l &point, unsigned i);
void geofence_load(void);
bool geofence_present(void) const;
void geofence_update_pwm_enabled_state();
bool geofence_set_enabled(bool enable);
bool geofence_enabled(void);
bool geofence_set_floor_enabled(bool floor_enable);
bool geofence_check_minalt(void);
bool geofence_check_maxalt(void);
void geofence_check(bool altitude_check_only);
bool geofence_prearm_check(void);
bool geofence_stickmixing(void);
void geofence_send_status(mavlink_channel_t chan);
bool geofence_breached(void);
void geofence_disable_and_send_error_msg(const char *errorMsg);
void disable_fence_for_landing(void);
#endif
// ArduPlane.cpp
void disarm_if_autoland_complete();
float tecs_hgt_afe(void);

30
ArduPlane/commands_logic.cpp
View File

@ -142,21 +142,6 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
plane.fence.disable_floor();
gcs().send_text(MAV_SEVERITY_INFO, "Fence floor disabled");
}
#endif
#if GEOFENCE_ENABLED == ENABLED
if (cmd.p1 != 2) {
if (!geofence_set_enabled((bool) cmd.p1)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Unable to set fence. Enabled state to %u", cmd.p1);
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Set fence enabled state to %u", cmd.p1);
}
} else { //commanding to only disable floor
if (! geofence_set_floor_enabled(false)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Unable to disable fence floor");
} else {
gcs().send_text(MAV_SEVERITY_WARNING, "Fence floor disabled");
}
}
#endif
break;
@ -413,21 +398,6 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
}
#endif
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable == FenceAutoEnable::Auto) {
if (!geofence_set_enabled(false)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
} else {
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
}
} else if (g.fence_autoenable == FenceAutoEnable::AutoDisableFloorOnly) {
if (!geofence_set_floor_enabled(false)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
} else {
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
}
}
#endif
}
void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd)

18
ArduPlane/config.h
View File

@ -246,23 +246,9 @@
# define SCALING_SPEED 15.0
#endif
#if 1
// Use AC_FENCE
#define AC_FENCE ENABLED
#define GEOFENCE_ENABLED DISABLED
#else
// Use GEOFENCE
#define AC_FENCE DISABLED
#define GEOFENCE_ENABLED ENABLED
#endif
#ifndef AC_FENCE
# define AC_FENCE DISABLED
#endif
// use this to disable geo-fencing
#ifndef GEOFENCE_ENABLED
# define GEOFENCE_ENABLED ENABLED
#ifndef AC_FENCE
# define AC_FENCE ENABLED
#endif
// pwm value on FENCE_CHANNEL to use to enable fenced mode

4
ArduPlane/fence.cpp
View File

@ -20,7 +20,7 @@ void Plane::fence_check()
case AC_FENCE_ACTION_GUIDED_THROTTLE_PASS:
case AC_FENCE_ACTION_RTL_AND_LAND:
if (plane.control_mode_reason == ModeReason::FENCE_BREACHED &&
(control_mode == &mode_guided || control_mode == &mode_avoidADSB)) {
control_mode->is_guided_mode()) {
set_mode(*previous_mode, ModeReason::FENCE_RETURN_PREVIOUS_MODE);
}
break;
@ -118,7 +118,7 @@ bool Plane::fence_stickmixing(void) const
{
if (fence.enabled() &&
fence.get_breaches() &&
(control_mode == &mode_guided || control_mode == &mode_avoidADSB))
control_mode->is_guided_mode())
{
// don't mix in user input
return false;

568
ArduPlane/geofence.cpp
View File

@ -1,568 +0,0 @@
/*
* geo-fencing support
* Andrew Tridgell, December 2011
*/
#include "Plane.h"
#if GEOFENCE_ENABLED == ENABLED
#define MIN_GEOFENCE_POINTS 5 // index [0] for return point, must be inside polygon
// index [1 to n-1] to define a polygon, minimum 3 for a triangle
// index [n] (must be same as index 1 to close the polygon)
/*
* The state of geo-fencing. This structure is dynamically allocated
* the first time it is used. This means we only pay for the pointer
* and not the structure on systems where geo-fencing is not being
* used.
*
* We store a copy of the boundary in memory as we need to access it
* very quickly at runtime
*/
static struct GeofenceState {
Vector2l *boundary; // point 0 is the return point
uint32_t breach_time;
int32_t guided_lat;
int32_t guided_lng;
uint16_t breach_count;
uint8_t breach_type;
uint8_t old_switch_position;
uint8_t num_points;
bool boundary_uptodate;
bool fence_triggered;
bool is_pwm_enabled; //true if above FENCE_ENABLE_PWM threshold
bool is_enabled;
bool floor_enabled; //typically used for landing
} *geofence_state;
static const StorageAccess fence_storage(StorageManager::StorageFence);
/*
maximum number of fencepoints
*/
uint8_t Plane::max_fencepoints(void) const
{
return MIN(255U, fence_storage.size() / sizeof(Vector2l));
}
/*
* fence boundaries fetch/store
*/
Vector2l Plane::get_fence_point_with_index(uint8_t i) const
{
if (i > (uint8_t)g.fence_total || i >= max_fencepoints()) {
return Vector2l(0,0);
}
// read fence point
return Vector2l(fence_storage.read_uint32(i * sizeof(Vector2l)),
fence_storage.read_uint32(i * sizeof(Vector2l) + sizeof(int32_t)));
}
// save a fence point
void Plane::set_fence_point_with_index(const Vector2l &point, unsigned i)
{
if (i >= (unsigned)g.fence_total.get() || i >= max_fencepoints()) {
// not allowed
return;
}
fence_storage.write_uint32(i * sizeof(Vector2l), point.x);
fence_storage.write_uint32(i * sizeof(Vector2l) + sizeof(int32_t), point.y);
if (geofence_state != nullptr) {
geofence_state->boundary_uptodate = false;
}
}
/*
* allocate and fill the geofence state structure
*/
void Plane::geofence_load(void)
{
if (geofence_state == nullptr) {
uint16_t boundary_size = sizeof(Vector2l) * max_fencepoints();
if (hal.util->available_memory() < 100 + boundary_size + sizeof(struct GeofenceState)) {
// too risky to enable as we could run out of stack
geofence_disable_and_send_error_msg("low on memory");
return;
}
geofence_state = (struct GeofenceState *)calloc(1, sizeof(struct GeofenceState));
if (geofence_state == nullptr) {
// not much we can do here except disable it
geofence_disable_and_send_error_msg("failed to init state memory");
return;
}
geofence_state->boundary = (Vector2l *)calloc(1, boundary_size);
if (geofence_state->boundary == nullptr) {
free(geofence_state);
geofence_state = nullptr;
geofence_disable_and_send_error_msg("failed to init boundary memory");
return;
}
geofence_state->old_switch_position = 254;
}
if (g.fence_total <= 0) {
g.fence_total.set(0);
return;
}
for (uint8_t i = 0; i<g.fence_total; i++) {
geofence_state->boundary[i] = get_fence_point_with_index(i);
}
geofence_state->num_points = g.fence_total;
if (!Polygon_complete(&geofence_state->boundary[1], geofence_state->num_points-1)) {
geofence_disable_and_send_error_msg("pt[1] and pt[total-1] must match");
return;
}
if (Polygon_outside(geofence_state->boundary[0], &geofence_state->boundary[1], geofence_state->num_points-1)) {
geofence_disable_and_send_error_msg("pt[0] must be inside fence");
return;
}
geofence_state->boundary_uptodate = true;
geofence_state->fence_triggered = false;
gcs().send_text(MAV_SEVERITY_INFO,"Geofence loaded");
gcs().send_message(MSG_FENCE_STATUS);
}
/*
* Disable geofence and send an error message string
*/
void Plane::geofence_disable_and_send_error_msg(const char *errorMsg)
{
g.fence_action.set(FENCE_ACTION_NONE);
gcs().send_text(MAV_SEVERITY_WARNING,"Geofence error, %s", errorMsg);
}
/*
* return true if a geo-fence has been uploaded and
* FENCE_ACTION is 1 (not necessarily enabled)
*/
bool Plane::geofence_present(void) const
{
//require at least a return point and a triangle
//to define a geofence area:
if (g.fence_action == FENCE_ACTION_NONE || g.fence_total < MIN_GEOFENCE_POINTS) {
return false;
}
return true;
}
/*
check FENCE_CHANNEL and update the is_pwm_enabled state
*/
void Plane::geofence_update_pwm_enabled_state()
{
if (rc_failsafe_active()) {
// do nothing based on the radio channel value as it may be at bind value
return;
}
bool is_pwm_enabled;
if (g.fence_channel == 0) {
is_pwm_enabled = false;
} else {
is_pwm_enabled = (RC_Channels::get_radio_in(g.fence_channel-1) > FENCE_ENABLE_PWM);
}
if (is_pwm_enabled && geofence_state == nullptr) {
// we need to load the fence
geofence_load();
return;
}
if (geofence_state == nullptr) {
// not loaded
return;
}
if (geofence_state->is_pwm_enabled != is_pwm_enabled) {
geofence_set_enabled(is_pwm_enabled);
geofence_state->is_pwm_enabled = is_pwm_enabled;
}
}
//return true on success, false on failure
bool Plane::geofence_set_enabled(bool enable)
{
if (geofence_state == nullptr && enable) {
geofence_load();
}
if (geofence_state == nullptr) {
return false;
}
geofence_state->is_enabled = enable;
if (enable == true) {
//turn the floor back on if it had been off
geofence_set_floor_enabled(true);
}
return true;
}
/*
* return true if geo-fencing is enabled
*/
bool Plane::geofence_enabled(void)
{
if (g.fence_action == FENCE_ACTION_NONE) {
if (geofence_state != nullptr) {
geofence_state->fence_triggered = false;
}
return false;
}
geofence_update_pwm_enabled_state();
if (geofence_state == nullptr) {
return false;
}
if (!geofence_present() ||
(g.fence_action != FENCE_ACTION_REPORT && !geofence_state->is_enabled)) {
// geo-fencing is disabled
// re-arm for when the channel trigger is switched on
geofence_state->fence_triggered = false;
return false;
}
return true;
}
/*
* Set floor state IF the fence is present.
* Return false on failure to set floor state.
*/
bool Plane::geofence_set_floor_enabled(bool floor_enable) {
if (geofence_state == nullptr) {
return false;
}
geofence_state->floor_enabled = floor_enable;
return true;
}
/*
* return true if we have breached the geo-fence minimum altiude
*/
bool Plane::geofence_check_minalt(void)
{
if (g.fence_maxalt <= g.fence_minalt) {
return false;
}
if (g.fence_minalt == 0) {
return false;
}
return (adjusted_altitude_cm() < (g.fence_minalt*100.0f) + home.alt);
}
/*
* return true if we have breached the geo-fence maximum altiude
*/
bool Plane::geofence_check_maxalt(void)
{
if (g.fence_maxalt <= g.fence_minalt) {
return false;
}
if (g.fence_maxalt == 0) {
return false;
}
return (adjusted_altitude_cm() > (g.fence_maxalt*100.0f) + home.alt);
}
/*
pre-arm check for being inside the fence
*/
bool Plane::geofence_prearm_check(void)
{
if (!geofence_enabled()) {
gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: Fence not enabled");
return false;
}
/* allocate the geo-fence state if need be */
if (geofence_state == nullptr || !geofence_state->boundary_uptodate) {
geofence_load();
if (!geofence_enabled()) {
// may have been disabled by load
gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: Fence load failed");
return false;
}
}
if (geofence_state->floor_enabled && g.fence_minalt != 0) {
// can't use minalt with prearm check
gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: Fence floor enabled");
return false;
}
if (geofence_check_maxalt()) {
gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: maxalt breached");
return false;
}
struct Location loc;
if (!ahrs.get_position(loc)) {
gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: no position available");
// must have position
return false;
}
Vector2l location;
location.x = loc.lat;
location.y = loc.lng;
bool outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
if (outside) {
gcs().send_text(MAV_SEVERITY_WARNING, "PreArm: outside fence");
return false;
}
return true;
}
/*
* check if we have breached the geo-fence
*/
void Plane::geofence_check(bool altitude_check_only)
{
if (!geofence_enabled()) {
// switch back to the chosen control mode if still in
// GUIDED to the return point
if (geofence_state != nullptr &&
(g.fence_action == FENCE_ACTION_GUIDED || g.fence_action == FENCE_ACTION_GUIDED_THR_PASS || g.fence_action == FENCE_ACTION_RTL) &&
control_mode->is_guided_mode() &&
geofence_present() &&
geofence_state->boundary_uptodate &&
geofence_state->old_switch_position == oldSwitchPosition &&
guided_WP_loc.lat == geofence_state->guided_lat &&
guided_WP_loc.lng == geofence_state->guided_lng) {
geofence_state->old_switch_position = 254;
set_mode(*previous_mode, ModeReason::GCS_COMMAND);
}
return;
}
/* allocate the geo-fence state if need be */
if (geofence_state == nullptr || !geofence_state->boundary_uptodate) {
geofence_load();
if (!geofence_enabled()) {
// may have been disabled by load
return;
}
}
bool outside = false;
uint8_t breach_type = FENCE_BREACH_NONE;
struct Location loc;
// Never trigger a fence breach in the final stage of landing
if (landing.is_expecting_impact()) {
return;
}
if (geofence_state->floor_enabled && geofence_check_minalt()) {
outside = true;
breach_type = FENCE_BREACH_MINALT;
} else if (geofence_check_maxalt()) {
outside = true;
breach_type = FENCE_BREACH_MAXALT;
} else if (!altitude_check_only && ahrs.get_position(loc)) {
Vector2l location;
location.x = loc.lat;
location.y = loc.lng;
outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
if (outside) {
breach_type = FENCE_BREACH_BOUNDARY;
}
}
if (!outside) {
if (geofence_state->fence_triggered && !altitude_check_only) {
// we have moved back inside the fence
geofence_state->fence_triggered = false;
gcs().send_text(MAV_SEVERITY_INFO,"Geofence OK");
#if FENCE_TRIGGERED_PIN > 0
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->write(FENCE_TRIGGERED_PIN, 0);
#endif
gcs().send_message(MSG_FENCE_STATUS);
}
// we're inside, all is good with the world
return;
}
// we are outside the fence
if (geofence_state->fence_triggered &&
(control_mode->is_guided_mode()
|| control_mode == &mode_rtl || g.fence_action == FENCE_ACTION_REPORT)) {
// we have already triggered, don't trigger again until the
// user disables/re-enables using the fence channel switch
return;
}
// we are outside, and have not previously triggered.
geofence_state->fence_triggered = true;
geofence_state->breach_count++;
geofence_state->breach_time = millis();
geofence_state->breach_type = breach_type;
#if FENCE_TRIGGERED_PIN > 0
hal.gpio->pinMode(FENCE_TRIGGERED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->write(FENCE_TRIGGERED_PIN, 1);
#endif
gcs().send_text(MAV_SEVERITY_NOTICE,"Geofence triggered");
gcs().send_message(MSG_FENCE_STATUS);
// see what action the user wants
switch (g.fence_action) {
case FENCE_ACTION_REPORT:
break;
case FENCE_ACTION_GUIDED:
case FENCE_ACTION_GUIDED_THR_PASS:
case FENCE_ACTION_RTL:
// make sure we don't auto trim the surfaces on this mode change
int8_t saved_auto_trim = g.auto_trim;
g.auto_trim.set(0);
if (g.fence_action == FENCE_ACTION_RTL) {
set_mode(mode_rtl, ModeReason::FENCE_BREACHED);
} else {
set_mode(mode_guided, ModeReason::FENCE_BREACHED);
}
g.auto_trim.set(saved_auto_trim);
if (g.fence_ret_rally != 0 || g.fence_action == FENCE_ACTION_RTL) { //return to a rally point
guided_WP_loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
} else { //return to fence return point, not a rally point
guided_WP_loc = {};
if (g.fence_retalt > 0) {
//fly to the return point using fence_retalt
guided_WP_loc.alt = home.alt + 100.0f*g.fence_retalt;
} else if (g.fence_minalt >= g.fence_maxalt) {
// invalid min/max, use RTL_altitude
guided_WP_loc.alt = home.alt + g.RTL_altitude_cm;
} else {
// fly to the return point, with an altitude half way between
// min and max
guided_WP_loc.alt = home.alt + 100.0f*(g.fence_minalt + g.fence_maxalt)/2;
}
guided_WP_loc.lat = geofence_state->boundary[0].x;
guided_WP_loc.lng = geofence_state->boundary[0].y;
}
geofence_state->guided_lat = guided_WP_loc.lat;
geofence_state->guided_lng = guided_WP_loc.lng;
geofence_state->old_switch_position = oldSwitchPosition;
if (g.fence_action != FENCE_ACTION_RTL) { //not needed for RTL mode
setup_terrain_target_alt(guided_WP_loc);
set_guided_WP();
}
if (g.fence_action == FENCE_ACTION_GUIDED_THR_PASS) {
guided_throttle_passthru = true;
}
break;
}
}
/*
* return true if geofencing allows stick mixing. When we have
* triggered failsafe and are in GUIDED mode then stick mixing is
* disabled. Otherwise the aircraft may not be able to recover from
* a breach of the geo-fence
*/
bool Plane::geofence_stickmixing(void) {
if (geofence_enabled() &&
geofence_state != nullptr &&
geofence_state->fence_triggered &&
control_mode->is_guided_mode()) {
// don't mix in user input
return false;
}
// normal mixing rules
return true;
}
/*
*
*/
void Plane::geofence_send_status(mavlink_channel_t chan)
{
if (geofence_enabled() && geofence_state != nullptr) {
mavlink_msg_fence_status_send(chan,
(int8_t)geofence_state->fence_triggered,
geofence_state->breach_count,
geofence_state->breach_type,
geofence_state->breach_time,
FENCE_MITIGATE_NONE);
}
}
/*
return true if geofence has been breached
*/
bool Plane::geofence_breached(void)
{
return geofence_state ? geofence_state->fence_triggered : false;
}
void Plane::disable_fence_for_landing(void)
{
if (g.fence_autoenable == FenceAutoEnable::Auto) {
if (!geofence_set_enabled(false)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (auto-disable)");
} else {
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (auto-disable)");
}
} else if (g.fence_autoenable == FenceAutoEnable::AutoDisableFloorOnly) {
if (!geofence_set_floor_enabled(false)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (auto-disable)");
} else {
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto-disable)");
}
}
}
#else // GEOFENCE_ENABLED
#if 0
void Plane::geofence_check(bool altitude_check_only) {
}
bool Plane::geofence_stickmixing(void) {
return true;
}
bool Plane::geofence_enabled(void) {
return false;
}
bool Plane::geofence_present(void) {
return false;
}
bool Plane::geofence_set_enabled(bool enable) {
return false;
}
bool Plane::geofence_set_floor_enabled(bool floor_enable) {
return false;
}
bool Plane::geofence_breached(void)
{
return false;
}
#endif
void Plane::disable_fence_for_landing(void)
{
}
#endif // GEOFENCE_ENABLED

3
ArduPlane/radio.cpp
View File

@ -189,9 +189,6 @@ void Plane::read_radio()
airspeed_nudge_cm = 0;
throttle_nudge = 0;
if (g.throttle_nudge && channel_throttle->get_control_in() > 50
#if GEOFENCE_ENABLED == ENABLED
&& geofence_stickmixing()
#endif
#if AC_FENCE == ENABLED
&& fence_stickmixing()
#endif

9
ArduPlane/takeoff.cpp
View File

@ -286,15 +286,6 @@ void Plane::complete_auto_takeoff(void)
break;
}
#endif
#if GEOFENCE_ENABLED == ENABLED
if (g.fence_autoenable != FenceAutoEnable::OFF) {
if (! geofence_set_enabled(true)) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Enable fence failed (cannot autoenable");
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Fence enabled (autoenabled)");
}
}
#endif
}