mirror of https://github.com/ArduPilot/ardupilot
AP_LandingGear: added height based triggering
very useful for manual takeoffs and landings
This commit is contained in:
parent
6b5088207b
commit
486ecd6148
|
@ -4,6 +4,7 @@
|
||||||
#include <SRV_Channel/SRV_Channel.h>
|
#include <SRV_Channel/SRV_Channel.h>
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <DataFlash/DataFlash.h>
|
#include <DataFlash/DataFlash.h>
|
||||||
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
@ -63,6 +64,24 @@ const AP_Param::GroupInfo AP_LandingGear::var_info[] = {
|
||||||
// @Values: 0:Low,1:High
|
// @Values: 0:Low,1:High
|
||||||
// @User: Standard
|
// @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, 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
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
@ -121,6 +140,9 @@ void AP_LandingGear::deploy()
|
||||||
|
|
||||||
// set deployed flag
|
// set deployed flag
|
||||||
_deployed = true;
|
_deployed = true;
|
||||||
|
_have_changed = true;
|
||||||
|
|
||||||
|
gcs().send_text(MAV_SEVERITY_INFO, "LandingGear: DEPLOY");
|
||||||
}
|
}
|
||||||
|
|
||||||
/// retract - retract landing gear
|
/// retract - retract landing gear
|
||||||
|
@ -131,6 +153,9 @@ void AP_LandingGear::retract()
|
||||||
|
|
||||||
// reset deployed flag
|
// reset deployed flag
|
||||||
_deployed = false;
|
_deployed = false;
|
||||||
|
_have_changed = true;
|
||||||
|
|
||||||
|
gcs().send_text(MAV_SEVERITY_INFO, "LandingGear: RETRACT");
|
||||||
}
|
}
|
||||||
|
|
||||||
bool AP_LandingGear::deployed()
|
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;
|
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) {
|
if (_pin_weight_on_wheels == -1) {
|
||||||
last_wow_event_ms = 0;
|
last_wow_event_ms = 0;
|
||||||
|
@ -208,6 +233,30 @@ void AP_LandingGear::update()
|
||||||
|
|
||||||
gear_state_current = gear_state_new;
|
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
|
// log weight on wheels state
|
||||||
|
|
|
@ -84,7 +84,7 @@ public:
|
||||||
|
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
void update();
|
void update(float height_above_ground_m);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// Parameters
|
// Parameters
|
||||||
|
@ -96,13 +96,17 @@ private:
|
||||||
AP_Int8 _pin_deployed_polarity;
|
AP_Int8 _pin_deployed_polarity;
|
||||||
AP_Int8 _pin_weight_on_wheels;
|
AP_Int8 _pin_weight_on_wheels;
|
||||||
AP_Int8 _pin_weight_on_wheels_polarity;
|
AP_Int8 _pin_weight_on_wheels_polarity;
|
||||||
|
AP_Int16 _deploy_alt;
|
||||||
|
AP_Int16 _retract_alt;
|
||||||
|
|
||||||
// internal variables
|
// internal variables
|
||||||
bool _deployed; // true if the landing gear has been deployed, initialized false
|
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_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 _deploy_pin_state;
|
||||||
bool _weight_on_wheels_pin_state;
|
bool _weight_on_wheels_pin_state;
|
||||||
|
int16_t _last_height_above_ground;
|
||||||
|
|
||||||
// debounce
|
// debounce
|
||||||
LG_WOW_State wow_state_current = LG_WOW_UNKNOWN;
|
LG_WOW_State wow_state_current = LG_WOW_UNKNOWN;
|
||||||
|
|
Loading…
Reference in New Issue