uncrustify libraries/AP_Limits/AP_Limits.cpp
This commit is contained in:
uncrustify
Pat Hickey
parent
900ece6ef4
commit
0fc400e43e
@ -10,198 +10,198 @@
|
||||
|
||||
const AP_Param::GroupInfo AP_Limits::var_info[] PROGMEM = {
|
||||
|
||||
// @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
|
||||
// @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("RECMODE", 5, AP_Limits, _recmode, 0),
|
||||
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() {
|
||||
last_trigger = 0;
|
||||
last_action = 0;
|
||||
last_recovery = 0;
|
||||
last_clear = 0;
|
||||
last_status_update = 0;
|
||||
breach_count = 0;
|
||||
mods_enabled = 0;
|
||||
mods_required = 0;
|
||||
mods_triggered = 0;
|
||||
mods_recovering = 0;
|
||||
old_mode_switch = 0;
|
||||
_channel = 0;
|
||||
_state = LIMITS_INIT;
|
||||
last_trigger = 0;
|
||||
last_action = 0;
|
||||
last_recovery = 0;
|
||||
last_clear = 0;
|
||||
last_status_update = 0;
|
||||
breach_count = 0;
|
||||
mods_enabled = 0;
|
||||
mods_required = 0;
|
||||
mods_triggered = 0;
|
||||
mods_recovering = 0;
|
||||
old_mode_switch = 0;
|
||||
_channel = 0;
|
||||
_state = LIMITS_INIT;
|
||||
}
|
||||
|
||||
void AP_Limits::modules(AP_Limit_Module *m)
|
||||
{
|
||||
_modules_head = m;
|
||||
_modules_head = m;
|
||||
}
|
||||
|
||||
bool AP_Limits::init() {
|
||||
AP_Limit_Module *m = modules_first();
|
||||
AP_Limit_Module *m = modules_first();
|
||||
|
||||
while (m) {
|
||||
m->init();
|
||||
m = modules_next();
|
||||
}
|
||||
return true;
|
||||
while (m) {
|
||||
m->init();
|
||||
m = modules_next();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool AP_Limits::enabled() {
|
||||
return _enabled;
|
||||
return _enabled;
|
||||
}
|
||||
|
||||
bool AP_Limits::debug() {
|
||||
return _debug;
|
||||
return _debug;
|
||||
}
|
||||
|
||||
|
||||
AP_Limit_Module *AP_Limits::modules_first() {
|
||||
_modules_current = _modules_head; // reset current to head of list
|
||||
return _modules_head;
|
||||
AP_Limit_Module *AP_Limits::modules_first() {
|
||||
_modules_current = _modules_head; // reset current to head of list
|
||||
return _modules_head;
|
||||
}
|
||||
|
||||
AP_Limit_Module *AP_Limits::modules_current() {
|
||||
return _modules_current;
|
||||
AP_Limit_Module *AP_Limits::modules_current() {
|
||||
return _modules_current;
|
||||
}
|
||||
|
||||
AP_Limit_Module *AP_Limits::modules_next() {
|
||||
if (_modules_current) {
|
||||
_modules_current = _modules_current->next(); // move to "next" (which might be NULL)
|
||||
}
|
||||
return _modules_current;
|
||||
AP_Limit_Module *AP_Limits::modules_next() {
|
||||
if (_modules_current) {
|
||||
_modules_current = _modules_current->next(); // move to "next" (which might be NULL)
|
||||
}
|
||||
return _modules_current;
|
||||
}
|
||||
|
||||
uint8_t AP_Limits::modules_count() {
|
||||
_modules_count = 0;
|
||||
AP_Limit_Module *m = _modules_head;
|
||||
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;
|
||||
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;
|
||||
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
|
||||
modules_last()->link(m); // add to the end of the list
|
||||
}
|
||||
else {
|
||||
_modules_head = m; // otherwise, this will be the "head"
|
||||
}
|
||||
void AP_Limits::modules_add(AP_Limit_Module *m) {
|
||||
if (_modules_head) { // if there is a module linked
|
||||
modules_last()->link(m); // add to the end of the list
|
||||
}
|
||||
else {
|
||||
_modules_head = m; // otherwise, this will be the "head"
|
||||
}
|
||||
}
|
||||
|
||||
bool AP_Limits::required() {
|
||||
return _required;
|
||||
return _required;
|
||||
}
|
||||
|
||||
bool AP_Limits::check_all() {
|
||||
return (bool) check_triggered(false); // required=false, means "all"
|
||||
return (bool) check_triggered(false); // required=false, means "all"
|
||||
}
|
||||
|
||||
bool AP_Limits::check_required() {
|
||||
return (bool) check_triggered(true); // required=true, means "only required modules"
|
||||
return (bool) check_triggered(true); // required=true, means "only required modules"
|
||||
}
|
||||
|
||||
bool AP_Limits::check_triggered(bool only_required) {
|
||||
|
||||
// check all enabled modules for triggered limits
|
||||
AP_Limit_Module *mod = _modules_head;
|
||||
// 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;
|
||||
// reset bit fields
|
||||
mods_triggered = 0;
|
||||
mods_enabled = 0;
|
||||
mods_required = 0;
|
||||
|
||||
while (mod) {
|
||||
while (mod) {
|
||||
|
||||
unsigned module_id = mod->get_module_id();
|
||||
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
|
||||
// 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->enabled()) {
|
||||
mods_enabled |= module_id;
|
||||
|
||||
if (mod->required()) mods_required |= module_id;
|
||||
if (mod->triggered()) mods_triggered |= module_id;
|
||||
}
|
||||
if (mod->required()) mods_required |= module_id;
|
||||
if (mod->triggered()) mods_triggered |= module_id;
|
||||
}
|
||||
|
||||
mod = mod->next();
|
||||
}
|
||||
mod = mod->next();
|
||||
}
|
||||
|
||||
if (only_required) {
|
||||
return (bool) (mods_required & mods_triggered); // just modules that are both required AND triggered. (binary AND)
|
||||
}
|
||||
else return (bool) (mods_triggered);
|
||||
if (only_required) {
|
||||
return (bool) (mods_required & mods_triggered); // just modules that are both required AND triggered. (binary AND)
|
||||
}
|
||||
else return (bool) (mods_triggered);
|
||||
}
|
||||
|
||||
AP_Int8 AP_Limits::state() {
|
||||
return _state;
|
||||
return _state;
|
||||
}
|
||||
|
||||
AP_Int8 AP_Limits::safetime() {
|
||||
return _safetime;
|
||||
return _safetime;
|
||||
}
|
||||
|
||||
void AP_Limits::set_state(int s) {
|
||||
_state = (int) s;
|
||||
_state = (int) s;
|
||||
}
|
||||
|
||||
|
||||
AP_Int8 AP_Limits::channel() {
|
||||
return _channel;
|
||||
return _channel;
|
||||
}
|
||||
|
||||
AP_Int8 AP_Limits::recmode() {
|
||||
return _recmode;
|
||||
return _recmode;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user