From 238c25c05e218e2f47f66d88f532644d8da6e8dc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Dec 2011 18:08:58 +1100 Subject: [PATCH] geofence: added geo-fencing control parameters --- ArduPlane/Parameters.h | 15 +++++++++++++++ ArduPlane/config.h | 10 ++++++++++ ArduPlane/defines.h | 8 +++++++- 3 files changed, 32 insertions(+), 1 deletion(-) diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index c50decc2c8..81b54d920a 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -168,6 +168,11 @@ public: k_param_command_index, k_param_waypoint_radius, k_param_loiter_radius, + k_param_fence_action, + k_param_fence_total, + k_param_fence_channel, + k_param_fence_minalt, + k_param_fence_maxalt, // // 240: PID Controllers @@ -260,6 +265,11 @@ public: AP_Int8 command_index; AP_Int8 waypoint_radius; AP_Int8 loiter_radius; + 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 // @@ -379,6 +389,11 @@ public: 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")), + 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")), diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 2acdcb1e18..4bfcd5adf2 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -794,3 +794,13 @@ #ifndef MAVLINK_TELEMETRY_PORT_DELAY # define MAVLINK_TELEMETRY_PORT_DELAY 2000 #endif + +// use this to disable gen-fencing +#ifndef GEOFENCE_ENABLED +# define GEOFENCE_ENABLED ENABLED +#endif + +// pwm value on FENCE_CHANNEL to use to enable fenced mode +#ifndef FENCE_ENABLE_PWM +# define FENCE_ENABLE_PWM 1750 +#endif diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 49c6243d96..e8f9449694 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -213,8 +213,14 @@ enum gcs_severity { #define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP #define WP_SIZE 15 +// fence points are stored at the end of the EEPROM +#define MAX_FENCEPOINTS 20 +#define FENCE_WP_SIZE sizeof(Vector2f) +#define FENCE_START_BYTE (EEPROM_MAX_ADDR-(MAX_FENCEPOINTS*FENCE_WP_SIZE)) + +#define MAX_WAYPOINTS ((FENCE_START_BYTE - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe + #define ONBOARD_PARAM_NAME_LENGTH 15 -#define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe // convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1) #define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1)