mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-21 16:18:29 -04:00
87b66b4b49
AC_Fence: Add fence floor breach checks and calculations AC_Fence: Add event logging to enable/disable of fence floor AC_Fence: Adjust sys_status reporting to look at total fence count AC_Fence: Make retrieving of the return point accessible AC_Fence: Check whether fence is enabled or autoenable is set for arming checks Checks whether the fence is currently enabled OR if the fence is intended to be enabled automatically. These checks are used to find out enabled fences, or prearm checks
223 lines
10 KiB
C++
223 lines
10 KiB
C++
#pragma once
|
|
|
|
#include <inttypes.h>
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
#include <AC_Fence/AC_PolyFence_loader.h>
|
|
#include <AP_Common/Location.h>
|
|
|
|
// bit masks for enabled fence types. Used for TYPE parameter
|
|
#define AC_FENCE_TYPE_ALT_MAX 1 // high alt fence which usually initiates an RTL
|
|
#define AC_FENCE_TYPE_CIRCLE 2 // circular horizontal fence (usually initiates an RTL)
|
|
#define AC_FENCE_TYPE_POLYGON 4 // polygon horizontal fence
|
|
#define AC_FENCE_TYPE_ALT_MIN 8 // low alt fence which usually initiates an RTL
|
|
|
|
// valid actions should a fence be breached
|
|
#define AC_FENCE_ACTION_REPORT_ONLY 0 // report to GCS that boundary has been breached but take no further action
|
|
#define AC_FENCE_ACTION_RTL_AND_LAND 1 // return to launch and, if that fails, land
|
|
#define AC_FENCE_ACTION_ALWAYS_LAND 2 // always land
|
|
#define AC_FENCE_ACTION_SMART_RTL 3 // smartRTL, if that fails, RTL, it that still fails, land
|
|
#define AC_FENCE_ACTION_BRAKE 4 // brake, if that fails, land
|
|
#define AC_FENCE_ACTION_SMART_RTL_OR_LAND 5 // SmartRTL, if that fails, Land
|
|
#define AC_FENCE_ACTION_GUIDED 6 // guided mode, with target waypoint as fence return point
|
|
#define AC_FENCE_ACTION_GUIDED_THROTTLE_PASS 7 // guided mode, but pilot retains manual throttle control
|
|
|
|
// default boundaries
|
|
#define AC_FENCE_ALT_MAX_DEFAULT 100.0f // default max altitude is 100m
|
|
#define AC_FENCE_ALT_MIN_DEFAULT -10.0f // default maximum depth in meters
|
|
#define AC_FENCE_CIRCLE_RADIUS_DEFAULT 300.0f // default circular fence radius is 300m
|
|
#define AC_FENCE_ALT_MAX_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further up
|
|
#define AC_FENCE_ALT_MIN_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further down
|
|
#define AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE 20.0f // after fence is broken we recreate the fence 20m further out
|
|
#define AC_FENCE_MARGIN_DEFAULT 2.0f // default distance in meters that autopilot's should maintain from the fence to avoid a breach
|
|
|
|
// give up distance
|
|
#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
|
|
#define AC_FENCE_MANUAL_RECOVERY_TIME_MIN 10000 // pilot has 10seconds to recover during which time the autopilot will not attempt to re-take control
|
|
|
|
class AC_Fence
|
|
{
|
|
public:
|
|
|
|
enum class AutoEnable
|
|
{
|
|
ALWAYS_DISABLED = 0,
|
|
ALWAYS_ENABLED = 1,
|
|
ENABLE_DISABLE_FLOOR_ONLY = 2,
|
|
ONLY_WHEN_ARMED = 3
|
|
};
|
|
|
|
AC_Fence();
|
|
|
|
/* Do not allow copies */
|
|
AC_Fence(const AC_Fence &other) = delete;
|
|
AC_Fence &operator=(const AC_Fence&) = delete;
|
|
|
|
void init() {
|
|
_poly_loader.init();
|
|
}
|
|
|
|
// get singleton instance
|
|
static AC_Fence *get_singleton() { return _singleton; }
|
|
|
|
/// enable - allows fence to be enabled/disabled.
|
|
void enable(bool value);
|
|
|
|
/// auto_enabled -
|
|
AutoEnable auto_enabled() { return static_cast<AutoEnable>(_auto_enabled.get()); }
|
|
|
|
/// enable_floor - allows fence floor to be enabled/disabled. Note this does not update the eeprom saved value
|
|
void enable_floor();
|
|
|
|
/// disable_floor - allows fence floor to be enabled/disabled. Note this does not update the eeprom saved value
|
|
void disable_floor();
|
|
|
|
/// enabled - returns true if fence is enabled
|
|
bool enabled() const { return _enabled; }
|
|
|
|
/// present - returns true if fence is present
|
|
bool present() const;
|
|
|
|
/// get_enabled_fences - returns bitmask of enabled fences
|
|
uint8_t get_enabled_fences() const;
|
|
|
|
// should be called @10Hz to handle loading from eeprom
|
|
void update() {
|
|
_poly_loader.update();
|
|
}
|
|
|
|
/// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
|
|
bool pre_arm_check(const char* &fail_msg) const;
|
|
|
|
///
|
|
/// methods to check we are within the boundaries and recover
|
|
///
|
|
|
|
/// check - returns the fence type that has been breached (if any)
|
|
uint8_t check();
|
|
|
|
// returns true if the destination is within fence (used to reject waypoints outside the fence)
|
|
bool check_destination_within_fence(const Location& loc);
|
|
|
|
/// get_breaches - returns bit mask of the fence types that have been breached
|
|
uint8_t get_breaches() const { return _breached_fences; }
|
|
|
|
/// get_breach_time - returns time the fence was breached
|
|
uint32_t get_breach_time() const { return _breach_time; }
|
|
|
|
/// get_breach_count - returns number of times we have breached the fence
|
|
uint16_t get_breach_count() const { return _breach_count; }
|
|
|
|
/// get_breach_distance - returns maximum distance in meters outside
|
|
/// of the given fences. fence_type is a bitmask here.
|
|
float get_breach_distance(uint8_t fence_type) const;
|
|
|
|
/// get_action - getter for user requested action on limit breach
|
|
uint8_t get_action() const { return _action.get(); }
|
|
|
|
/// get_safe_alt - returns maximum safe altitude (i.e. alt_max - margin)
|
|
float get_safe_alt_max() const { return _alt_max - _margin; }
|
|
|
|
/// get_safe_alt_min - returns the minimum safe altitude (i.e. alt_min - margin)
|
|
float get_safe_alt_min() const { return _alt_min + _margin; }
|
|
|
|
/// get_radius - returns the fence radius in meters
|
|
float get_radius() const { return _circle_radius.get(); }
|
|
|
|
/// get_margin - returns the fence margin in meters
|
|
float get_margin() const { return _margin.get(); }
|
|
|
|
/// get_return_rally - returns whether returning to fence return point or rally point
|
|
uint8_t get_return_rally() const { return _ret_rally; }
|
|
|
|
/// get_return_rally - returns whether returning to fence return point or rally point
|
|
float get_return_altitude() const { return _ret_altitude; }
|
|
|
|
/// manual_recovery_start - caller indicates that pilot is re-taking manual control so fence should be disabled for 10 seconds
|
|
/// should be called whenever the pilot changes the flight mode
|
|
/// has no effect if no breaches have occurred
|
|
void manual_recovery_start();
|
|
|
|
// methods for mavlink SYS_STATUS message (send_sys_status)
|
|
bool sys_status_present() const;
|
|
bool sys_status_enabled() const;
|
|
bool sys_status_failed() const;
|
|
|
|
AC_PolyFence_loader &polyfence();
|
|
const AC_PolyFence_loader &polyfence() const;
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
private:
|
|
static AC_Fence *_singleton;
|
|
|
|
/// check_fence_alt_max - true if max alt fence has been newly breached
|
|
bool check_fence_alt_max();
|
|
|
|
/// check_fence_alt_min - true if min alt fence has been newly breached
|
|
bool check_fence_alt_min();
|
|
|
|
/// check_fence_polygon - true if polygon fence has been newly breached
|
|
bool check_fence_polygon();
|
|
|
|
/// check_fence_circle - true if circle fence has been newly breached
|
|
bool check_fence_circle();
|
|
|
|
/// record_breach - update breach bitmask, time and count
|
|
void record_breach(uint8_t fence_type);
|
|
|
|
/// clear_breach - update breach bitmask, time and count
|
|
void clear_breach(uint8_t fence_type);
|
|
|
|
// additional checks for the different fence types:
|
|
bool pre_arm_check_polygon(const char* &fail_msg) const;
|
|
bool pre_arm_check_circle(const char* &fail_msg) const;
|
|
bool pre_arm_check_alt(const char* &fail_msg) const;
|
|
|
|
// parameters
|
|
AP_Int8 _enabled; // fence enable/disable control
|
|
AP_Int8 _auto_enabled; // top level flag for auto enabling fence
|
|
AP_Int8 _enabled_fences; // bit mask holding which fences are enabled
|
|
AP_Int8 _action; // recovery action specified by user
|
|
AP_Float _alt_max; // altitude upper limit in meters
|
|
AP_Float _alt_min; // altitude lower limit in meters
|
|
AP_Float _circle_radius; // circle fence radius in meters
|
|
AP_Float _margin; // distance in meters that autopilot's should maintain from the fence to avoid a breach
|
|
AP_Int8 _total; // number of polygon points saved in eeprom
|
|
AP_Int8 _ret_rally; // return to fence return point or rally point/home
|
|
AP_Float _ret_altitude; // return to this altitude
|
|
|
|
// backup fences
|
|
float _alt_max_backup; // backup altitude upper limit in meters used to refire the breach if the vehicle continues to move further away
|
|
float _alt_min_backup; // backup altitude lower limit in meters used to refire the breach if the vehicle continues to move further away
|
|
float _circle_radius_backup; // backup circle fence radius in meters used to refire the breach if the vehicle continues to move further away
|
|
|
|
// breach distances
|
|
float _alt_max_breach_distance; // distance above the altitude max
|
|
float _alt_min_breach_distance; // distance below the altitude min
|
|
float _circle_breach_distance; // distance beyond the circular fence
|
|
|
|
// other internal variables
|
|
bool _floor_enabled; // fence floor is enabled
|
|
float _home_distance; // distance from home in meters (provided by main code)
|
|
float _curr_alt;
|
|
|
|
|
|
// breach information
|
|
uint8_t _breached_fences; // bitmask holding the fence type that was breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE)
|
|
uint32_t _breach_time; // time of last breach in milliseconds
|
|
uint16_t _breach_count; // number of times we have breached the fence
|
|
uint32_t _last_breach_notify_sent_ms; // last time we sent a message about newly-breaching the fences
|
|
|
|
uint32_t _manual_recovery_start_ms; // system time in milliseconds that pilot re-took manual control
|
|
|
|
|
|
AC_PolyFence_loader _poly_loader{_total}; // polygon fence
|
|
};
|
|
|
|
namespace AP {
|
|
AC_Fence *fence();
|
|
};
|