diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 66ac2cc0d9..bc7eae340d 100644 --- a/ArduPlane/ArduPlane.pde +++ b/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(); } } diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 909c7a7541..df210d510b 100644 --- a/ArduPlane/Attitude.pde +++ b/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); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index cf6f2293f8..7320769a12 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/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; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 81b54d920a..9628316035 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -262,17 +262,19 @@ public: // AP_Int8 waypoint_mode; AP_Int8 command_total; - AP_Int8 command_index; - AP_Int8 waypoint_radius; - AP_Int8 loiter_radius; + AP_Int8 command_index; + AP_Int8 waypoint_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 - - // Fly-by-wire - // +#endif + + // 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")), - 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")), + command_index (0, k_param_command_index, PSTR("CMD_INDEX")), + 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")), + +#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")), - - 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")), +#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")), throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde new file mode 100644 index 0000000000..a1666d197d --- /dev/null +++ b/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; iboundary[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