diff --git a/libraries/AP_LandingGear/AP_LandingGear.cpp b/libraries/AP_LandingGear/AP_LandingGear.cpp index 208df7b180..17d317f35c 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.cpp +++ b/libraries/AP_LandingGear/AP_LandingGear.cpp @@ -29,10 +29,24 @@ const AP_Param::GroupInfo AP_LandingGear::var_info[] = { AP_GROUPEND }; -/// enable - enable or disable land gear retraction -void AP_LandingGear::enable(bool on_off) +/// set landing gear position to retract, deploy or deploy-and-keep-deployed +void AP_LandingGear::set_position(LandingGearCommand cmd) { - _retract_enabled = on_off; + switch (cmd) { + case LandingGear_Retract: + if (!_deploy_lock) { + retract(); + } + break; + case LandingGear_Deploy: + deploy(); + _deploy_lock = false; + break; + case LandingGear_Deploy_And_Keep_Deployed: + deploy(); + _deploy_lock = true; + break; + } } /// deploy - deploy landing gear @@ -42,43 +56,15 @@ void AP_LandingGear::deploy() SRV_Channels::set_output_pwm(SRV_Channel::k_landing_gear_control, _servo_deploy_pwm); // set deployed flag - _deployed = true; + _deployed = true; } /// retract - retract landing gear void AP_LandingGear::retract() -{ +{ // set servo PWM to retracted position SRV_Channels::set_output_pwm(SRV_Channel::k_landing_gear_control, _servo_retract_pwm); // reset deployed flag - _deployed = false; -} - -/// update - should be called at 10hz -void AP_LandingGear::update() -{ - // if there is a force deploy active, disable retraction, then reset force deploy to consume it - // gear will be deployed automatically because _retract_enabled is false. - // this will disable retract switch until it is cycled through deploy position - if (_force_deploy){ - enable(false); - force_deploy(false); - } - - if (!_retract_enabled) { - // force deployment if retract is not enabled - deploy(); - // retract is disabled until switch is placed into deploy position to prevent accidental retraction on bootup if switch was left in retract position - enable(_command_mode == LandingGear_Deploy); - return; - } - - if (_command_mode == LandingGear_Deploy){ - deploy(); - } - - if (_command_mode == LandingGear_Retract){ - retract(); - } + _deployed = false; } diff --git a/libraries/AP_LandingGear/AP_LandingGear.h b/libraries/AP_LandingGear/AP_LandingGear.h index f6371bfafa..01cfbcab93 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.h +++ b/libraries/AP_LandingGear/AP_LandingGear.h @@ -8,59 +8,43 @@ #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 -// Gear command modes -enum LandingGearCommandMode { - LandingGear_Deploy, - LandingGear_Auto, - LandingGear_Retract -}; - /// @class AP_LandingGear /// @brief Class managing the control of landing gear class AP_LandingGear { public: + // Gear command modes + enum LandingGearCommand { + LandingGear_Retract, + LandingGear_Deploy, + LandingGear_Deploy_And_Keep_Deployed, + }; + /// Constructor - AP_LandingGear() : - _retract_enabled(false), - _deployed(false), - _force_deploy(false), - _command_mode(LandingGear_Deploy) + AP_LandingGear() { // setup parameter defaults AP_Param::setup_object_defaults(this, var_info); } - /// deployed - returns true if the landing gear is deployed + /// returns true if the landing gear is deployed bool deployed() const { return _deployed; } - /// update - should be called at 10hz - void update(); - - /// set_cmd_mode - set command mode to deploy, auto or retract - void set_cmd_mode(LandingGearCommandMode cmd) { _command_mode = cmd; } - - /// force_deploy - set to true to force gear to deploy - void force_deploy(bool force) { _force_deploy = force;} + /// set landing gear position to retract, deploy or deploy-and-keep-deployed + void set_position(LandingGearCommand cmd); static const struct AP_Param::GroupInfo var_info[]; private: - bool _retract_enabled; // true if landing gear retraction is enabled - // 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 // internal variables bool _deployed; // true if the landing gear has been deployed, initialized false - bool _force_deploy; // used by main code to force landing gear to deploy, such as in Land mode - LandingGearCommandMode _command_mode; // pilots commanded control mode: Manual Deploy, Auto, or Manual Retract - - /// enable - enable landing gear retraction - void enable(bool on_off); + bool _deploy_lock; // used to force landing gear to remain deployed until another regular Deploy command is received to reduce accidental retraction /// retract - retract landing gear void retract();