diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index bc7eae340d..f5fc8244e7 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -819,7 +819,7 @@ static void update_GPS(void) current_loc.lat = g_gps->latitude; // Lat * 10**7 // see if we've breached the geo-fence - geofence_check(); + geofence_check(false); } } @@ -980,6 +980,8 @@ static void update_alt() current_loc.alt += g.altitude_mix * (read_barometer() + home.alt); #endif + geofence_check(true); + // Calculate new climb rate //if(medium_loopCounter == 0 && slow_loopCounter == 0) // add_altitude_data(millis() / 100, g_gps->altitude / 10); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 7320769a12..eb82e0d1a9 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -120,6 +120,14 @@ static NOINLINE void send_attitude(mavlink_channel_t chan) omega.z); } +#if GEOFENCE_ENABLED == ENABLED +static NOINLINE void send_fence_status(mavlink_channel_t chan) +{ + geofence_send_status(chan); +} +#endif + + static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) { #ifdef MAVLINK10 @@ -612,6 +620,13 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, send_statustext(chan); break; +#if GEOFENCE_ENABLED == ENABLED + case MSG_FENCE_STATUS: + CHECK_PAYLOAD_SIZE(FENCE_STATUS); + send_fence_status(chan); + break; +#endif + case MSG_RETRY_DEFERRED: break; // just here to prevent a warning } @@ -820,6 +835,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working send_message(MSG_NAV_CONTROLLER_OUTPUT); + send_message(MSG_FENCE_STATUS); } if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { @@ -1635,6 +1651,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // receive a fence point from GCS and store in EEPROM case MAVLINK_MSG_ID_FENCE_POINT: { mavlink_fence_point_t packet; + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; mavlink_msg_fence_point_decode(msg, &packet); if (g.fence_action != FENCE_ACTION_NONE) { send_text(SEVERITY_LOW,PSTR("fencing must be disabled")); @@ -1652,12 +1670,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // send a fence point to GCS case MAVLINK_MSG_ID_FENCE_FETCH_POINT: { mavlink_fence_fetch_point_t packet; + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; 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); + mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, g.fence_total, point.x, point.y); } break; } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index e8f9449694..56b4067434 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -122,6 +122,7 @@ enum ap_message { MSG_NEXT_WAYPOINT, MSG_NEXT_PARAM, MSG_STATUSTEXT, + MSG_FENCE_STATUS, MSG_RETRY_DEFERRED // this must be last }; diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde index be82d4697d..4185dbd113 100644 --- a/ArduPlane/geofence.pde +++ b/ArduPlane/geofence.pde @@ -19,6 +19,9 @@ static struct geofence_state { uint8_t num_points; bool boundary_uptodate; bool fence_triggered; + uint16_t breach_count; + uint8_t breach_type; + uint32_t breach_time; /* point 0 is the return point */ Vector2f boundary[MAX_FENCEPOINTS]; } *geofence_state; @@ -29,10 +32,10 @@ static struct geofence_state { */ static Vector2f get_fence_point_with_index(unsigned i) { - uint32_t mem; + uint32_t mem; Vector2f ret; - if (i > (unsigned)g.fence_total) { + if (i > (unsigned)g.fence_total) { return Vector2f(0,0); } @@ -42,24 +45,24 @@ static Vector2f get_fence_point_with_index(unsigned i) mem += sizeof(float); eeprom_read_block(&ret.y, (void *)mem, sizeof(float)); - return ret; + return ret; } // save a fence point static void set_fence_point_with_index(Vector2f &point, unsigned i) { - uint32_t mem; + uint32_t mem; - if (i >= (unsigned)g.fence_total.get()) { + 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)); + 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; @@ -135,10 +138,39 @@ static bool geofence_enabled(void) } +/* + return true if we have breached the geo-fence minimum altiude + */ +static bool geofence_check_minalt(void) +{ + if (g.fence_maxalt <= g.fence_minalt) { + return false; + } + if (g.fence_minalt == 0) { + return false; + } + return (current_loc.alt < (g.fence_minalt*100) + home.alt); +} + +/* + return true if we have breached the geo-fence maximum altiude + */ +static bool geofence_check_maxalt(void) +{ + if (g.fence_maxalt <= g.fence_minalt) { + return false; + } + if (g.fence_maxalt == 0) { + return false; + } + return (current_loc.alt > (g.fence_maxalt*100) + home.alt); +} + + /* check if we have breached the geo-fence */ -static void geofence_check(void) +static void geofence_check(bool altitude_check_only) { if (!geofence_enabled()) { return; @@ -147,30 +179,36 @@ static void geofence_check(void) /* 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) { + if (!geofence_enabled()) { // may have been disabled by load return; } } bool outside = false; + uint8_t breach_type = FENCE_BREACH_NONE; - 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 + if (geofence_check_minalt()) { outside = true; - } else { + breach_type = FENCE_BREACH_MINALT; + } else if (geofence_check_maxalt()) { + outside = true; + breach_type = FENCE_BREACH_MAXALT; + } else if (!altitude_check_only) { 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) { + breach_type = FENCE_BREACH_BOUNDARY; + } } if (!outside) { - if (geofence_state->fence_triggered) { + if (geofence_state->fence_triggered && !altitude_check_only) { // we have moved back inside the fence geofence_state->fence_triggered = false; + gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence OK")); } // we're inside, all is good with the world return; @@ -186,6 +224,9 @@ static void geofence_check(void) // 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; gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence triggered")); // see what action the user wants @@ -234,9 +275,24 @@ static bool geofence_stickmixing(void) { return true; } +/* + + */ +static void geofence_send_status(mavlink_channel_t chan) +{ + if (geofence_enabled() && geofence_state != NULL) { + mavlink_msg_fence_status_send(chan, + (int8_t)geofence_state->fence_triggered, + geofence_state->breach_count, + geofence_state->breach_type, + geofence_state->breach_time); + } +} + #else // GEOFENCE_ENABLED -static void geofence_check(void) { } +static void geofence_check(bool altitude_check_only) { } static bool geofence_stickmixing(void) { return true; } +static bool geofence_enabled(void) { return false; } #endif // GEOFENCE_ENABLED