Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_LandingGear: weight on wheels, debounce, singleton

This commit is contained in:
Eugene Shamaev 2018-06-10 09:34:02 +03:00 committed by Andrew Tridgell
parent 3794f9d51e
commit fa6afe145f
2 changed files with 197 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
libraries/AP_LandingGear/AP_LandingGear.cpp
View File

@ -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
@ -33,12 +36,56 @@ const AP_Param::GroupInfo AP_LandingGear::var_info[] = {
// @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);
}
}

64
libraries/AP_LandingGear/AP_LandingGear.h
View File

@ -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,12 +23,22 @@ 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 {
LandingGear_Retract,
@ -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;
};