dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fw pos control: improve mode logic slightly

This commit is contained in:
Thomas Gubler 2014-12-13 12:36:33 +01:00
parent 59ec2401b6
commit 5cd2ee8342
1 changed files with 36 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -200,9 +200,11 @@ private:
ECL_L1_Pos_Controller _l1_control; ECL_L1_Pos_Controller _l1_control;
TECS _tecs; TECS _tecs;
fwPosctrl::mTecs _mTecs; fwPosctrl::mTecs _mTecs;
bool _was_pos_control_mode; enum FW_POSCTRL_MODE {
bool _was_velocity_control_mode; FW_POSCTRL_MODE_AUTO,
bool _was_alt_control_mode; FW_POSCTRL_MODE_POSITION,
FW_POSCTRL_MODE_OTHER
} _control_mode_current; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
struct { struct {
float l1_period; float l1_period;
@ -471,7 +473,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_global_pos_valid(false), _global_pos_valid(false),
_l1_control(), _l1_control(),
_mTecs(), _mTecs(),
_was_pos_control_mode(false) _control_mode_current(FW_POSCTRL_MODE_OTHER)
{ {
_nav_capabilities.turn_distance = 0.0f; _nav_capabilities.turn_distance = 0.0f;
@ -908,20 +910,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* no throttle limit as default */ /* no throttle limit as default */
float throttle_max = 1.0f; float throttle_max = 1.0f;
/* AUTONOMOUS FLIGHT */
if (_control_mode.flag_control_auto_enabled && if (_control_mode.flag_control_auto_enabled &&
pos_sp_triplet.current.valid) { pos_sp_triplet.current.valid) {
/* AUTONOMOUS FLIGHT */
if (!_was_pos_control_mode) { /* Reset integrators if switching to this mode from a other mode in which posctl was not active */
if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
/* reset integrators */ /* reset integrators */
if (_mTecs.getEnabled()) { if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators(); _mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
} }
} }
_control_mode_current = FW_POSCTRL_MODE_AUTO;
_was_pos_control_mode = true;
_was_velocity_control_mode = false;
/* reset hold altitude */ /* reset hold altitude */
_hold_alt = _global_pos.alt; _hold_alt = _global_pos.alt;
@ -1248,35 +1249,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (_control_mode.flag_control_velocity_enabled && } else if (_control_mode.flag_control_velocity_enabled &&
_control_mode.flag_control_altitude_enabled) { _control_mode.flag_control_altitude_enabled) {
/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
const float deadBand = (60.0f/1000.0f); const float deadBand = (60.0f/1000.0f);
const float factor = 1.0f - deadBand; const float factor = 1.0f - deadBand;
if (!_was_velocity_control_mode) { if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt; _hold_alt = _global_pos.alt;
_was_alt_control_mode = false;
} }
_was_velocity_control_mode = true; /* Reset integrators if switching to this mode from a other mode in which posctl was not active */
_was_pos_control_mode = false; if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
}
}
_control_mode_current = FW_POSCTRL_MODE_POSITION;
// Get demanded airspeed // Get demanded airspeed
float altctrl_airspeed = _parameters.airspeed_min + float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) * (_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z; _manual.z;
// Get demanded vertical velocity from pitch control // Get demanded vertical velocity from pitch control
float pitch = 0.0f; static bool was_in_deadband = false;
if (_manual.x > deadBand) { if (_manual.x > deadBand) {
pitch = (_manual.x - deadBand) / factor; float pitch = (_manual.x - deadBand) / factor;
} else if (_manual.x < - deadBand) {
pitch = (_manual.x + deadBand) / factor;
}
if (pitch > 0.0f) {
_hold_alt -= (_parameters.max_climb_rate * dt) * pitch; _hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
_was_alt_control_mode = false; was_in_deadband = false;
} else if (pitch < 0.0f) { } else if (_manual.x < - deadBand) {
float pitch = (_manual.x + deadBand) / factor;
_hold_alt -= (_parameters.max_sink_rate * dt) * pitch; _hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
_was_alt_control_mode = false; was_in_deadband = false;
} else if (!_was_alt_control_mode) { } else if (!was_in_deadband) {
/* store altitude at which manual.x was inside deadBand
* The aircraft should immediately try to fly at this altitude
* as this is what the pilot expects when he moves the stick to the center */
_hold_alt = _global_pos.alt; _hold_alt = _global_pos.alt;
_was_alt_control_mode = true; was_in_deadband = true;
} }
tecs_update_pitch_throttle(_hold_alt, tecs_update_pitch_throttle(_hold_alt,
altctrl_airspeed, altctrl_airspeed,
@ -1292,8 +1303,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
ground_speed, ground_speed,
TECS_MODE_NORMAL); TECS_MODE_NORMAL);
} else { } else {
_was_velocity_control_mode = false; _control_mode_current = FW_POSCTRL_MODE_OTHER;
_was_pos_control_mode = false;
/** MANUAL FLIGHT **/ /** MANUAL FLIGHT **/