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

AP_LandingGear: added height based triggering 

very useful for manual takeoffs and landings
This commit is contained in:
Andrew Tridgell 2018-11-09 15:39:59 +11:00
parent 6b5088207b
commit 486ecd6148
2 changed files with 55 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/AP_LandingGear/AP_LandingGear.cpp
View File

@ -4,6 +4,7 @@
#include <SRV_Channel/SRV_Channel.h>
#include <AP_HAL/AP_HAL.h>
#include <DataFlash/DataFlash.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
@ -63,6 +64,24 @@ const AP_Param::GroupInfo AP_LandingGear::var_info[] = {
// @Values: 0:Low,1:High
// @User: Standard
AP_GROUPINFO("WOW_POL", 6, AP_LandingGear, _pin_weight_on_wheels_polarity, DEFAULT_PIN_WOW_POL),
// @Param: DEPLOY_ALT
// @DisplayName: Landing gear deployment altitude
// @Description: Altitude where the landing gear will be deployed. This should be lower than the RETRACT_ALT. If zero then altitude is not used for deploying landing gear. Only applies when vehicle is armed.
// @Units: m
// @Range: 0 1000
// @Increment: 1
// @User: Standard
AP_GROUPINFO("DEPLOY_ALT", 7, AP_LandingGear, _deploy_alt, 0),
// @Param: RETRACT_ALT
// @DisplayName: Landing gear retract altitude
// @Description: Altitude where the landing gear will be retracted. This should be higher than the DEPLOY_ALT. If zero then altitude is not used for retracting landing gear. Only applies when vehicle is armed.
// @Units: m
// @Range: 0 1000
// @Increment: 1
// @User: Standard
AP_GROUPINFO("RETRACT_ALT", 8, AP_LandingGear, _retract_alt, 0),
AP_GROUPEND
};
@ -121,6 +140,9 @@ void AP_LandingGear::deploy()
// set deployed flag
_deployed = true;
_have_changed = true;
gcs().send_text(MAV_SEVERITY_INFO, "LandingGear: DEPLOY");
}
/// retract - retract landing gear
@ -131,6 +153,9 @@ void AP_LandingGear::retract()
// reset deployed flag
_deployed = false;
_have_changed = true;
gcs().send_text(MAV_SEVERITY_INFO, "LandingGear: RETRACT");
}
bool AP_LandingGear::deployed()
@ -170,7 +195,7 @@ uint32_t AP_LandingGear::get_wow_state_duration_ms()
return AP_HAL::millis() - last_wow_event_ms;
}
void AP_LandingGear::update()
void AP_LandingGear::update(float height_above_ground_m)
{
if (_pin_weight_on_wheels == -1) {
last_wow_event_ms = 0;
@ -208,6 +233,30 @@ void AP_LandingGear::update()
gear_state_current = gear_state_new;
}
/*
check for height based triggering
*/
int16_t alt_m = constrain_int16(height_above_ground_m, 0, INT16_MAX);
if (hal.util->get_soft_armed()) {
// only do height based triggering when armed
if ((!_deployed || !_have_changed) &&
_deploy_alt > 0 &&
alt_m <= _deploy_alt &&
_last_height_above_ground > _deploy_alt) {
deploy();
}
if ((_deployed || !_have_changed) &&
_retract_alt > 0 &&
_retract_alt >= _deploy_alt &&
alt_m >= _retract_alt &&
_last_height_above_ground < _retract_alt) {
retract();
}
}
_last_height_above_ground = alt_m;
}
// log weight on wheels state

6
libraries/AP_LandingGear/AP_LandingGear.h
View File

@ -84,7 +84,7 @@ public:
static const struct AP_Param::GroupInfo var_info[];
void update();
void update(float height_above_ground_m);
private:
// Parameters
@ -96,13 +96,17 @@ private:
AP_Int8 _pin_deployed_polarity;
AP_Int8 _pin_weight_on_wheels;
AP_Int8 _pin_weight_on_wheels_polarity;
AP_Int16 _deploy_alt;
AP_Int16 _retract_alt;
// internal variables
bool _deployed; // true if the landing gear has been deployed, initialized false
bool _have_changed; // have we changed the servo state?
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;
int16_t _last_height_above_ground;
// debounce
LG_WOW_State wow_state_current = LG_WOW_UNKNOWN;