diff --git a/libraries/AP_LandingGear/AP_LandingGear.cpp b/libraries/AP_LandingGear/AP_LandingGear.cpp index 17d317f35c..51350ce783 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.cpp +++ b/libraries/AP_LandingGear/AP_LandingGear.cpp @@ -26,9 +26,33 @@ const AP_Param::GroupInfo AP_LandingGear::var_info[] = { // @User: Standard AP_GROUPINFO("SERVO_DEPLOY", 1, AP_LandingGear, _servo_deploy_pwm, AP_LANDINGGEAR_SERVO_DEPLOY_PWM_DEFAULT), + // @Param: STARTUP + // @DisplayName: Landing Gear Startup position + // @Description: Landing Gear Startup behaviour control + // @Values: 0:WaitForPilotInput, 1:Retract, 2:Deploy + // @User: Standard + AP_GROUPINFO("STARTUP", 2, AP_LandingGear, _startup_behaviour, (uint8_t)AP_LandingGear::LandingGear_Startup_WaitForPilotInput), + AP_GROUPEND }; +/// initialise state of landing gear +void AP_LandingGear::init() +{ + switch ((enum LandingGearStartupBehaviour)_startup_behaviour.get()) { + default: + case LandingGear_Startup_WaitForPilotInput: + // do nothing + break; + case LandingGear_Startup_Retract: + retract(); + break; + case LandingGear_Startup_Deploy: + deploy(); + break; + } +} + /// set landing gear position to retract, deploy or deploy-and-keep-deployed void AP_LandingGear::set_position(LandingGearCommand cmd) { diff --git a/libraries/AP_LandingGear/AP_LandingGear.h b/libraries/AP_LandingGear/AP_LandingGear.h index 01cfbcab93..de7bc78bfc 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.h +++ b/libraries/AP_LandingGear/AP_LandingGear.h @@ -21,6 +21,13 @@ public: LandingGear_Deploy_And_Keep_Deployed, }; + // Gear command modes + enum LandingGearStartupBehaviour { + LandingGear_Startup_WaitForPilotInput = 0, + LandingGear_Startup_Retract = 1, + LandingGear_Startup_Deploy = 2, + }; + /// Constructor AP_LandingGear() { @@ -28,6 +35,9 @@ public: AP_Param::setup_object_defaults(this, var_info); } + /// initialise state of landing gear + void init(); + /// returns true if the landing gear is deployed bool deployed() const { return _deployed; } @@ -41,6 +51,7 @@ 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) // internal variables bool _deployed; // true if the landing gear has been deployed, initialized false