Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

geofence: main geo-fence code

Browse Source 
this implements the primary logic of geo-fencing
This commit is contained in:
Andrew Tridgell 2011-12-15 18:09:29 +11:00
parent 238c25c05e
commit 1b64185b18
5 changed files with 296 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.pde
View File

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

12
ArduPlane/Attitude.pde
View File

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

32
ArduPlane/GCS_Mavlink.pde
View File

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

29
ArduPlane/Parameters.h
View File

@ -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")),

236
ArduPlane/geofence.pde Normal file
View File

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