Line data Source code
1 : #pragma once 2 : 3 : #include "AC_Fence_config.h" 4 : 5 : #if AP_FENCE_ENABLED 6 : 7 : #include <inttypes.h> 8 : #include <AP_Common/AP_Common.h> 9 : #include <AP_Common/ExpandingString.h> 10 : #include <AP_Param/AP_Param.h> 11 : #include <AP_Math/AP_Math.h> 12 : #include <AC_Fence/AC_PolyFence_loader.h> 13 : 14 : // bit masks for enabled fence types. Used for TYPE parameter 15 : #define AC_FENCE_TYPE_ALT_MAX 1 // high alt fence which usually initiates an RTL 16 : #define AC_FENCE_TYPE_CIRCLE 2 // circular horizontal fence (usually initiates an RTL) 17 : #define AC_FENCE_TYPE_POLYGON 4 // polygon horizontal fence 18 : #define AC_FENCE_TYPE_ALT_MIN 8 // low alt fence which usually initiates an RTL 19 : #define AC_FENCE_ARMING_FENCES (AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON) 20 : #define AC_FENCE_ALL_FENCES (AC_FENCE_ARMING_FENCES | AC_FENCE_TYPE_ALT_MIN) 21 : 22 : // give up distance 23 : #define AC_FENCE_GIVE_UP_DISTANCE 100.0f // distance outside the fence at which we should give up and just land. Note: this is not used by library directly but is intended to be used by the main code 24 : 25 : class AC_Fence 26 : { 27 : public: 28 : friend class AC_PolyFence_loader; 29 : 30 : // valid actions should a fence be breached 31 : enum class Action { 32 : REPORT_ONLY = 0, // report to GCS that boundary has been breached but take no further action 33 : RTL_AND_LAND = 1, // return to launch and, if that fails, land 34 : ALWAYS_LAND = 2, // always land 35 : SMART_RTL = 3, // smartRTL, if that fails, RTL, and if that still fails, land 36 : BRAKE = 4, // brake, if that fails, land 37 : SMART_RTL_OR_LAND = 5, // SmartRTL, if that fails, Land 38 : GUIDED = 6, // guided mode, with target waypoint as fence return point 39 : GUIDED_THROTTLE_PASS = 7, // guided mode, but pilot retains manual throttle control 40 : AUTOLAND_OR_RTL = 8, // fixed wing autoland,if enabled, or RTL 41 : }; 42 : 43 : enum class AutoEnable : uint8_t 44 : { 45 : ALWAYS_DISABLED = 0, 46 : ENABLE_ON_AUTO_TAKEOFF = 1, // enable on auto takeoff 47 : ENABLE_DISABLE_FLOOR_ONLY = 2, // enable on takeoff but disable floor on landing 48 : ONLY_WHEN_ARMED = 3 // enable on arming 49 : }; 50 : 51 : enum class MavlinkFenceActions : uint16_t 52 : { 53 : DISABLE_FENCE = 0, 54 : ENABLE_FENCE = 1, 55 : DISABLE_ALT_MIN_FENCE = 2 56 : }; 57 : 58 : AC_Fence(); 59 : 60 : /* Do not allow copies */ 61 : CLASS_NO_COPY(AC_Fence); 62 : 63 2086 : void init() { 64 2086 : _poly_loader.init(); 65 2086 : } 66 : 67 : // get singleton instance 68 16409821 : static AC_Fence *get_singleton() { return _singleton; } 69 : 70 : /// enable - allows fence to be enabled/disabled. 71 : /// returns a bitmask of fences that were changed 72 : uint8_t enable(bool value, uint8_t fence_types, bool update_auto_mask = true); 73 : 74 : /// enable_configured - allows configured fences to be enabled/disabled. 75 : /// returns a bitmask of fences that were changed 76 15 : uint8_t enable_configured(bool value) { return enable(value, _configured_fences, true); } 77 : 78 : /// auto_enabled - automaticaly enable/disable fence depending of flight status 79 1104227 : AutoEnable auto_enabled() const { return static_cast<AutoEnable>(_auto_enabled.get()); } 80 : 81 : /// enable_floor - allows fence floor to be enabled/disabled. Note this does not update the eeprom saved value 82 : void enable_floor(); 83 : 84 : /// disable_floor - allows fence floor to be enabled/disabled. Note this does not update the eeprom saved value 85 : void disable_floor(); 86 : 87 : /// auto_enable_fence_on_takeoff - auto enables the fence. Called after takeoff conditions met 88 : void auto_enable_fence_after_takeoff(); 89 : 90 : /// auto_enable_fences_on_arming - auto enables all applicable fences on arming 91 : void auto_enable_fence_on_arming(); 92 : 93 : /// auto_disable_fences_on_disarming - auto disables all applicable fences on disarming 94 : void auto_disable_fence_on_disarming(); 95 : 96 : uint8_t get_auto_disable_fences(void) const; 97 : 98 : /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. 99 : bool auto_enable_fence_floor(); 100 : 101 : /// enabled - returns whether fencing is enabled or not 102 19632462 : bool enabled() const { return _enabled_fences; } 103 : 104 : /// present - returns true if any of the provided types is present 105 : uint8_t present() const; 106 : 107 : /// get_enabled_fences - returns bitmask of enabled fences 108 : uint8_t get_enabled_fences() const; 109 : 110 : // should be called @10Hz to handle loading from eeprom 111 : void update(); 112 : 113 : /// pre_arm_check - returns true if all pre-takeoff checks have completed successfully 114 : bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const; 115 : 116 : // used by AP_Arming for pre-arm checks 117 : bool terrain_database_required() const; 118 : 119 : /// 120 : /// methods to check we are within the boundaries and recover 121 : /// 122 : 123 : /// check - returns the fence type that has been breached (if any) 124 : /// disabled_fences can be used to disable fences for certain conditions (e.g. landing) 125 : uint8_t check(bool disable_auto_fence = false); 126 : 127 : // returns true if the location is within fence (used to reject waypoints outside the fence) 128 : bool check_location_within_fence(const class Location& loc); 129 : 130 : /// get_breaches - returns bit mask of the fence types that have been breached 131 3484153 : uint8_t get_breaches() const { return _breached_fences; } 132 : 133 : /// get_breach_time - returns time the fence was breached 134 54500 : uint32_t get_breach_time() const { return _breach_time; } 135 : 136 : /// get_breach_count - returns number of times we have breached the fence 137 54500 : uint16_t get_breach_count() const { return _breach_count; } 138 : 139 : /// get_breaches - returns bitmask of the fence types that have had their margins breached 140 0 : uint8_t get_margin_breaches() const { return _breached_fence_margins; } 141 : 142 : /// get_margin_breach_time - returns time the fence margin was breached 143 0 : uint32_t get_margin_breach_time() const { return _margin_breach_time; } 144 : 145 : /// get_breach_distance - returns maximum distance in meters outside 146 : /// of the given fences. fence_type is a bitmask here. 147 : float get_breach_distance(uint8_t fence_type) const; 148 : 149 : /// get_breach_direction_NED - returns direction in meters outside/inside 150 : /// of the given fences. fence_check_pos is the absolute position when the check was made. 151 : // fence_type is a bitmask. 152 : bool get_breach_direction_NED(uint8_t fence_type, Vector3f& direction, Location& fence_check_pos) const; 153 : 154 : /// get_action - getter for user requested action on limit breach 155 159344 : Action get_action() const { return _action; } 156 : 157 : /// get_relative_safe_alt_max_m - returns maximum safe altitude in relative frame 158 0 : float get_relative_safe_alt_max_m() const { return _safe_relhome_alt_max_m ; } 159 : 160 : /// get_relative_safe_alt_min_m - returns the minimum safe altitude in relative frame 161 493 : float get_relative_safe_alt_min_m() const { return _safe_relhome_alt_min_m ; } 162 : 163 : /// get_safe_alt_max_m - returns maximum safe altitude in alt max frame (i.e. alt_max - margin) 164 161278 : float get_safe_alt_max_m() const { return _alt_max_m - _margin_m; } 165 : 166 : /// get_safe_alt_min_m - returns the minimum safe altitude in alt min frame (i.e. alt_min + margin) 167 85670 : float get_safe_alt_min_m() const { return _alt_min_m + _margin_m; } 168 : 169 : /// get_safe_alt_max_m_and_frame - returns maximum safe altitude and its frame via references 170 0 : void get_safe_alt_max_m_and_frame(float &alt, uint8_t &frame) const { 171 0 : alt = _alt_max_m - _margin_m; 172 0 : frame = (uint8_t)_alt_max_type.get(); 173 0 : } 174 : 175 : /// get_safe_alt_min_m_and_frame - returns minimum safe altitude and its frame via references 176 0 : void get_safe_alt_min_m_and_frame(float &alt, uint8_t &frame) const { 177 0 : alt = _alt_min_m + _margin_m; 178 0 : frame = (uint8_t)_alt_min_type.get(); 179 0 : } 180 : 181 : /// get_radius_m - returns the fence radius in meters 182 227931 : float get_radius_m() const { return _circle_radius_m.get(); } 183 : 184 : /// get_margin_ne_m - returns the horizontal fence margin in meters 185 : float get_margin_ne_m() const; 186 : 187 : /// get_return_rally - returns whether returning to fence return point or rally point 188 7 : uint8_t get_return_rally() const { return _ret_rally; } 189 : 190 : /// get_return_rally - returns whether returning to fence return point or rally point 191 7 : float get_return_altitude() const { return _ret_altitude; } 192 : 193 : /// get a user-friendly list of fences 194 : static void get_fence_names(uint8_t fences, ExpandingString& msg); 195 : 196 : // print a message about the fences to the GCS 197 : void print_fence_message(const char* msg, uint8_t fences) const; 198 : 199 : /// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds 200 : /// should be called whenever the pilot changes the flight mode 201 : /// has no effect if no breaches have occurred 202 : void manual_recovery_start(); 203 : 204 : // methods for mavlink SYS_STATUS message (send_sys_status) 205 : bool sys_status_present() const; 206 : bool sys_status_enabled() const; 207 : bool sys_status_failed() const; 208 : 209 : AC_PolyFence_loader &polyfence(); 210 : const AC_PolyFence_loader &polyfence() const; 211 : 212 : enum class OPTIONS { 213 : DISABLE_MODE_CHANGE = 1U << 0, 214 : INCLUSION_UNION = 1U << 1, 215 : NOTIFY_MARGIN_BREACH = 1U << 2, 216 : }; 217 3476447 : static bool option_enabled(OPTIONS opt, const AP_Int16 &options) { 218 3476447 : return (options.get() & int16_t(opt)) != 0; 219 : } 220 14836 : bool option_enabled(OPTIONS opt) const { 221 14836 : return option_enabled(opt, _options); 222 : } 223 : 224 : static const struct AP_Param::GroupInfo var_info[]; 225 : 226 : #if AP_SDCARD_STORAGE_ENABLED 227 681 : bool failed_sdcard_storage(void) const { 228 681 : return _poly_loader.failed_sdcard_storage(); 229 : } 230 : #endif 231 : 232 : // get altitude in alt max frame 233 165854 : bool get_alt_in_alt_max_frame_m(float &alt) const { return get_alt_in_frame_m(_alt_max_type, alt); } 234 : // get altitude in alt min frame 235 91279 : bool get_alt_in_alt_min_frame_m(float &alt) const { return get_alt_in_frame_m(_alt_min_type, alt); } 236 : // get alt max frame 237 1700 : Location::AltFrame get_alt_max_frame() const { return (Location::AltFrame)_alt_max_type.get(); } 238 : // get alt min frame 239 1713 : Location::AltFrame get_alt_min_frame() const { return (Location::AltFrame)_alt_min_type.get(); } 240 : 241 : private: 242 : static AC_Fence *_singleton; 243 : 244 : /// check_fence_alt_max - true if max alt fence has been newly breached 245 : bool check_fence_alt_max(); 246 : 247 : /// check_fence_alt_min - true if min alt fence has been newly breached 248 : bool check_fence_alt_min(); 249 : 250 : /// check_fence_polygon - true if polygon fence has been newly breached 251 : bool check_fence_polygon(); 252 : 253 : /// check_fence_circle - true if circle fence has been newly breached 254 : bool check_fence_circle(); 255 : 256 : /// update safe alt min 257 : void update_safe_alt_min(); 258 : 259 : /// record_breach - update breach bitmask, time and count 260 : void record_breach(uint8_t fence_type); 261 : 262 : /// clear_breach - update breach bitmask 263 : void clear_breach(uint8_t fence_type); 264 : 265 : /// record_margin_breach - update margin breach bitmask 266 : void record_margin_breach(uint8_t fence_type); 267 : 268 : /// clear_margin_breach - update margin breach bitmask 269 : void clear_margin_breach(uint8_t fence_type); 270 : 271 : /// retrieve the current NED position relative to home 272 : bool get_current_position_NED(Vector3f& currpos) const; 273 : 274 : // get altitude in the supplied frame 275 : bool get_alt_in_frame_m(Location::AltFrame alt_frame, float &alt) const; 276 : 277 : // additional checks for the different fence types: 278 : bool pre_arm_check_polygon(char *failure_msg, const uint8_t failure_msg_len) const; 279 : bool pre_arm_check_circle(char *failure_msg, const uint8_t failure_msg_len) const; 280 : bool pre_arm_check_alt(char *failure_msg, const uint8_t failure_msg_len) const; 281 : // fence floor is enabled 282 5368 : bool floor_enabled() const { return _enabled_fences & AC_FENCE_TYPE_ALT_MIN; } 283 : 284 : // parameters 285 : uint8_t _enabled_fences; // fences that are currently enabled/disabled 286 : bool _last_enabled; // value of enabled last time we checked 287 : AP_Int8 _enabled; // overall feature control 288 : AP_Int8 _auto_enabled; // top level flag for auto enabling fence 289 : uint8_t _last_auto_enabled; // value of auto_enabled last time we checked 290 : AP_Int8 _configured_fences; // bit mask holding which fences are enabled 291 : AP_Enum<Action> _action; // recovery action specified by user 292 : AP_Float _alt_max_m; // altitude upper limit in meters 293 : AP_Float _alt_min_m; // altitude lower limit in meters 294 : AP_Float _circle_radius_m; // circle fence radius in meters 295 : AP_Float _margin_m; // distance in meters that autopilot's should maintain from the fence to avoid a breach 296 : AP_Float _margin_ne_m; // distance in meters that autopilot's should maintain from the horizontal fence to avoid a breach 297 : AP_Int8 _total; // number of polygon points saved in eeprom 298 : AP_Int8 _ret_rally; // return to fence return point or rally point/home 299 : AP_Int16 _ret_altitude; // return to this altitude 300 : AP_Int16 _options; // options bitmask, see OPTIONS enum 301 : AP_Float _notify_freq; // margin notification frequency 302 : AP_Enum<Location::AltFrame> _alt_max_type; // altitude max frame type 303 : AP_Enum<Location::AltFrame> _alt_min_type; // altitude min frame type 304 : 305 : // backup fences 306 : float _alt_max_backup_m; // backup altitude upper limit in meters used to refire the breach if the vehicle continues to move further away 307 : float _alt_min_backup_m; // backup altitude lower limit in meters used to refire the breach if the vehicle continues to move further away 308 : float _circle_radius_backup_m; // backup circle fence radius in meters used to refire the breach if the vehicle continues to move further away 309 : 310 : // breach distances - negative means distance to fence 311 : float _alt_max_breach_distance_m; // distance above the altitude max 312 : float _alt_min_breach_distance_m; // distance below the altitude min 313 : float _circle_breach_distance_m; // distance beyond the circular fence 314 : float _polygon_breach_distance_m; // distance beyond the polygon fence 315 : Vector2f _polygon_nearest_point; // direction towards the polygon breach 316 : Vector2f _circle_breach_direction; // direction towards the circle breach 317 : Location _last_fence_check_loc; // position used in the last fence check 318 : bool _last_fence_check_loc_valid;// whether the position determined in the last fence check was valid 319 : 320 : // other internal variables 321 : float _home_distance_m; // distance from home in meters (provided by main code) 322 : float _fence_distance_m; // distance to the nearest fence 323 : float _safe_relhome_alt_max_m ; // calculated safe alt max 324 : float _safe_relhome_alt_min_m ; // calculated safe alt min 325 : 326 : // breach information 327 : uint8_t _breached_fences; // bitmask holding the fence types that were breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE) 328 : uint8_t _breached_fence_margins; // bitmask holding the fence types that have margin breaches (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE) 329 : uint32_t _breach_time; // time of last breach in milliseconds 330 : uint32_t _margin_breach_time; // time of last margin breach in milliseconds 331 : uint16_t _breach_count; // number of times we have breached the fence 332 : uint32_t _last_breach_notify_sent_ms; // last time we sent a message about newly-breaching the fences 333 : uint32_t _last_margin_breach_notify_sent_ms; // last time we sent a message about newly-breaching the fences 334 : 335 : uint32_t _manual_recovery_start_ms; // system time in milliseconds that pilot re-took manual control 336 : 337 : enum class MinAltState 338 : { 339 : DEFAULT = 0, 340 : MANUALLY_ENABLED, 341 : MANUALLY_DISABLED 342 : } _min_alt_state; 343 : 344 : 345 : AC_PolyFence_loader _poly_loader{_total, _options}; // polygon fence 346 : }; 347 : 348 : namespace AP { 349 : AC_Fence *fence(); 350 : }; 351 : 352 : #endif // AP_FENCE_ENABLED
