From e511f722948361b262ed05e49d9bd90e1195f5bb Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 27 Jul 2022 19:20:12 +0100 Subject: [PATCH] AP_LandingGear: SITL: only set defualts is SITL pin is set avoiding enable via param conversion --- libraries/AP_LandingGear/AP_LandingGear.cpp | 15 +++++++++++++-- libraries/AP_LandingGear/AP_LandingGear.h | 8 -------- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/libraries/AP_LandingGear/AP_LandingGear.cpp b/libraries/AP_LandingGear/AP_LandingGear.cpp index 3f5b8ceaf7..807b4350f9 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.cpp +++ b/libraries/AP_LandingGear/AP_LandingGear.cpp @@ -6,6 +6,10 @@ #include #include +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#endif + extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_LandingGear::var_info[] = { @@ -47,14 +51,14 @@ const AP_Param::GroupInfo AP_LandingGear::var_info[] = { // @Values: -1:Disabled,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6 // @User: Standard // @RebootRequired: True - AP_GROUPINFO("WOW_PIN", 5, AP_LandingGear, _pin_weight_on_wheels, DEFAULT_PIN_WOW), + AP_GROUPINFO("WOW_PIN", 5, AP_LandingGear, _pin_weight_on_wheels, -1), // @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_GROUPINFO("WOW_POL", 6, AP_LandingGear, _pin_weight_on_wheels_polarity, 0), // @Param: DEPLOY_ALT // @DisplayName: Landing gear deployment altitude @@ -91,6 +95,13 @@ AP_LandingGear *AP_LandingGear::_singleton; /// initialise state of landing gear void AP_LandingGear::init() { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (AP::sitl()->wow_pin > 0) { + _pin_weight_on_wheels.set_and_default(AP::sitl()->wow_pin); + _pin_weight_on_wheels_polarity.set_and_default(1); + } +#endif + if (!_enable.configured() && (SRV_Channels::function_assigned(SRV_Channel::k_landing_gear_control) || (_pin_deployed > 0) || (_pin_weight_on_wheels > 0))) { // if not configured set enable param if output servo or sense pins are defined diff --git a/libraries/AP_LandingGear/AP_LandingGear.h b/libraries/AP_LandingGear/AP_LandingGear.h index 22222207c0..51ee1702ea 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.h +++ b/libraries/AP_LandingGear/AP_LandingGear.h @@ -5,14 +5,6 @@ #include #include -#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 {