From fa6afe145f62c9821034772bd6e232e43f05372b Mon Sep 17 00:00:00 2001 From: Eugene Shamaev Date: Sun, 10 Jun 2018 09:34:02 +0300 Subject: [PATCH] AP_LandingGear: weight on wheels, debounce, singleton --- libraries/AP_LandingGear/AP_LandingGear.cpp | 134 ++++++++++++++++++++ libraries/AP_LandingGear/AP_LandingGear.h | 64 +++++++++- 2 files changed, 197 insertions(+), 1 deletion(-) diff --git a/libraries/AP_LandingGear/AP_LandingGear.cpp b/libraries/AP_LandingGear/AP_LandingGear.cpp index ee91564c29..eb2f47bed6 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.cpp +++ b/libraries/AP_LandingGear/AP_LandingGear.cpp @@ -3,9 +3,12 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; +#define MASK_LOG_CTUN (1<<4) + const AP_Param::GroupInfo AP_LandingGear::var_info[] = { // @Param: SERVO_RTRACT @@ -32,13 +35,57 @@ const AP_Param::GroupInfo AP_LandingGear::var_info[] = { // @Values: 0:WaitForPilotInput, 1:Retract, 2:Deploy // @User: Standard AP_GROUPINFO("STARTUP", 2, AP_LandingGear, _startup_behaviour, (uint8_t)AP_LandingGear::LandingGear_Startup_WaitForPilotInput), + + // @Param: DEPLOY_PIN + // @DisplayName: Chassis deployment feedback pin + // @Description: Pin number to use for feedback of gear deployment. If set to -1 feedback is disabled. + // @Values: -1:Disabled,50:PX4 AUX1,51:PX4 AUX2,52:PX4 AUX3,53:PX4 AUX4,54:PX4 AUX5,55:PX4 AUX6 + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO("DEPLOY_PIN", 3, AP_LandingGear, _pin_deployed, -1), + // @Param: DEPLOY_POL + // @DisplayName: Chassis deployment feedback pin polarity + // @Description: Polarity for feedback pin. If this is 1 then the pin should be high when gear are deployed. If set to 0 then then deployed gear level is low. + // @Values: 0:Low,1:High + // @User: Standard + AP_GROUPINFO("DEPLOY_POL", 4, AP_LandingGear, _pin_deployed_polarity, 0), + + // @Param: WOW_PIN + // @DisplayName: Weight on wheels feedback pin + // @Description: Pin number to use for feedback of weight on wheels condition. If set to -1 feedback is disabled. + // @Values: -1:Disabled,50:PX4 AUX1,51:PX4 AUX2,52:PX4 AUX3,53:PX4 AUX4,54:PX4 AUX5,55:PX4 AUX6 + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO("WOW_PIN", 5, AP_LandingGear, _pin_weight_on_wheels, DEFAULT_PIN_WOW), + + // @Param: WOW_POL + // @DisplayName: Weight on wheels feedback pin polarity + // @Description: Polarity for feedback pin. If this is 1 then the pin should be high when there is weight on wheels. If set to 0 then then weight on wheels level is low. + // @Values: 0:Low,1:High + // @User: Standard + AP_GROUPINFO("WOW_POL", 6, AP_LandingGear, _pin_weight_on_wheels_polarity, DEFAULT_PIN_WOW_POL), + AP_GROUPEND }; +AP_LandingGear *AP_LandingGear::_singleton; + /// initialise state of landing gear void AP_LandingGear::init() { + if (_pin_deployed != -1) { + hal.gpio->pinMode(_pin_deployed, HAL_GPIO_INPUT); + hal.gpio->write(_pin_deployed, 1); + log_wow_state(wow_state_current); + } + + if (_pin_weight_on_wheels != -1) { + hal.gpio->pinMode(_pin_weight_on_wheels, HAL_GPIO_INPUT); + hal.gpio->write(_pin_weight_on_wheels, 1); + log_wow_state(wow_state_current); + } + switch ((enum LandingGearStartupBehaviour)_startup_behaviour.get()) { default: case LandingGear_Startup_WaitForPilotInput: @@ -85,3 +132,90 @@ void AP_LandingGear::retract() // reset deployed flag _deployed = false; } + +bool AP_LandingGear::deployed() +{ + if (_pin_deployed == -1) { + return _deployed; + } else { + return hal.gpio->read(_pin_deployed) == _pin_deployed_polarity ? true : false; + } +} + +AP_LandingGear::LG_WOW_State AP_LandingGear::get_wow_state() +{ + return wow_state_current; +} + +AP_LandingGear::LG_LandingGear_State AP_LandingGear::get_state() +{ + return gear_state_current; +} + +uint32_t AP_LandingGear::get_gear_state_duration_ms() +{ + if (last_gear_event_ms == 0) { + return 0; + } + + return AP_HAL::millis() - last_gear_event_ms; +} + +uint32_t AP_LandingGear::get_wow_state_duration_ms() +{ + if (last_wow_event_ms == 0) { + return 0; + } + + return AP_HAL::millis() - last_wow_event_ms; +} + +void AP_LandingGear::update() +{ + if (_pin_weight_on_wheels == -1) { + last_wow_event_ms = 0; + wow_state_current = LG_WOW_UNKNOWN; + } else { + LG_WOW_State wow_state_new = hal.gpio->read(_pin_weight_on_wheels) == _pin_weight_on_wheels_polarity ? LG_WOW : LG_NO_WOW; + + if (wow_state_new != wow_state_current) { + // we changed states, lets note the time. + last_wow_event_ms = AP_HAL::millis(); + log_wow_state(wow_state_new); + } + + wow_state_current = wow_state_new; + } + + if (_pin_deployed == -1) { + last_gear_event_ms = 0; + gear_state_current = (_deployed == true ? LG_DEPLOYED : LG_RETRACTED); + } else { + LG_LandingGear_State gear_state_new; + + if (_deployed) { + gear_state_new = (deployed() == true ? LG_DEPLOYED : LG_DEPLOYING); + } else { + gear_state_new = (deployed() == false ? LG_RETRACTED : LG_RETRACTING); + } + + if (gear_state_new != gear_state_current) { + // we changed states, lets note the time. + last_gear_event_ms = AP_HAL::millis(); + + log_wow_state(wow_state_current); + } + + gear_state_current = gear_state_new; + } +} + +// log weight on wheels state +void AP_LandingGear::log_wow_state(LG_WOW_State state) +{ + if (DataFlash_Class::instance()->should_log(MASK_LOG_CTUN)) { + DataFlash_Class::instance()->Log_Write("LGR", "TimeUS,LandingGear,WeightOnWheels", "Qbb", + AP_HAL::micros64(), + (int8_t)gear_state_current, (int8_t)state); + } +} diff --git a/libraries/AP_LandingGear/AP_LandingGear.h b/libraries/AP_LandingGear/AP_LandingGear.h index 829efef24a..12e249f655 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.h +++ b/libraries/AP_LandingGear/AP_LandingGear.h @@ -8,6 +8,14 @@ #define AP_LANDINGGEAR_SERVO_RETRACT_PWM_DEFAULT 1250 // default PWM value to move servo to when landing gear is up #define AP_LANDINGGEAR_SERVO_DEPLOY_PWM_DEFAULT 1750 // default PWM value to move servo to when landing gear is down +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#define DEFAULT_PIN_WOW 8 +#define DEFAULT_PIN_WOW_POL 1 +#else +#define DEFAULT_PIN_WOW -1 +#define DEFAULT_PIN_WOW_POL 0 +#endif + /// @class AP_LandingGear /// @brief Class managing the control of landing gear class AP_LandingGear { @@ -15,11 +23,21 @@ public: AP_LandingGear() { // setup parameter defaults AP_Param::setup_object_defaults(this, var_info); + + if (_singleton != nullptr) { + AP_HAL::panic("AP_LandingGear must be singleton"); + } + _singleton = this; } /* Do not allow copies */ AP_LandingGear(const AP_LandingGear &other) = delete; AP_LandingGear &operator=(const AP_LandingGear&) = delete; + + // get singleton instance + static AP_LandingGear &instance(void) { + return *_singleton; + } // Gear command modes enum LandingGearCommand { @@ -38,25 +56,69 @@ public: void init(); /// returns true if the landing gear is deployed - bool deployed() const { return _deployed; } + bool deployed(); + + enum LG_LandingGear_State { + LG_UNKNOWN = -1, + LG_RETRACTED = 0, + LG_DEPLOYED = 1, + LG_RETRACTING = 2, + LG_DEPLOYING = 3, + }; + /// returns detailed state of gear + LG_LandingGear_State get_state(); + + enum LG_WOW_State { + LG_WOW_UNKNOWN = -1, + LG_NO_WOW = 0, + LG_WOW = 1, + }; + + LG_WOW_State get_wow_state(); /// set landing gear position to retract, deploy or deploy-and-keep-deployed void set_position(LandingGearCommand cmd); + + uint32_t get_gear_state_duration_ms(); + uint32_t get_wow_state_duration_ms(); static const struct AP_Param::GroupInfo var_info[]; + + void update(); private: // Parameters AP_Int16 _servo_retract_pwm; // PWM value to move servo to when gear is retracted AP_Int16 _servo_deploy_pwm; // PWM value to move servo to when gear is deployed AP_Int8 _startup_behaviour; // start-up behaviour (see LandingGearStartupBehaviour) + + AP_Int8 _pin_deployed; + AP_Int8 _pin_deployed_polarity; + AP_Int8 _pin_weight_on_wheels; + AP_Int8 _pin_weight_on_wheels_polarity; // internal variables bool _deployed; // true if the landing gear has been deployed, initialized false + + bool _deploy_lock; // used to force landing gear to remain deployed until another regular Deploy command is received to reduce accidental retraction + bool _deploy_pin_state; + bool _weight_on_wheels_pin_state; + // debounce + LG_WOW_State wow_state_current = LG_WOW_UNKNOWN; + uint32_t last_wow_event_ms; + + LG_LandingGear_State gear_state_current = LG_UNKNOWN; + uint32_t last_gear_event_ms; + /// retract - retract landing gear void retract(); /// deploy - deploy the landing gear void deploy(); + + // log weight on wheels state + void log_wow_state(LG_WOW_State state); + + static AP_LandingGear *_singleton; };