Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

geofence: added fence status reporting and faster altitude check

This commit is contained in:
Andrew Tridgell 2011-12-16 12:41:11 +11:00
parent 2e2b320560
commit b3327c64de
4 changed files with 98 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.pde
View File

@ -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);

22
ArduPlane/GCS_Mavlink.pde
View File

@ -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;
}

1
ArduPlane/defines.h
View File

@ -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
};

90
ArduPlane/geofence.pde
View File

@ -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