ArduPlane: Add support for AC_Fence

ArduPlane: Remove some geofence logic from ArduPlane

ArduPlane: Refactor action and mode return implementation

ArduPlane: Use polyfence to find the return point.

ArduPlane/ArduPlane.cpp

@@ -58,6 +58,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300),
     SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150),
     SCHED_TASK_CLASS(AP_Notify,      &plane.notify,  update, 50, 300),
+#if AC_FENCE == ENABLED
+    SCHED_TASK_CLASS(AC_Fence,       &plane.fence,   update, 10, 100),
+#endif
     SCHED_TASK(read_rangefinder,       50,    100),
     SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100),
     SCHED_TASK_CLASS(Compass,          &plane.compass,              cal_update, 50, 50),
@@ -66,6 +69,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK_CLASS(OpticalFlow, &plane.optflow, update,    50,    50),
 #endif
     SCHED_TASK(one_second_loop,         1,    400),
+    SCHED_TASK(three_hz_loop,           3,    75),
     SCHED_TASK(check_long_failsafe,     3,    400),
     SCHED_TASK(rpm_update,             10,    100),
 #if AP_AIRSPEED_AUTOCAL_ENABLE
@@ -243,7 +247,12 @@ void Plane::update_logging2(void)
 void Plane::afs_fs_check(void)
 {
     // perform AFS failsafe checks
-    afs.check(failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms);
+#if AC_FENCE == ENABLED
+    const bool fence_breached = fence.get_breaches() != 0;
+#else
+    const bool fence_breached = false;
+#endif
+    afs.check(failsafe.last_heartbeat_ms, fence_breached, failsafe.AFS_last_valid_rc_ms);
 }
 #endif
 
@@ -299,6 +308,13 @@ void Plane::one_second_loop()
     }
 }
 
+void Plane::three_hz_loop()
+{
+#if AC_FENCE == ENABLED
+    fence_check();
+#endif
+}
+
 void Plane::compass_save()
 {
     if (AP::compass().enabled() &&
@@ -391,8 +407,10 @@ 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();
@@ -478,7 +496,9 @@ 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();
 

ArduPlane/Attitude.cpp

@@ -58,7 +58,12 @@ 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
             failsafe.state == FAILSAFE_NONE &&
             !rc_failsafe_active()) {
             // we're in an auto mode, and haven't triggered failsafe

ArduPlane/GCS_Mavlink.cpp

@@ -415,12 +415,12 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
 #endif
         break;
 
-    case MSG_FENCE_STATUS:
 #if GEOFENCE_ENABLED == ENABLED
+    case MSG_FENCE_STATUS:
         CHECK_PAYLOAD_SIZE(FENCE_STATUS);
         plane.send_fence_status(chan);
-#endif
         break;
+#endif
 
     case MSG_TERRAIN:
 #if AP_TERRAIN_AVAILABLE
@@ -1050,7 +1050,9 @@ 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;
@@ -1076,6 +1078,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
             break;
         }
         return MAV_RESULT_FAILED;
+#endif
 
     case MAV_CMD_DO_SET_HOME: {
         // param1 : use current (1=use current location, 0=use specified location)

ArduPlane/GCS_Plane.cpp

@@ -20,9 +20,12 @@ void GCS_Plane::update_vehicle_sensor_status_flags(void)
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
     }
 #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;
@@ -32,9 +35,11 @@ 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 |

ArduPlane/Parameters.cpp

@@ -1073,6 +1073,12 @@ const AP_Param::Info Plane::var_info[] = {
     // @Path: ../libraries/AP_Rally/AP_Rally.cpp
     GOBJECT(rally,  "RALLY_",       AP_Rally),
 
+#if AC_FENCE == ENABLED
+    // @Group: FENCE_
+    // @Path: ../libraries/AC_Fence/AC_Fence.cpp
+    GOBJECT(fence, "FENCE_",        AC_Fence),
+#endif
+
 #if AP_AHRS_NAVEKF_AVAILABLE
 #if HAL_NAVEKF2_AVAILABLE
     // @Group: EK2_

ArduPlane/Parameters.h

@@ -350,6 +350,7 @@ public:
         k_param_gcs4,          // stream rates
         k_param_gcs5,          // stream rates
         k_param_gcs6,          // stream rates
+        k_param_fence,         // vehicle fence
     };
 
     AP_Int16 format_version;

ArduPlane/Plane.h

@@ -98,6 +98,10 @@
 #include "afs_plane.h"
 #endif
 
+#if AC_FENCE == ENABLED
+#include <AC_Fence/AC_Fence.h>
+#endif
+
 // Local modules
 #include "defines.h"
 #include "mode.h"
@@ -257,6 +261,10 @@ private:
     AP_OSD osd;
 #endif
 
+#if AC_FENCE == ENABLED
+    AC_Fence fence;
+#endif
+
     ModeCircle mode_circle;
     ModeStabilize mode_stabilize;
     ModeTraining mode_training;
@@ -918,6 +926,13 @@ private:
     void failsafe_long_off_event(ModeReason reason);
     void handle_battery_failsafe(const char* type_str, const int8_t action);
 
+#if AC_FENCE == ENABLED
+    // fence.cpp
+    void fence_check();
+    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;
@@ -937,6 +952,7 @@ private:
     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();
@@ -955,6 +971,7 @@ private:
     void afs_fs_check(void);
 #endif
     void one_second_loop(void);
+    void three_hz_loop(void);
 #if AP_AIRSPEED_AUTOCAL_ENABLE
     void airspeed_ratio_update(void);
 #endif 

ArduPlane/commands_logic.cpp

@@ -131,6 +131,18 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_DO_FENCE_ENABLE:
+#if AC_FENCE == ENABLED
+        if (cmd.p1 == 0) { // disable fence
+            plane.fence.enable(false);
+            gcs().send_text(MAV_SEVERITY_INFO, "Fence disabled");
+        } else if (cmd.p1 == 1) { // enable fence
+            plane.fence.enable(true);
+            gcs().send_text(MAV_SEVERITY_INFO, "Fence enabled");
+        } else if (cmd.p1 == 2) { // disable fence floor only
+            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)) {
@@ -385,7 +397,37 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
         set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
     }
 
-    disable_fence_for_landing();
+#if AC_FENCE == ENABLED
+    switch(fence.auto_enabled()) {
+        case AC_Fence::AutoEnable::ALWAYS_ENABLED:
+            fence.enable(false);
+            gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)");
+            break;
+        case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
+            fence.disable_floor();
+            gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
+            break;
+        default:
+            // fence does not auto-disable in other landing conditions
+            break;
+    }
+#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)

ArduPlane/config.h

@@ -246,6 +246,20 @@
  # 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

ArduPlane/fence.cpp

@@ -0,0 +1,129 @@
+#include "Plane.h"
+
+// Code to integrate AC_Fence library with main ArduPlane code
+
+#if AC_FENCE == ENABLED
+
+// fence_check - ask fence library to check for breaches and initiate the response
+void Plane::fence_check()
+{
+    const uint8_t orig_breaches = fence.get_breaches();
+
+    // check for new breaches; new_breaches is bitmask of fence types breached
+    const uint8_t new_breaches = fence.check();
+
+    if (!fence.enabled()) {
+        // Switch back to the chosen control mode if still in
+        // GUIDED to the return point
+        switch(fence.get_action()) {
+            case AC_FENCE_ACTION_GUIDED:
+            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)) {
+                    set_mode(*previous_mode, ModeReason::FENCE_RETURN_PREVIOUS_MODE);
+                }
+                break;
+            default:
+                // No returning to a previous mode, unless our action allows it
+                break;
+        }
+    }
+
+    // we still don't do anything when disarmed, but we do check for fence breaches.
+    // fence pre-arm check actually checks if any fence has been breached
+    // that's not ever going to be true if we don't call check on AP_Fence while disarmed
+    if (!arming.is_armed()) {
+        return;
+    }
+
+    // Never trigger a fence breach in the final stage of landing
+    if (landing.is_expecting_impact()) {
+        return;
+    }
+
+    if (new_breaches) {
+        // if the user wants some kind of response and motors are armed
+        const uint8_t fence_act = fence.get_action();
+        switch (fence_act) {
+        case AC_FENCE_ACTION_REPORT_ONLY:
+            break;
+        case AC_FENCE_ACTION_GUIDED:
+        case AC_FENCE_ACTION_GUIDED_THROTTLE_PASS:
+        case AC_FENCE_ACTION_RTL_AND_LAND:
+            // 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 (fence_act == AC_FENCE_ACTION_RTL_AND_LAND) {
+                set_mode(mode_rtl, ModeReason::FENCE_BREACHED);
+            } else {
+                set_mode(mode_guided, ModeReason::FENCE_BREACHED);
+            }
+            g.auto_trim.set(saved_auto_trim);
+
+            if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) {
+                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 (fence.get_return_altitude() > 0) {
+                    // fly to the return point using _retalt
+                    guided_WP_loc.alt = home.alt + 100.0f * fence.get_return_altitude();
+                } else if (fence.get_safe_alt_min() >= fence.get_safe_alt_max()) {
+                    // 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 * (fence.get_safe_alt_min() + fence.get_safe_alt_max()) / 2;
+                }
+
+                Vector2l return_point;
+                if(fence.polyfence().get_return_point(return_point)) {
+                    guided_WP_loc.lat = return_point[0];
+                    guided_WP_loc.lng = return_point[1];
+                } else {
+                    // should. not. happen.
+                    guided_WP_loc.lat = current_loc.lat;
+                    guided_WP_loc.lng = current_loc.lng;
+                }
+            }
+
+
+            //! TODO: Update setting of guided location
+            /*
+                fence_state->guided_lat = guided_WP_loc.lat;
+                fence_state->guided_lng = guided_WP_loc.lng;
+            */
+
+            if (fence.get_action() != AC_FENCE_ACTION_RTL_AND_LAND) {
+                setup_terrain_target_alt(guided_WP_loc);
+                set_guided_WP();
+            }
+
+            if (fence.get_action() == AC_FENCE_ACTION_GUIDED_THROTTLE_PASS) {
+                guided_throttle_passthru = true;
+            }
+            break;
+        }
+
+        AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches));
+    } else if (orig_breaches) {
+        // record clearing of breach
+        AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED);
+    }
+}
+
+bool Plane::fence_stickmixing(void) const
+{
+    if (fence.enabled() &&
+        fence.get_breaches() &&
+        (control_mode == &mode_guided || control_mode == &mode_avoidADSB))
+    {
+        // don't mix in user input
+        return false;
+    }
+    // normal mixing rules
+    return true;
+}
+#endif

ArduPlane/geofence.cpp

@@ -53,6 +53,7 @@ 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
@@ -532,7 +533,7 @@ void Plane::disable_fence_for_landing(void)
 
 
 #else // GEOFENCE_ENABLED
-
+#if 0
 void Plane::geofence_check(bool altitude_check_only) {
 }
 bool Plane::geofence_stickmixing(void) {
@@ -558,6 +559,7 @@ bool Plane::geofence_breached(void)
 {
     return false;
 }
+#endif
 
 void Plane::disable_fence_for_landing(void)
 {

ArduPlane/radio.cpp

@@ -188,7 +188,14 @@ void Plane::read_radio()
 
     airspeed_nudge_cm = 0;
     throttle_nudge = 0;
-    if (g.throttle_nudge && channel_throttle->get_control_in() > 50 && geofence_stickmixing()) {
+    if (g.throttle_nudge && channel_throttle->get_control_in() > 50
+        #if GEOFENCE_ENABLED == ENABLED
+        && geofence_stickmixing()
+        #endif
+        #if AC_FENCE == ENABLED
+        && fence_stickmixing()
+        #endif
+        ) {
         float nudge = (channel_throttle->get_control_in() - 50) * 0.02f;
         if (ahrs.airspeed_sensor_enabled()) {
             airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge;

ArduPlane/system.cpp

@@ -154,6 +154,11 @@ void Plane::init_ardupilot()
 #if GRIPPER_ENABLED == ENABLED
     g2.gripper.init();
 #endif
+
+    // init fence
+#if AC_FENCE == ENABLED
+    fence.init();
+#endif
 }
 
 //********************************************************************************

ArduPlane/takeoff.cpp

@@ -275,6 +275,17 @@ return_zero:
  */
 void Plane::complete_auto_takeoff(void)
 {
+#if AC_FENCE == ENABLED
+    switch(fence.auto_enabled()) {
+        case AC_Fence::AutoEnable::ALWAYS_ENABLED:
+        case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
+            fence.enable(true);
+            break;
+        default:
+            // fence does not auto-enable in other takeoff conditions
+            break;
+    }
+#endif
 #if GEOFENCE_ENABLED == ENABLED
     if (g.fence_autoenable != FenceAutoEnable::OFF) {
         if (! geofence_set_enabled(true)) {