geofence: main geo-fence code

this implements the primary logic of geo-fencing

ArduPlane/ArduPlane.pde

@@ -818,6 +818,8 @@ static void update_GPS(void)
 		current_loc.lng = g_gps->longitude;    // Lon * 10**7
 		current_loc.lat = g_gps->latitude;     // Lat * 10**7
 
+        // see if we've breached the geo-fence
+        geofence_check();
 	}
 }
 

ArduPlane/Attitude.pde

@@ -61,8 +61,11 @@ static void stabilize()
 
 	// Mix Stick input to allow users to override control surfaces
 	// -----------------------------------------------------------
-	if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B && failsafe == FAILSAFE_NONE)) {
+	if ((control_mode < FLY_BY_WIRE_A) ||
-
+        (ENABLE_STICK_MIXING == 1 &&
+         geofence_stickmixing() &&
+         control_mode > FLY_BY_WIRE_B &&
+         failsafe == FAILSAFE_NONE)) {
 
 		// TODO: use RC_Channel control_mix function?
 		ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
@@ -92,7 +95,10 @@ static void stabilize()
 	// stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
 	// important for steering on the ground during landing
 	// -----------------------------------------------
-	if (control_mode <= FLY_BY_WIRE_B || (ENABLE_STICK_MIXING == 1 && failsafe == FAILSAFE_NONE)) {
+	if (control_mode <= FLY_BY_WIRE_B ||
+        (ENABLE_STICK_MIXING == 1 &&
+         geofence_stickmixing() &&
+         failsafe == FAILSAFE_NONE)) {
 		ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
 		ch4_inf = fabs(ch4_inf);
 		ch4_inf = min(ch4_inf, 400.0);

ArduPlane/GCS_Mavlink.pde

@@ -1631,6 +1631,38 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+#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 (g.fence_action != FENCE_ACTION_NONE) {
+            send_text(SEVERITY_LOW,PSTR("fencing must be disabled"));
+        } else if (packet.count != g.fence_total) {
+            send_text(SEVERITY_LOW,PSTR("bad fence point"));
+        } else {
+            Vector2f point;
+            point.x = packet.lat;
+            point.y = packet.lng;
+            set_fence_point_with_index(point, 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 >= g.fence_total) {
+            send_text(SEVERITY_LOW,PSTR("bad fence point"));
+        } else {
+            Vector2f point = get_fence_point_with_index(packet.idx);
+            mavlink_msg_fence_point_send(chan, packet.idx, g.fence_total, point.x, point.y);
+        }
+        break;
+    }
+#endif // GEOFENCE_ENABLED
+
     case MAVLINK_MSG_ID_PARAM_SET:
         {
             AP_Var                  *vp;

ArduPlane/Parameters.h

@@ -262,17 +262,19 @@ public:
     //
     AP_Int8     waypoint_mode;
     AP_Int8     command_total;
-    AP_Int8     command_index;
+    AP_Int8     command_index;
-    AP_Int8     waypoint_radius;
+    AP_Int8     waypoint_radius;
-    AP_Int8     loiter_radius;
+    AP_Int8     loiter_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
-
+#endif
-    // Fly-by-wire
+
-    //
+    // Fly-by-wire
+    //
     AP_Int8     flybywire_airspeed_min;
     AP_Int8     flybywire_airspeed_max;
     AP_Int16    FBWB_min_altitude;
@@ -386,17 +388,20 @@ public:
 
         /* XXX waypoint_mode missing here */
         command_total           (0,                         k_param_command_total,         PSTR("CMD_TOTAL")),
-        command_index           (0,                         k_param_command_index,         PSTR("CMD_INDEX")),
+        command_index           (0,                         k_param_command_index,         PSTR("CMD_INDEX")),
-        waypoint_radius         (WP_RADIUS_DEFAULT,         k_param_waypoint_radius,        PSTR("WP_RADIUS")),
+        waypoint_radius         (WP_RADIUS_DEFAULT,         k_param_waypoint_radius,        PSTR("WP_RADIUS")),
-        loiter_radius           (LOITER_RADIUS_DEFAULT,     k_param_loiter_radius,          PSTR("WP_LOITER_RAD")),
+        loiter_radius           (LOITER_RADIUS_DEFAULT,     k_param_loiter_radius,          PSTR("WP_LOITER_RAD")),
+
+#if GEOFENCE_ENABLED == ENABLED
         fence_action            (0,                         k_param_fence_action,           PSTR("FENCE_ACTION")),
         fence_total             (0,                         k_param_fence_total,            PSTR("FENCE_TOTAL")),
         fence_channel           (0,                         k_param_fence_channel,          PSTR("FENCE_CHANNEL")),
         fence_minalt            (0,                         k_param_fence_minalt,           PSTR("FENCE_MINALT")),
         fence_maxalt            (0,                         k_param_fence_maxalt,           PSTR("FENCE_MAXALT")),
-
+#endif
-        flybywire_airspeed_min  (AIRSPEED_FBW_MIN,          k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")),
+
-        flybywire_airspeed_max  (AIRSPEED_FBW_MAX,          k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")),
+        flybywire_airspeed_min  (AIRSPEED_FBW_MIN,          k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")),
+        flybywire_airspeed_max  (AIRSPEED_FBW_MAX,          k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")),
 
         throttle_min            (THROTTLE_MIN,              k_param_throttle_min,           PSTR("THR_MIN")),
         throttle_max            (THROTTLE_MAX,              k_param_throttle_max,           PSTR("THR_MAX")),

ArduPlane/geofence.pde

@@ -0,0 +1,236 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+  geo-fencing support
+  Andrew Tridgell, December 2011
+ */
+
+#if GEOFENCE_ENABLED == ENABLED
+
+/*
+  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 geofence_state {
+    uint8_t num_points;
+    bool boundary_uptodate;
+    bool fence_triggered;
+    /* point 0 is the return point */
+    Vector2f boundary[MAX_FENCEPOINTS];
+} *geofence_state;
+
+
+/*
+  fence boundaries fetch/store
+ */
+static Vector2f get_fence_point_with_index(unsigned i)
+{
+	uint32_t mem;
+    Vector2f ret;
+
+	if (i > (unsigned)g.fence_total) {
+        return Vector2f(0,0);
+    }
+
+    // read fence point
+    mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE);
+    eeprom_read_block(&ret.x, (void *)mem, sizeof(float));
+    mem += sizeof(float);
+    eeprom_read_block(&ret.y, (void *)mem, sizeof(float));
+
+	return ret;
+}
+
+// save a fence point
+static void set_fence_point_with_index(Vector2f &point, unsigned i)
+{
+	uint32_t mem;
+
+	if (i >= (unsigned)g.fence_total.get()) {
+        // not allowed
+        return;
+    }
+
+    mem = FENCE_START_BYTE + (i * FENCE_WP_SIZE);
+
+	eeprom_write_block(&point.x, (void *)mem, sizeof(float));
+	mem += 4;
+	eeprom_write_block(&point.y, (void *)mem, sizeof(float));
+
+    if (geofence_state != NULL) {
+        geofence_state->boundary_uptodate = false;
+    }
+}
+
+/*
+  allocate and fill the geofence state structure
+ */
+static void geofence_load(void)
+{
+    uint8_t i;
+
+    if (geofence_state == NULL) {
+        if (memcheck_available_memory() < 1024 + sizeof(struct geofence_state)) {
+            // too risky to enable as we could run out of stack
+            goto failed;
+        }
+        geofence_state = (struct geofence_state *)calloc(1, sizeof(struct geofence_state));
+        if (geofence_state == NULL) {
+            // not much we can do here except disable it
+            goto failed;
+        }
+    }
+
+    for (i=0; i<g.fence_total; i++) {
+        geofence_state->boundary[i] = get_fence_point_with_index(i);
+    }
+    geofence_state->num_points = i;
+
+    if (!Polygon_complete(&geofence_state->boundary[1], geofence_state->num_points-1)) {
+        // first point and last point must be the same
+        goto failed;
+    }
+    if (Polygon_outside(geofence_state->boundary[0], &geofence_state->boundary[1], geofence_state->num_points-1)) {
+        // return point needs to be inside the fence
+        goto failed;
+    }
+
+    geofence_state->boundary_uptodate = true;
+    geofence_state->fence_triggered = false;
+
+    gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence loaded"));
+    return;
+
+failed:
+    g.fence_action.set(FENCE_ACTION_NONE);
+    gcs_send_text_P(SEVERITY_HIGH,PSTR("geo-fence setup error"));
+}
+
+/*
+  return true if geo-fencing is enabled
+ */
+static bool geofence_enabled(void)
+{
+    if (g.fence_action == FENCE_ACTION_NONE ||
+        g.fence_channel == 0 ||
+        APM_RC.InputCh(g.fence_channel-1) < FENCE_ENABLE_PWM) {
+        // geo-fencing is disabled
+        if (geofence_state != NULL) {
+            // re-arm for when the channel trigger is switched on
+            geofence_state->fence_triggered = false;
+        }
+        return false;
+    }
+
+    if (!g_gps->fix) {
+        // we can't do much without a GPS fix
+        return false;
+    }
+
+    return true;
+}
+
+
+/*
+  check if we have breached the geo-fence
+ */
+static void geofence_check(void)
+{
+    if (!geofence_enabled()) {
+        return;
+    }
+
+    /* allocate the geo-fence state if need be */
+    if (geofence_state == NULL || !geofence_state->boundary_uptodate) {
+        geofence_load();
+        if (g.fence_action == FENCE_ACTION_NONE) {
+            // may have been disabled by load
+            return;
+        }
+    }
+
+    bool outside = false;
+
+    if (g.fence_maxalt > g.fence_minalt &&
+        ((g.fence_minalt != 0 && current_loc.alt < (g.fence_minalt*100) + home.alt) ||
+          (current_loc.alt > (g.fence_maxalt*100) + home.alt))) {
+        // we are too high or low
+        outside = true;
+    } else {
+        Vector2f location;
+        location.x = 1.0e-7 * current_loc.lat;
+        location.y = 1.0e-7 * current_loc.lng;
+        outside = Polygon_outside(location, &geofence_state->boundary[1], geofence_state->num_points-1);
+    }
+
+    if (!outside) {
+        if (geofence_state->fence_triggered) {
+            // we have moved back inside the fence
+            geofence_state->fence_triggered = false;
+        }
+        // we're inside, all is good with the world
+        return;
+    }
+
+    // we are outside the fence
+    if (geofence_state->fence_triggered) {
+        // 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;
+    gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence triggered"));
+
+    // see what action the user wants
+    switch (g.fence_action) {
+    case FENCE_ACTION_GUIDED:
+        // fly to the return point, with an altitude half way between
+        // min and max
+        if (g.fence_minalt >= g.fence_maxalt) {
+            // invalid min/max, use RTL_altitude
+            guided_WP.alt = home.alt + (g.RTL_altitude * 100);
+        } else {
+            guided_WP.alt = home.alt + 100*(g.fence_minalt + g.fence_maxalt)/2;
+        }
+        guided_WP.id = 0;
+        guided_WP.p1  = 0;
+        guided_WP.options = 0;
+        guided_WP.lat = geofence_state->boundary[0].x * 1.0e7;
+        guided_WP.lng = geofence_state->boundary[0].y * 1.0e7;
+        set_mode(GUIDED);
+        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
+ */
+static bool geofence_stickmixing(void) {
+    if (geofence_enabled() &&
+        geofence_state != NULL &&
+        geofence_state->fence_triggered &&
+        control_mode == GUIDED) {
+        // don't mix in user input
+        return false;
+    }
+    // normal mixing rules
+    return true;
+}
+
+#else // GEOFENCE_ENABLED
+
+static void geofence_check(void) { }
+static bool geofence_stickmixing(void) { return true; }
+
+#endif // GEOFENCE_ENABLED