From b01406e6714482223616661885f31bf62d58ca6a Mon Sep 17 00:00:00 2001 From: Daniel Frenzel Date: Wed, 3 Feb 2016 13:20:16 +0100 Subject: [PATCH] AP_Limits: Remove unused module --- libraries/AP_Limits/AP_Limit_Altitude.cpp | 81 --------- libraries/AP_Limits/AP_Limit_Altitude.h | 37 ---- libraries/AP_Limits/AP_Limit_GPSLock.cpp | 48 ----- libraries/AP_Limits/AP_Limit_GPSLock.h | 30 ---- libraries/AP_Limits/AP_Limit_Geofence.cpp | 194 -------------------- libraries/AP_Limits/AP_Limit_Geofence.h | 61 ------- libraries/AP_Limits/AP_Limit_Module.cpp | 75 -------- libraries/AP_Limits/AP_Limit_Module.h | 71 -------- libraries/AP_Limits/AP_Limits.cpp | 204 ---------------------- libraries/AP_Limits/AP_Limits.h | 122 ------------- 10 files changed, 923 deletions(-) delete mode 100644 libraries/AP_Limits/AP_Limit_Altitude.cpp delete mode 100644 libraries/AP_Limits/AP_Limit_Altitude.h delete mode 100644 libraries/AP_Limits/AP_Limit_GPSLock.cpp delete mode 100644 libraries/AP_Limits/AP_Limit_GPSLock.h delete mode 100644 libraries/AP_Limits/AP_Limit_Geofence.cpp delete mode 100644 libraries/AP_Limits/AP_Limit_Geofence.h delete mode 100644 libraries/AP_Limits/AP_Limit_Module.cpp delete mode 100644 libraries/AP_Limits/AP_Limit_Module.h delete mode 100644 libraries/AP_Limits/AP_Limits.cpp delete mode 100644 libraries/AP_Limits/AP_Limits.h diff --git a/libraries/AP_Limits/AP_Limit_Altitude.cpp b/libraries/AP_Limits/AP_Limit_Altitude.cpp deleted file mode 100644 index d2ef1f13fa..0000000000 --- a/libraries/AP_Limits/AP_Limit_Altitude.cpp +++ /dev/null @@ -1,81 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - - -/// @file limits.cpp -/// @brief Imposes limits on location (geofence), altitude and other parameters -/// Each limit breach will trigger an action or set of actions to recover. -// Adapted from geofence. -/// @author Andrew Tridgell -/// Andreas Antonopoulos - -#include "AP_Limit_Altitude.h" - -const AP_Param::GroupInfo AP_Limit_Altitude::var_info[] = { - // @Param: ALT_ON - // @DisplayName: Enable altitude - // @Description: Setting this to Enabled(1) will enable the altitude. Setting this to Disabled(0) will disable the altitude - // @Values: 0:Disabled,1:Enabled - // @User: Standard - AP_GROUPINFO("ALT_ON", 0, AP_Limit_Altitude, _enabled, 0), - - // @Param: ALT_REQ - // @DisplayName: Require altitude - // @Description: Setting this to Enabled(1) will make being inside the altitude a required check before arming the vehicle. - // @Values: 0:Disabled,1:Enabled - // @User: Standard - AP_GROUPINFO("ALT_REQ", 1, AP_Limit_Altitude, _required, 0), - - // @Param: ALT_MIN - // @DisplayName: Minimum Altitude - // @Description: Minimum Altitude. Zero to disable. Sets a "floor" that your vehicle will try to stay above. IF the vehicle is crossing the threshold at speed, it will take a while to recover, so give yourself enough room. Caution: minimum altitude limits can cause unexpected behaviour, such as inability to land, or sudden takeoff. Read the wiki instructions before setting. - // @Units: Meters - // @Range: 0 250000 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("ALT_MIN", 2, AP_Limit_Altitude, _min_alt, 0), - - // @Param: ALT_MAX - // @DisplayName: Maximum Altitude - // @Description: Maximum Altitude. Zero to disable. Sets a "ceiling" that your vehicle will try to stay below. IF the vehicle is crossing the threshold at speed, it will take a while to recover. - // @Units: Meters - // @Range: 0 250000 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("ALT_MAX", 3, AP_Limit_Altitude, _max_alt, 0), - AP_GROUPEND -}; - -AP_Limit_Altitude::AP_Limit_Altitude(const struct Location *current_loc) : - AP_Limit_Module(AP_LIMITS_ALTITUDE), // enabled and required - _current_loc(current_loc) -{ - AP_Param::setup_object_defaults(this, var_info); - //_current_loc = current_loc; -} - -bool AP_Limit_Altitude::triggered() -{ - _triggered = false; // reset trigger before checking - - // _max_alt is zero if disabled - // convert _max_alt to centimeters to compare to actual altitude - if (_max_alt > 0 && _current_loc->alt > _max_alt*100 ) { - _triggered = true; - } - - // _min_alt is zero if disabled - // convert _min_alt to centimeters to compare to actual altitude - if (_min_alt > 0 && _current_loc->alt < _min_alt*100 ) { - _triggered = true; - } - return _triggered; -} - -AP_Int32 AP_Limit_Altitude::max_alt() { - return _max_alt; -} - -AP_Int32 AP_Limit_Altitude::min_alt() { - return _min_alt; -} - diff --git a/libraries/AP_Limits/AP_Limit_Altitude.h b/libraries/AP_Limits/AP_Limit_Altitude.h deleted file mode 100644 index ef2decc1c9..0000000000 --- a/libraries/AP_Limits/AP_Limit_Altitude.h +++ /dev/null @@ -1,37 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/// @file limits.h -/// @brief Imposes limits on location (geofence), altitude and other parameters. -/// Each limit breach will trigger an action or set of actions to recover. Adapted from geofence. -/// @author Andrew Tridgell -/// Andreas Antonopoulos - -#ifndef __AP_LIMIT_ALTITUDE_H__ -#define __AP_LIMIT_ALTITUDE_H__ - -#include "AP_Limits.h" -#include "AP_Limit_Module.h" -#include - -class AP_Limit_Altitude : public AP_Limit_Module { - -public: - AP_Limit_Altitude(const struct Location *current_loc); - - AP_Int32 min_alt(); - AP_Int32 max_alt(); - - bool init(); - bool triggered(); - - static const struct AP_Param::GroupInfo var_info[]; - -protected: - const struct Location * _current_loc; - AP_Int32 _min_alt; - AP_Int32 _max_alt; - - -}; - -#endif // __AP_LIMIT_ALTITUDE_H__ diff --git a/libraries/AP_Limits/AP_Limit_GPSLock.cpp b/libraries/AP_Limits/AP_Limit_GPSLock.cpp deleted file mode 100644 index 8f209803e6..0000000000 --- a/libraries/AP_Limits/AP_Limit_GPSLock.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - - -/// @file limits.cpp -/// @brief Imposes limits on location (geofence), altitude and other parameters. -/// Each limit breach will trigger an action or set of actions to recover. -/// Adapted from geofence. -/// @author Andrew Tridgell -/// Andreas Antonopoulos - -#include "AP_Limit_GPSLock.h" - -const AP_Param::GroupInfo AP_Limit_GPSLock::var_info[] = { - // @Param: GPSLCK_ON - // @DisplayName: Enable gpslock - // @Description: Setting this to Enabled(1) will enable the gpslock. Setting this to Disabled(0) will disable the gpslock - // @Values: 0:Disabled,1:Enabled - // @User: Standard - AP_GROUPINFO("GPSLCK_ON", 0, AP_Limit_GPSLock, _enabled, 0), - - // @Param: GPSLCK_REQ - // @DisplayName: Require gpslock - // @Description: Setting this to Enabled(1) will make being inside the gpslock a required check before arming the vehicle. - // @Values: 0:Disabled,1:Enabled - // @User: Standard - AP_GROUPINFO("GPSLCK_REQ", 1, AP_Limit_GPSLock, _required, 0), - AP_GROUPEND - -}; - -AP_Limit_GPSLock::AP_Limit_GPSLock(GPS *&gps) : - AP_Limit_Module(AP_LIMITS_GPSLOCK), // enabled and required - _gps(gps) -{ - AP_Param::setup_object_defaults(this, var_info); -} - - -bool AP_Limit_GPSLock::triggered() { - _triggered = false; // reset trigger before checking - - if (!_gps || !_gps->fix) { - _triggered = true; - } - - return _triggered; -} - diff --git a/libraries/AP_Limits/AP_Limit_GPSLock.h b/libraries/AP_Limits/AP_Limit_GPSLock.h deleted file mode 100644 index db3c4677ad..0000000000 --- a/libraries/AP_Limits/AP_Limit_GPSLock.h +++ /dev/null @@ -1,30 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/// @file limits.h -/// @brief Imposes limits on location (geofence), altitude and other parameters. -/// Each limit breach will trigger an action or set of actions to recover. Adapted from geofence. -/// @author Andrew Tridgell -/// Andreas Antonopoulos - -#ifndef __AP_LIMIT_GPSLOCK_H__ -#define __AP_LIMIT_GPSLOCK_H__ - -#include "AP_Limits.h" -#include "AP_Limit_Module.h" - -#include - -class AP_Limit_GPSLock : public AP_Limit_Module { - -public: - AP_Limit_GPSLock(GPS *&gps); - bool init(); - bool triggered(); - - static const struct AP_Param::GroupInfo var_info[]; - -protected: - GPS *& _gps; -}; - -#endif // __AP_LIMIT_GPSLOCK_H__ diff --git a/libraries/AP_Limits/AP_Limit_Geofence.cpp b/libraries/AP_Limits/AP_Limit_Geofence.cpp deleted file mode 100644 index 2849c7ba33..0000000000 --- a/libraries/AP_Limits/AP_Limit_Geofence.cpp +++ /dev/null @@ -1,194 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/// @file limits.cpp -/// @brief Imposes limits on location (geofence), altitude and other parameters. -/// Each limit breach will trigger an action or set of actions to recover. -/// Adapted from geofence. -/// @author Andrew Tridgell -/// Andreas Antonopoulos - -#include "AP_Limit_Geofence.h" -#include - -extern const AP_HAL::HAL& hal; - -const AP_Param::GroupInfo AP_Limit_Geofence::var_info[] = { - // @Param: FNC_ON - // @DisplayName: Enable Geofence - // @Description: Setting this to Enabled(1) will enable the geofence. Setting this to Disabled(0) will disable the geofence - // @Values: 0:Disabled,1:Enabled - // @User: Standard - AP_GROUPINFO("FNC_ON", 0, AP_Limit_Geofence, _enabled, 0), - - // @Param: FNC_REQ - // @DisplayName: Require Geofence - // @Description: Setting this to Enabled(1) will make being inside the geofence a required check before arming the vehicle. - // @Values: 0:Disabled,1:Enabled - // @User: Standard - AP_GROUPINFO("FNC_REQ", 1, AP_Limit_Geofence, _required, 0), - - // @Param: FNC_SMPL - // @DisplayName: Require Geofence - // @Description: "Simple" geofence (enabled - 1) is based on a radius from the home position, "Complex" (disabled - 0) define a complex fence by lat/long positions - // @Values: 0:Disabled,1:Enabled - // @User: Standard - AP_GROUPINFO("FNC_SMPL", 2, AP_Limit_Geofence, _simple, 0), - - // @Param: FNC_RAD - // @DisplayName: Require Geofence - // @Description: Radius of fenced area in meters. A value of 20 creates a 20-meter radius circle (40-meter diameter) from the home point. - // @Units: Meters - // @Range: 0 32767 - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("FNC_RAD", 3, AP_Limit_Geofence, _radius, 0), - - // @Param: FNC_TOT - // @DisplayName: Total number of geofence points - // @Description: Total number of geofence points. This parameter should not be updated manually - // @Range: 0 6 - // @Increment: 1 - AP_GROUPINFO("FNC_TOT", 4, AP_Limit_Geofence, _fence_total, 0), - AP_GROUPEND - - -}; - -// storage object -StorageAccess AP_Limit_Geofence::_storage(StorageManager::StorageFence); - -AP_Limit_Geofence::AP_Limit_Geofence(uint16_t efs, uint8_t f_wp_s, - GPS *&gps, const struct Location *h_loc, - const struct Location *c_loc) : - AP_Limit_Module(AP_LIMITS_GEOFENCE), - _gps(gps), - _current_loc(c_loc), - _home(h_loc), - _fence_wp_size(f_wp_s), - _boundary_uptodate(false) -{ - AP_Param::setup_object_defaults(this, var_info); - update_boundary(); -} - - - - -bool AP_Limit_Geofence::triggered() { - - // reset trigger before checking - _triggered = false; - - // never trigger while disabled - if (!_enabled) return false; - - // if Geofence is required and we don't know where we are, trigger. - if (_required && (!_gps || !_gps->fix || !_home || !_current_loc)) { - - // TRIGGER - _triggered = true; - } - - uint32_t distance; - - // simple mode, pointers to current and home exist. - if (_simple && _current_loc && _home) { - distance = (uint32_t) get_distance(*_current_loc, *_home); - if (distance > 0 && distance > (uint16_t) _radius) { - - // TRIGGER - _triggered = true; - } - } - else { - - // COMPLEX GEOFENCE mode - - // check boundary and update if necessary - if (!_boundary_uptodate) { - update_boundary(); - } - - // if boundary is correct, and current_loc exists check if we - // are inside the fence. - if (boundary_correct() && _current_loc) { - Vector2l location; - location.x = _current_loc->lat; - location.y = _current_loc->lng; - // trigger if outside - if (Polygon_outside(location, &_boundary[1], _fence_total-1)) { - // TRIGGER - _triggered = true; - } - } else { - // boundary incorrect - // If geofence is required and our boundary fence is incorrect, - // we trigger. - if (_required) { - // TRIGGER - _triggered = true; - } - } - } - return _triggered; -} - - - -AP_Int8 AP_Limit_Geofence::fence_total() { - return _fence_total; -} - -// save a fence point -void AP_Limit_Geofence::set_fence_point_with_index(Vector2l &point, uint8_t i) -{ - if (i >= (unsigned)fence_total()) { - // not allowed - return; - } - - _storage.write_uint32(i * 8, point.x); - _storage.write_uint32(i * 8+4, point.y); - - _boundary_uptodate = false; -} - -/* - * fence boundaries fetch/store - */ -Vector2l AP_Limit_Geofence::get_fence_point_with_index(uint8_t i) -{ - Vector2l ret; - - if (i > (unsigned) fence_total()) { - return Vector2l(0,0); - } - - // read fence point - ret.x = _storage.read_uint32(i * 8); - ret.y = _storage.read_uint32(i * 8+4); - - return ret; -} - -void AP_Limit_Geofence::update_boundary() { - if (!_simple && _fence_total > 0) { - - for (uint8_t i = 0; i < (uint8_t) _fence_total; i++) { - _boundary[i] = get_fence_point_with_index(i); - } - - _boundary_uptodate = true; - - } -} - -bool AP_Limit_Geofence::boundary_correct() { - - if (Polygon_complete(&_boundary[1], _fence_total - 1) && - !Polygon_outside(_boundary[0], &_boundary[1], _fence_total - 1)) { - return true; - } else return false; -} - - diff --git a/libraries/AP_Limits/AP_Limit_Geofence.h b/libraries/AP_Limits/AP_Limit_Geofence.h deleted file mode 100644 index da289e0c44..0000000000 --- a/libraries/AP_Limits/AP_Limit_Geofence.h +++ /dev/null @@ -1,61 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/// @file limits.h -/// @brief Imposes limits on location (geofence), altitude and other parameters. -/// Each limit breach will trigger an action or set of actions to recover. Adapted from geofence. -/// @author Andrew Tridgell -/// Andreas Antonopoulos - -#ifndef __AP_LIMIT_GEOFENCE_H__ -#define __AP_LIMIT_GEOFENCE_H__ - -#include "AP_Limits.h" -#include "AP_Limit_Module.h" -#include -#include -#include -#include - -#define MAX_FENCEPOINTS 6 - -class AP_Limit_Geofence : public AP_Limit_Module { - -public: - AP_Limit_Geofence(uint16_t eeprom_fence_start, uint8_t fpsize, uint8_t max_fp, GPS *&gps, const struct Location *home_loc, const struct Location *current_loc); - bool init(); - bool triggered(); - - AP_Int8 fence_total(); - void set_fence_point_with_index(Vector2l &point, uint8_t i); - Vector2l get_fence_point_with_index(uint8_t i); - void update_boundary(); - bool boundary_correct(); - - - - static const struct AP_Param::GroupInfo var_info[]; - -protected: - - // pointers to gps, current location and home - GPS *& _gps; - const struct Location * _current_loc; - const struct Location * _home; - - - // Simple mode, just radius - AP_Int8 _simple; // 1 = simple, 0 = complex - AP_Int16 _radius; // in meters, for simple mode - - // Complex mode, defined fence points - AP_Int8 _fence_total; - AP_Int8 _num_points; - -private: - static StorageAccess _storage; - bool _boundary_uptodate; - Vector2l _boundary[MAX_FENCEPOINTS]; // complex mode fence - -}; - -#endif // __AP_LIMIT_GEOFENCE_H__ diff --git a/libraries/AP_Limits/AP_Limit_Module.cpp b/libraries/AP_Limits/AP_Limit_Module.cpp deleted file mode 100644 index b9a28d0b9f..0000000000 --- a/libraries/AP_Limits/AP_Limit_Module.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - - -/// @brief Imposes limits on location (geofence), altitude and other parameters -/// Each breach will trigger an action or set of actions to recover. -/// Adapted from geofence. -/// @author Andrew Tridgell -/// Andreas Antonopoulos - - -#include "AP_Limit_Module.h" - -extern const char *get_module_name(enum moduleid i) { - - switch (i) { - case AP_LIMITS_GPSLOCK: - return "GPSLock Limit"; - break; - case AP_LIMITS_GEOFENCE: - return "Geofence Limit"; - break; - case AP_LIMITS_ALTITUDE: - return "Altitude Limit"; - break; - default: - return "ERROR"; - break; - } -} - -AP_Limit_Module::AP_Limit_Module(enum moduleid i) { - _id = i; - _next = NULL; -} - -bool AP_Limit_Module::init() { - - _triggered = false; - return true; -}; - -moduleid AP_Limit_Module::get_module_id() { - return(_id); -} - -bool AP_Limit_Module::enabled() { - return(_enabled); -} - -bool AP_Limit_Module::required() { - return(_required); -} - -AP_Limit_Module *AP_Limit_Module::next() { - return(_next); -} - -void AP_Limit_Module::link(AP_Limit_Module *m) { - _next = m; -} - -bool AP_Limit_Module::triggered() { - return(_triggered); -} - -void AP_Limit_Module::action() { - // do nothing -} - - - - - - - diff --git a/libraries/AP_Limits/AP_Limit_Module.h b/libraries/AP_Limits/AP_Limit_Module.h deleted file mode 100644 index 1ef11933eb..0000000000 --- a/libraries/AP_Limits/AP_Limit_Module.h +++ /dev/null @@ -1,71 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - - -/// @brief Imposes limits on location (geofence), altitude and other parameters -/// Each breach will trigger an action or set of actions to recover. -// Adapted from geofence. -/// @author Andrew Tridgell -/// Andreas Antonopoulos - -#ifndef __AP_LIMIT_MODULE_H__ -#define __AP_LIMIT_MODULE_H__ - -#include -#include - -// The module IDs are defined as powers of 2, to make a bit-field -enum moduleid { - // not a module - used to set the "zero" value - AP_LIMITS_NULLMODULE = 0, - // a GPS-lock-required limit - AP_LIMITS_GPSLOCK = (1 << 0), - // fence within a set of coordinates - AP_LIMITS_GEOFENCE = (1 << 1), - // altitude limits - AP_LIMITS_ALTITUDE = (1 << 2) -}; - -extern const char *get_module_name(enum moduleid i); - -// an integer type big enough to fit a bit field for all modules. -// We have 3 modules, so 8-bit int is enough. -typedef uint8_t LimitModuleBits; - -class AP_Limit_Module { - -public: - // initialize a new module - AP_Limit_Module(enum moduleid id); - // initialize self - bool init(); - - virtual moduleid get_module_id(); - virtual bool enabled(); - virtual bool required(); - - // return next module in linked list - virtual AP_Limit_Module * next(); - - // link the next module in the linked list - virtual void link(AP_Limit_Module *m); - - // trigger check function - virtual bool triggered(); - - // recovery action - virtual void action(); - -protected: - // often exposed as a MAVLink parameter - AP_Int8 _enabled; - - AP_Int8 _required; - AP_Int8 _triggered; - -private: - enum moduleid _id; - AP_Limit_Module * _next; -}; - - -#endif // __AP_LIMIT_MODULE_H__ diff --git a/libraries/AP_Limits/AP_Limits.cpp b/libraries/AP_Limits/AP_Limits.cpp deleted file mode 100644 index 50137c40c9..0000000000 --- a/libraries/AP_Limits/AP_Limits.cpp +++ /dev/null @@ -1,204 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/// @file ap_limits.cpp -/// @brief Imposes limits on location (geofence), altitude and other parameters -/// Each breach will trigger an action or set of actions to recover. -/// Adapted from geofence. -/// @author Andrew Tridgell -/// Andreas Antonopoulos - -#include "AP_Limits.h" -#include "AP_Limit_Module.h" - -const AP_Param::GroupInfo AP_Limits::var_info[] = { - - // @Param: ENABLED - // @DisplayName: Enable Limits Library - // @Description: Setting this to Enabled(1) will enable the limits system - // @Values: 0:Disabled,1:Enabled - // @User: Standard - AP_GROUPINFO("ENABLED", 0, AP_Limits, _enabled, 0), - - // @Param: REQUIRED - // @DisplayName: Limits Library Required - // @Description: Setting this to 1 will enable the limits pre-arm checklist - // @Values: 0:Disabled,1:Enabled - // @User: Standard - AP_GROUPINFO("REQUIRED", 1, AP_Limits, _required, 0), - - // @Param: DEBUG - // @DisplayName: Enable Limits Debug - // @Description: Setting this to 1 will turn on debugging messages on the console and via MAVLink STATUSTEXT messages - // @Values: 0:Disabled,1:Enabled - // @User: Standard - AP_GROUPINFO("DEBUG", 2, AP_Limits, _debug, 0), - - // @Param: SAFETIME - // @DisplayName: Limits Safetime - // @Description: Automatic return of controls to pilot. Set to 0 to disable (full RTL) or a number of seconds after complete recovery to return the controls to the pilot in STABILIZE - // @Values: 0:Disabled,1-255:Seconds before returning control - // @User: Standard - AP_GROUPINFO("SAFETIME", 3, AP_Limits, _safetime, 0), - - // @Param: CHANNEL - // @DisplayName: Limits Channel - // @Description: Channel for Limits on/off control. If channel exceeds LIMITS_ENABLE_PWM, it turns limits on, and vice-versa. - // @Range: 1-8 - // @User: Standard - AP_GROUPINFO("CHANNEL", 4, AP_Limits, _channel, 0), - - // @Param: RECMODE - // @DisplayName: Limits Recovery Mode - // @Description: Select how Limits should "recover". Set to 0 for RTL-like mode, where the vehicle navigates back to home until it is "safe". Set to 1, for bounce-mode, where the vehicle will hold position when it hits a limit. RTL mode is better for large fenced areas, Bounce mode for smaller spaces. Note: RTL mode will cause the vehicle to yaw 180 degrees (turn around) to navigate towards home when it hits a limit. - // @Values: 0:RTL mode, 1: Bounce mode - // @User: Standard - AP_GROUPINFO("RECMODE", 5, AP_Limits, _recmode, 0), - - AP_GROUPEND -}; - -AP_Limits::AP_Limits() { - AP_Param::setup_object_defaults(this, var_info); - _state = LIMITS_INIT; -} - -void AP_Limits::modules(AP_Limit_Module *m) -{ - _modules_head = m; -} - -bool AP_Limits::init() { - AP_Limit_Module *m = modules_first(); - - while (m) { - m->init(); - m = modules_next(); - } - return true; -} - - -bool AP_Limits::enabled() { - return _enabled; -} - -bool AP_Limits::debug() { - return _debug; -} - - -AP_Limit_Module *AP_Limits::modules_first() { - // reset current to head of list - _modules_current = _modules_head; - return _modules_head; -} - -AP_Limit_Module *AP_Limits::modules_current() { - return _modules_current; -} - -AP_Limit_Module *AP_Limits::modules_next() { - if (_modules_current) { - // move to "next" (which might be NULL) - _modules_current = _modules_current->next(); - } - return _modules_current; -} - -uint8_t AP_Limits::modules_count() { - _modules_count = 0; - AP_Limit_Module *m = _modules_head; - - while (m) { - _modules_count++; - m = m->next(); - } - return _modules_count; -} - -AP_Limit_Module *AP_Limits::modules_last() { - AP_Limit_Module *m = _modules_head; - while (m && m->next()) { - m = m->next(); - } - return m; -} - -void AP_Limits::modules_add(AP_Limit_Module *m) { - if (_modules_head) { - // if there is a module linked add to the end of the list - modules_last()->link(m); - } else { - // otherwise, this will be the "head" - _modules_head = m; - } -} - -bool AP_Limits::required() { - return _required; -} - -bool AP_Limits::check_all() { - // required=false, means "all" - return check_triggered(false); -} - -bool AP_Limits::check_required() { - // required=true, means "only required modules" - return check_triggered(true); -} - -bool AP_Limits::check_triggered(bool only_required) { - - // check all enabled modules for triggered limits - AP_Limit_Module *mod = _modules_head; - - // reset bit fields - mods_triggered = 0; - mods_enabled = 0; - mods_required = 0; - - while (mod) { - - unsigned module_id = mod->get_module_id(); - - // We check enabled, triggered and required across all modules - // We set all the bit-fields each time we check, keeping them up to date - - if (mod->enabled()) { - mods_enabled |= module_id; - - if (mod->required()) mods_required |= module_id; - if (mod->triggered()) mods_triggered |= module_id; - } - - mod = mod->next(); - } - - if (only_required) { - // just modules that are both required AND triggered. (binary AND) - return (mods_required & mods_triggered) != 0; - } else { - return mods_triggered != 0; - } -} - -int8_t AP_Limits::state() { - return _state.get(); -} - -int8_t AP_Limits::safetime() { - return _safetime.get(); -} - -void AP_Limits::set_state(int s) { - _state.set(s); -} - - -int8_t AP_Limits::channel() { - return _channel.get(); -} - -int8_t AP_Limits::recmode() { - return _recmode.get(); -} diff --git a/libraries/AP_Limits/AP_Limits.h b/libraries/AP_Limits/AP_Limits.h deleted file mode 100644 index 13903ac425..0000000000 --- a/libraries/AP_Limits/AP_Limits.h +++ /dev/null @@ -1,122 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/// @brief Imposes limits on location (geofence), altitude and other parameters. -/// Each breach will trigger an action or set of actions to recover. -/// dapted from geofence. -/// @author Andrew Tridgell -/// Andreas Antonopoulos - -#ifndef __AP_LIMITS_H__ -#define __AP_LIMITS_H__ - -// XXX this isn't where enabled/disabled goes. -#ifndef AP_LIMITS - #define AP_LIMITS ENABLED -#endif - -#include -#include -#include "AP_Limit_Module.h" -#include "AP_Limit_Altitude.h" -#include "AP_Limit_Geofence.h" -#include "AP_Limit_GPSLock.h" - -#ifndef HAVE_ENUM_LIMITS_STATE - #define HAVE_ENUM_LIMITS_STATE -enum LimitState { - LIMITS_INIT, // pre-initialization - LIMITS_DISABLED, // disabled - LIMITS_ENABLED, // checking limits - LIMITS_TRIGGERED, // a limit has been breached - LIMITS_RECOVERING, // taking action, eg. RTL - LIMITS_RECOVERED, // we're no longer in breach of a limit -}; -#endif - -class AP_Limits { -public: - AP_Limits(); - - // access functions - bool enabled(); - bool debug(); - bool required(); - int8_t state(); - int8_t safetime(); - int8_t channel(); - int8_t recmode(); - - // module linked list management methods - // pointer to the first module in linked list of modules - void modules(AP_Limit_Module *m); - AP_Limit_Module * modules_first(); - AP_Limit_Module * modules_current(); - AP_Limit_Module * modules_last(); - AP_Limit_Module * modules_next(); - void modules_add(AP_Limit_Module *m); - uint8_t modules_count(); - - // main limit methods - // initialize self and all modules - bool init(); - // change state - void set_state(int s); - // check if any limit is triggered - bool check_all(); - // check if any of the required modules is triggered - bool check_required(); - // the function that does the checking for the two above - bool check_triggered(bool required); - - // time of last limit breach (trigger) - uint32_t last_trigger; - // time of last recovery action taken - uint32_t last_action; - // time of last recovery success - uint32_t last_recovery; - // time of last recovery success - uint32_t last_status_update; - // last time all triggers were clear (or 0 if never clear) - uint32_t last_clear; - // count of total breaches - uint16_t breach_count; - - uint8_t old_mode_switch; - - LimitModuleBits mods_enabled; - LimitModuleBits mods_required; - LimitModuleBits mods_triggered; - LimitModuleBits mods_recovering; - - // module parameters - static const struct AP_Param::GroupInfo var_info[]; - -protected: - // the entire AP_Limits system on/off switch - AP_Int8 _enabled; - // master switch for pre-arm checks of limits. Will not allow vehicle to - // "arm" if breaching limits. - AP_Int8 _required; - // enable debug console messages - AP_Int8 _debug; - // how long after recovered before switching to stabilize - AP_Int8 _safetime; - // overall state - used in the main loop state machine inside the - // vehicle's slow loop. - AP_Int8 _state; - // channel used for switching limits on/off - AP_Int8 _channel; - // recovery mode: 0=RTL mode, 1=bounce mode - AP_Int8 _recmode; - -private: - // points to linked list of modules - AP_Limit_Module * _modules_head; - // points to linked list of modules - AP_Limit_Module * _modules_current; - uint8_t _modules_count; - -}; - - -#endif // __AP_LIMITS_H__