zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Limits: Remove unused module

mission-4.1.18
Daniel Frenzel 9 years ago committed by Randy Mackay
parent
05f4c8b5b1
commit
b01406e671
10 changed files with 0 additions and 923 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 81
      libraries/AP_Limits/AP_Limit_Altitude.cpp
  2. 37
      libraries/AP_Limits/AP_Limit_Altitude.h
  3. 48
      libraries/AP_Limits/AP_Limit_GPSLock.cpp
  4. 30
      libraries/AP_Limits/AP_Limit_GPSLock.h
  5. 194
      libraries/AP_Limits/AP_Limit_Geofence.cpp
  6. 61
      libraries/AP_Limits/AP_Limit_Geofence.h
  7. 75
      libraries/AP_Limits/AP_Limit_Module.cpp
  8. 71
      libraries/AP_Limits/AP_Limit_Module.h
  9. 204
      libraries/AP_Limits/AP_Limits.cpp
  10. 122
      libraries/AP_Limits/AP_Limits.h

81
libraries/AP_Limits/AP_Limit_Altitude.cpp
Unescape View File

@ -1,81 +0,0 @@ @@ -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;
}

37
libraries/AP_Limits/AP_Limit_Altitude.h
Unescape View File

@ -1,37 +0,0 @@ @@ -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 <AP_Param/AP_Param.h>
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__

48
libraries/AP_Limits/AP_Limit_GPSLock.cpp
Unescape View File

@ -1,48 +0,0 @@ @@ -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;
}

30
libraries/AP_Limits/AP_Limit_GPSLock.h
Unescape View File

@ -1,30 +0,0 @@ @@ -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 <GPS.h>
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__

194
libraries/AP_Limits/AP_Limit_Geofence.cpp
Unescape View File

@ -1,194 +0,0 @@ @@ -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 <AP_HAL/AP_HAL.h>
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;
}

61
libraries/AP_Limits/AP_Limit_Geofence.h
Unescape View File

@ -1,61 +0,0 @@ @@ -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 <AP_Math/AP_Math.h>
#include <AP_Param/AP_Param.h>
#include <GPS.h>
#include <StorageManager/StorageManager.h>
#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__

75
libraries/AP_Limits/AP_Limit_Module.cpp
Unescape View File

@ -1,75 +0,0 @@ @@ -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
}

71
libraries/AP_Limits/AP_Limit_Module.h
Unescape View File

@ -1,71 +0,0 @@ @@ -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 <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
// 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__

204
libraries/AP_Limits/AP_Limits.cpp
Unescape View File

@ -1,204 +0,0 @@ @@ -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();
}

122
libraries/AP_Limits/AP_Limits.h
Unescape View File

@ -1,122 +0,0 @@ @@ -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 <stdint.h>
#include <AP_Param/AP_Param.h>
#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__
Write Preview
Loading…
Cancel
Save