AP_LandingGear: weight on wheels, debounce, singleton

libraries/AP_LandingGear/AP_LandingGear.cpp

@@ -3,9 +3,12 @@
 #include <AP_Math/AP_Math.h>
 #include <SRV_Channel/SRV_Channel.h>
 #include <AP_HAL/AP_HAL.h>
+#include <DataFlash/DataFlash.h>
 
 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);
+    }
+}

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;
 };