diff --git a/libraries/AP_LandingGear/AP_LandingGear.cpp b/libraries/AP_LandingGear/AP_LandingGear.cpp index 93b3d9f36a..d2e9ad14c1 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.cpp +++ b/libraries/AP_LandingGear/AP_LandingGear.cpp @@ -4,6 +4,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -63,6 +64,24 @@ const AP_Param::GroupInfo AP_LandingGear::var_info[] = { // @Values: 0:Low,1:High // @User: Standard AP_GROUPINFO("WOW_POL", 6, AP_LandingGear, _pin_weight_on_wheels_polarity, DEFAULT_PIN_WOW_POL), + + // @Param: DEPLOY_ALT + // @DisplayName: Landing gear deployment altitude + // @Description: Altitude where the landing gear will be deployed. This should be lower than the RETRACT_ALT. If zero then altitude is not used for deploying landing gear. Only applies when vehicle is armed. + // @Units: m + // @Range: 0 1000 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("DEPLOY_ALT", 7, AP_LandingGear, _deploy_alt, 0), + + // @Param: RETRACT_ALT + // @DisplayName: Landing gear retract altitude + // @Description: Altitude where the landing gear will be retracted. This should be higher than the DEPLOY_ALT. If zero then altitude is not used for retracting landing gear. Only applies when vehicle is armed. + // @Units: m + // @Range: 0 1000 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("RETRACT_ALT", 8, AP_LandingGear, _retract_alt, 0), AP_GROUPEND }; @@ -121,6 +140,9 @@ void AP_LandingGear::deploy() // set deployed flag _deployed = true; + _have_changed = true; + + gcs().send_text(MAV_SEVERITY_INFO, "LandingGear: DEPLOY"); } /// retract - retract landing gear @@ -131,6 +153,9 @@ void AP_LandingGear::retract() // reset deployed flag _deployed = false; + _have_changed = true; + + gcs().send_text(MAV_SEVERITY_INFO, "LandingGear: RETRACT"); } bool AP_LandingGear::deployed() @@ -170,7 +195,7 @@ uint32_t AP_LandingGear::get_wow_state_duration_ms() return AP_HAL::millis() - last_wow_event_ms; } -void AP_LandingGear::update() +void AP_LandingGear::update(float height_above_ground_m) { if (_pin_weight_on_wheels == -1) { last_wow_event_ms = 0; @@ -208,6 +233,30 @@ void AP_LandingGear::update() gear_state_current = gear_state_new; } + + /* + check for height based triggering + */ + int16_t alt_m = constrain_int16(height_above_ground_m, 0, INT16_MAX); + + if (hal.util->get_soft_armed()) { + // only do height based triggering when armed + if ((!_deployed || !_have_changed) && + _deploy_alt > 0 && + alt_m <= _deploy_alt && + _last_height_above_ground > _deploy_alt) { + deploy(); + } + if ((_deployed || !_have_changed) && + _retract_alt > 0 && + _retract_alt >= _deploy_alt && + alt_m >= _retract_alt && + _last_height_above_ground < _retract_alt) { + retract(); + } + } + + _last_height_above_ground = alt_m; } // log weight on wheels state diff --git a/libraries/AP_LandingGear/AP_LandingGear.h b/libraries/AP_LandingGear/AP_LandingGear.h index 61b5d03bb2..493433e587 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.h +++ b/libraries/AP_LandingGear/AP_LandingGear.h @@ -84,7 +84,7 @@ public: static const struct AP_Param::GroupInfo var_info[]; - void update(); + void update(float height_above_ground_m); private: // Parameters @@ -96,13 +96,17 @@ private: AP_Int8 _pin_deployed_polarity; AP_Int8 _pin_weight_on_wheels; AP_Int8 _pin_weight_on_wheels_polarity; + AP_Int16 _deploy_alt; + AP_Int16 _retract_alt; // internal variables bool _deployed; // true if the landing gear has been deployed, initialized false + bool _have_changed; // have we changed the servo state? 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; + int16_t _last_height_above_ground; // debounce LG_WOW_State wow_state_current = LG_WOW_UNKNOWN;