Rover: add fence support

APMrover2/APMrover2.cpp

@@ -71,6 +71,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(mount_update,           50,    600),
     SCHED_TASK(update_trigger,         50,    600),
     SCHED_TASK(gcs_failsafe_check,     10,    600),
+    SCHED_TASK(fence_check,            10,    100),
     SCHED_TASK(compass_accumulate,     50,    900),
     SCHED_TASK(smart_rtl_update,        3,    100),
     SCHED_TASK(update_notify,          50,    300),

APMrover2/AP_Arming.cpp

@@ -72,5 +72,20 @@ bool AP_Arming_Rover::gps_checks(bool display_failure)
 
 bool AP_Arming_Rover::pre_arm_checks(bool report)
 {
-    return rover.g2.motors.pre_arm_check(report) & AP_Arming::pre_arm_checks(report);
+    return (AP_Arming::pre_arm_checks(report)
+            & rover.g2.motors.pre_arm_check(report)
+            & fence_checks(report));
+}
+
+bool AP_Arming_Rover::fence_checks(bool report)
+{
+    // check fence is initialised
+    const char *fail_msg = nullptr;
+    if (!_fence.pre_arm_check(fail_msg)) {
+        if (report && fail_msg != nullptr) {
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Fence : %s", fail_msg);
+        }
+        return false;
+    }
+    return true;
 }

APMrover2/AP_Arming.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <AP_Arming/AP_Arming.h>
+#include <AC_Fence/AC_Fence.h>
 
 /*
   a rover-specific arming class
@@ -9,8 +10,9 @@ class AP_Arming_Rover : public AP_Arming
 {
 public:
     AP_Arming_Rover(const AP_AHRS &ahrs_ref, const AP_Baro &baro, Compass &compass,
-                    const AP_BattMonitor &battery)
-        : AP_Arming(ahrs_ref, baro, compass, battery)
+                    const AP_BattMonitor &battery, const AC_Fence &fence)
+        : AP_Arming(ahrs_ref, baro, compass, battery),
+          _fence(fence)
     {
     }
 
@@ -24,4 +26,8 @@ public:
 
 protected:
     enum HomeState home_status() const override;
+    bool fence_checks(bool report);
+
+private:
+    const AC_Fence& _fence;
 };

APMrover2/GCS_Mavlink.cpp

@@ -254,6 +254,11 @@ void Rover::send_pid_tuning(mavlink_channel_t chan)
     }
 }
 
+void Rover::send_fence_status(mavlink_channel_t chan)
+{
+    fence_send_mavlink_status(chan);
+}
+
 void Rover::send_wheel_encoder(mavlink_channel_t chan)
 {
     // send wheel encoder data using rpm message
@@ -390,6 +395,11 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
 #endif  // MOUNT == ENABLED
         break;
 
+    case MSG_FENCE_STATUS:
+        CHECK_PAYLOAD_SIZE(FENCE_STATUS);
+        rover.send_fence_status(chan);
+        break;
+
     case MSG_VIBRATION:
         CHECK_PAYLOAD_SIZE(VIBRATION);
         send_vibration(rover.ins);
@@ -563,6 +573,7 @@ GCS_MAVLINK_Rover::data_stream_send(void)
         send_message(MSG_GPS2_RAW);
         send_message(MSG_GPS2_RTK);
         send_message(MSG_NAV_CONTROLLER_OUTPUT);
+        send_message(MSG_FENCE_STATUS);
     }
 
     if (gcs().out_of_time()) {
@@ -883,6 +894,21 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             }
             break;
 
+        case MAV_CMD_DO_FENCE_ENABLE:
+            result = MAV_RESULT_ACCEPTED;
+            switch ((uint16_t)packet.param1) {
+                case 0:
+                    rover.g2.fence.enable(false);
+                    break;
+                case 1:
+                    rover.g2.fence.enable(true);
+                    break;
+                default:
+                    result = MAV_RESULT_FAILED;
+                    break;
+            }
+            break;
+
         case MAV_CMD_GET_HOME_POSITION:
             if (rover.home_is_set != HOME_UNSET) {
                 send_home(rover.ahrs.get_home());
@@ -1352,6 +1378,12 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+    // send or receive fence points with GCS
+    case MAVLINK_MSG_ID_FENCE_POINT:  // MAV ID: 160
+    case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
+        rover.g2.fence.handle_msg(*this, msg);
+        break;
+
     case MAVLINK_MSG_ID_DISTANCE_SENSOR:
         rover.rangefinder.handle_msg(msg);
         break;

APMrover2/Parameters.cpp

@@ -542,6 +542,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("FRAME_CLASS", 16, ParametersG2, frame_class, 1),
 
+    // @Group: FENCE_
+    // @Path: ../libraries/AC_Fence/AC_Fence.cpp
+    AP_SUBGROUPINFO(fence, "FENCE_", 17, ParametersG2, AC_Fence),
+
     AP_GROUPEND
 };
 
@@ -554,7 +558,8 @@ ParametersG2::ParametersG2(void)
     beacon(rover.serial_manager),
     motors(rover.ServoRelayEvents),
     attitude_control(rover.ahrs),
-    smart_rtl(rover.ahrs)
+    smart_rtl(rover.ahrs),
+    fence(rover.ahrs)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }

APMrover2/Parameters.h

@@ -329,6 +329,9 @@ public:
 
     // frame class for vehicle
     AP_Int8 frame_class;
+
+    // fence library
+    AC_Fence fence;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -76,6 +76,7 @@
 #include <Filter/ModeFilter.h>                      // Mode Filter from Filter library
 #include <RC_Channel/RC_Channel.h>                  // RC Channel Library
 #include <StorageManager/StorageManager.h>
+#include <AC_Fence/AC_Fence.h>
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include <SITL/SITL.h>
 #endif
@@ -174,7 +175,7 @@ private:
 #endif
 
     // Arming/Disarming management class
-    AP_Arming_Rover arming{ahrs, barometer, compass, battery};
+    AP_Arming_Rover arming{ahrs, barometer, compass, battery, g2.fence};
 
     AP_L1_Control L1_controller{ahrs, nullptr};
 
@@ -480,6 +481,10 @@ private:
     void afs_fs_check(void);
 #endif
 
+    // fence.cpp
+    void fence_check();
+    void fence_send_mavlink_status(mavlink_channel_t chan);
+
     // GCS_Mavlink.cpp
     void send_heartbeat(mavlink_channel_t chan);
     void send_attitude(mavlink_channel_t chan);
@@ -492,6 +497,7 @@ private:
     void send_rangefinder(mavlink_channel_t chan);
     void send_pid_tuning(mavlink_channel_t chan);
     void send_wheel_encoder(mavlink_channel_t chan);
+    void send_fence_status(mavlink_channel_t chan);
     void gcs_data_stream_send(void);
     void gcs_update(void);
     void gcs_retry_deferred(void);

APMrover2/commands_logic.cpp

@@ -117,6 +117,16 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
         do_set_reverse(cmd);
         break;
 
+    case MAV_CMD_DO_FENCE_ENABLE:
+        if (cmd.p1 == 0) {  //disable
+            g2.fence.enable(false);
+            gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled");
+        } else {  //enable fence
+            g2.fence.enable(true);
+            gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled");
+        }
+        break;
+
     default:
         // return false for unhandled commands
         return false;
@@ -193,6 +203,7 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
     case MAV_CMD_DO_SET_ROI:
     case MAV_CMD_DO_SET_REVERSE:
+    case MAV_CMD_DO_FENCE_ENABLE:
         return true;
 
     default:

APMrover2/defines.h

@@ -100,6 +100,8 @@ enum mode {
 #define MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE      (1<<6)
 #define MAVLINK_SET_ATT_TYPE_MASK_ATTITUDE_IGNORE      (1<<7)
 
+// general error codes
+#define ERROR_CODE_ERROR_RESOLVED       0
 // Error message sub systems and error codes
 #define ERROR_SUBSYSTEM_FAILSAFE_FENCE  9
 #define ERROR_SUBSYSTEM_FLIGHT_MODE     10
@@ -122,7 +124,8 @@ enum mode_reason_t {
     MODE_REASON_FAILSAFE,
     MODE_REASON_MISSION_END,
     MODE_REASON_CRASH_FAILSAFE,
-    MODE_REASON_MISSION_COMMAND
+    MODE_REASON_MISSION_COMMAND,
+    MODE_REASON_FENCE_BREACH,
 };
 
 // values used by the ap.ch7_opt and ap.ch8_opt flags

APMrover2/failsafe.cpp

@@ -102,6 +102,6 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
 void Rover::afs_fs_check(void)
 {
     // perform AFS failsafe checks
-    g2.afs.check(rover.last_heartbeat_ms, false, failsafe.last_valid_rc_ms);  // Rover don't have fence
+    g2.afs.check(rover.last_heartbeat_ms, rover.g2.fence.get_breaches() != 0, failsafe.last_valid_rc_ms);
 }
 #endif

APMrover2/fence.cpp

@@ -0,0 +1,58 @@
+#include "Rover.h"
+
+// fence_check - ask fence library to check for breaches and initiate the response
+void Rover::fence_check()
+{
+    uint8_t new_breaches;  // the type of fence that has been breached
+    const uint8_t orig_breaches = g2.fence.get_breaches();
+
+    // check for a breach
+    new_breaches = g2.fence.check();
+
+    // return immediately if motors are not armed
+    if (!arming.is_armed()) {
+        return;
+    }
+
+    // if there is a new breach take action
+    if (new_breaches) {
+        // if the user wants some kind of response and motors are armed
+        if (g2.fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY) {
+            // if we are within 100m of the fence, RTL
+            if (g2.fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
+                if (!set_mode(mode_rtl, MODE_REASON_FENCE_BREACH)) {
+                    set_mode(mode_hold, MODE_REASON_FENCE_BREACH);
+                }
+            } else {
+                // if more than 100m outside the fence just force to HOLD
+                set_mode(mode_hold, MODE_REASON_FENCE_BREACH);
+            }
+        }
+        // log an error in the dataflash
+        Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, new_breaches);
+
+    } else if (orig_breaches) {
+        // record clearing of breach
+        Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED);
+    }
+}
+
+// fence_send_mavlink_status - send fence status to ground station
+void Rover::fence_send_mavlink_status(mavlink_channel_t chan)
+{
+    if (g2.fence.enabled()) {
+        // traslate fence library breach types to mavlink breach types
+        uint8_t mavlink_breach_type = FENCE_BREACH_NONE;
+        const uint8_t breaches = g2.fence.get_breaches();
+        if ((breaches & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) != 0) {
+            mavlink_breach_type = FENCE_BREACH_BOUNDARY;
+        }
+
+        // send status
+        mavlink_msg_fence_status_send(chan,
+                                      static_cast<int8_t>(g2.fence.get_breaches() != 0),
+                                      g2.fence.get_breach_count(),
+                                      mavlink_breach_type,
+                                      g2.fence.get_breach_time());
+    }
+}

APMrover2/system.cpp

@@ -224,6 +224,11 @@ bool Rover::set_mode(Mode &new_mode, mode_reason_t reason)
 
     control_mode = &new_mode;
 
+    // pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover
+    // this flight mode change could be automatic (i.e. fence, battery, GPS or GCS failsafe)
+    // but it should be harmless to disable the fence temporarily in these situations as well
+    g2.fence.manual_recovery_start();
+
 #if FRSKY_TELEM_ENABLED == ENABLED
     frsky_telemetry.update_control_mode(control_mode->mode_number());
 #endif

APMrover2/wscript

@@ -22,6 +22,7 @@ def build(bld):
             'AP_AdvancedFailsafe',
             'AP_WheelEncoder',
             'AP_SmartRTL',
+            'AC_Fence',
         ],
     )