mirror of https://github.com/ArduPilot/ardupilot
ArduPlane: Remove all unused geofence capability
ArduPlane: Remove missed geofence logic
This commit is contained in:
James O'Shannessy
Peter Barker
parent
a23e8d46da
commit
aae092b88f
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue