mirror of https://github.com/ArduPilot/ardupilot
uncrustify libraries/AP_Limits/AP_Limits.h
This commit is contained in:
parent
06c79f9aa6
commit
815d7c583e
|
@ -9,7 +9,7 @@
|
||||||
#define AP_LIMITS_H
|
#define AP_LIMITS_H
|
||||||
|
|
||||||
#ifndef AP_LIMITS
|
#ifndef AP_LIMITS
|
||||||
#define AP_LIMITS ENABLED
|
#define AP_LIMITS ENABLED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
@ -25,14 +25,14 @@
|
||||||
|
|
||||||
|
|
||||||
#ifndef HAVE_ENUM_LIMITS_STATE
|
#ifndef HAVE_ENUM_LIMITS_STATE
|
||||||
#define HAVE_ENUM_LIMITS_STATE
|
#define HAVE_ENUM_LIMITS_STATE
|
||||||
enum LimitState {
|
enum LimitState {
|
||||||
LIMITS_INIT, // pre-initialization
|
LIMITS_INIT, // pre-initialization
|
||||||
LIMITS_DISABLED, // disabled
|
LIMITS_DISABLED, // disabled
|
||||||
LIMITS_ENABLED, // checking limits
|
LIMITS_ENABLED, // checking limits
|
||||||
LIMITS_TRIGGERED, // a limit has been breached
|
LIMITS_TRIGGERED, // a limit has been breached
|
||||||
LIMITS_RECOVERING, // taking action, eg. RTL
|
LIMITS_RECOVERING, // taking action, eg. RTL
|
||||||
LIMITS_RECOVERED, // we're no longer in breach of a limit
|
LIMITS_RECOVERED, // we're no longer in breach of a limit
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -40,64 +40,64 @@ class AP_Limits {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
AP_Limits();
|
AP_Limits();
|
||||||
|
|
||||||
// access functions
|
// access functions
|
||||||
bool enabled();
|
bool enabled();
|
||||||
bool debug();
|
bool debug();
|
||||||
bool required();
|
bool required();
|
||||||
AP_Int8 state();
|
AP_Int8 state();
|
||||||
AP_Int8 safetime();
|
AP_Int8 safetime();
|
||||||
AP_Int8 channel();
|
AP_Int8 channel();
|
||||||
AP_Int8 recmode();
|
AP_Int8 recmode();
|
||||||
|
|
||||||
// module linked list management methods
|
// module linked list management methods
|
||||||
void modules(AP_Limit_Module *m); // pointer to the first module in linked list of modules
|
void modules(AP_Limit_Module *m); // pointer to the first module in linked list of modules
|
||||||
AP_Limit_Module *modules_first();
|
AP_Limit_Module * modules_first();
|
||||||
AP_Limit_Module *modules_current();
|
AP_Limit_Module * modules_current();
|
||||||
AP_Limit_Module *modules_last();
|
AP_Limit_Module * modules_last();
|
||||||
AP_Limit_Module *modules_next();
|
AP_Limit_Module * modules_next();
|
||||||
void modules_add(AP_Limit_Module *m);
|
void modules_add(AP_Limit_Module *m);
|
||||||
uint8_t modules_count();
|
uint8_t modules_count();
|
||||||
|
|
||||||
// main limit methods
|
// main limit methods
|
||||||
bool init(); // initialize self and all modules
|
bool init(); // initialize self and all modules
|
||||||
void set_state(int s); // change state
|
void set_state(int s); // change state
|
||||||
bool check_all(); // check if any limit is triggered
|
bool check_all(); // check if any limit is triggered
|
||||||
bool check_required(); // check if any of the required modules is triggered
|
bool check_required(); // check if any of the required modules is triggered
|
||||||
bool check_triggered(bool required); // the function that does the checking for the two above
|
bool check_triggered(bool required); // the function that does the checking for the two above
|
||||||
|
|
||||||
uint32_t last_trigger; // time of last limit breach (trigger)
|
uint32_t last_trigger; // time of last limit breach (trigger)
|
||||||
uint32_t last_action; // time of last recovery action taken
|
uint32_t last_action; // time of last recovery action taken
|
||||||
uint32_t last_recovery; // time of last recovery success
|
uint32_t last_recovery; // time of last recovery success
|
||||||
uint32_t last_status_update; // time of last recovery success
|
uint32_t last_status_update; // time of last recovery success
|
||||||
uint32_t last_clear; // last time all triggers were clear (or 0 if never clear)
|
uint32_t last_clear; // last time all triggers were clear (or 0 if never clear)
|
||||||
|
|
||||||
uint16_t breach_count; // count of total breaches
|
uint16_t breach_count; // count of total breaches
|
||||||
|
|
||||||
byte old_mode_switch;
|
byte old_mode_switch;
|
||||||
|
|
||||||
LimitModuleBits mods_enabled;
|
LimitModuleBits mods_enabled;
|
||||||
LimitModuleBits mods_required;
|
LimitModuleBits mods_required;
|
||||||
LimitModuleBits mods_triggered;
|
LimitModuleBits mods_triggered;
|
||||||
LimitModuleBits mods_recovering;
|
LimitModuleBits mods_recovering;
|
||||||
|
|
||||||
|
|
||||||
static const struct AP_Param::GroupInfo var_info[]; // module parameters
|
static const struct AP_Param::GroupInfo var_info[]; // module parameters
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
AP_Int8 _enabled; // the entire AP_Limits system on/off switch
|
AP_Int8 _enabled; // the entire AP_Limits system on/off switch
|
||||||
AP_Int8 _required; // master switch for pre-arm checks of limits. Will not allow vehicle to "arm" if breaching limits.
|
AP_Int8 _required; // master switch for pre-arm checks of limits. Will not allow vehicle to "arm" if breaching limits.
|
||||||
AP_Int8 _debug; // enable debug console messages
|
AP_Int8 _debug; // enable debug console messages
|
||||||
AP_Int8 _safetime; // how long after recovered before switching to stabilize
|
AP_Int8 _safetime; // how long after recovered before switching to stabilize
|
||||||
AP_Int8 _state; // overall state - used in the main loop state machine inside the vehicle's slow loop.
|
AP_Int8 _state; // overall state - used in the main loop state machine inside the vehicle's slow loop.
|
||||||
AP_Int8 _channel; // channel used for switching limits on/off
|
AP_Int8 _channel; // channel used for switching limits on/off
|
||||||
AP_Int8 _recmode; // recovery mode: 0=RTL mode, 1=bounce mode
|
AP_Int8 _recmode; // recovery mode: 0=RTL mode, 1=bounce mode
|
||||||
|
|
||||||
private:
|
private:
|
||||||
AP_Limit_Module *_modules_head; // points to linked list of modules
|
AP_Limit_Module * _modules_head; // points to linked list of modules
|
||||||
AP_Limit_Module *_modules_current; // points to linked list of modules
|
AP_Limit_Module * _modules_current; // points to linked list of modules
|
||||||
uint8_t _modules_count;
|
uint8_t _modules_count;
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue