forked from Archive/PX4-Autopilot
fw pos control: improve mode logic slightly
This commit is contained in:
parent
59ec2401b6
commit
5cd2ee8342
|
@ -200,9 +200,11 @@ private:
|
|||
ECL_L1_Pos_Controller _l1_control;
|
||||
TECS _tecs;
|
||||
fwPosctrl::mTecs _mTecs;
|
||||
bool _was_pos_control_mode;
|
||||
bool _was_velocity_control_mode;
|
||||
bool _was_alt_control_mode;
|
||||
enum FW_POSCTRL_MODE {
|
||||
FW_POSCTRL_MODE_AUTO,
|
||||
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 {
|
||||
float l1_period;
|
||||
|
@ -471,7 +473,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
|||
_global_pos_valid(false),
|
||||
_l1_control(),
|
||||
_mTecs(),
|
||||
_was_pos_control_mode(false)
|
||||
_control_mode_current(FW_POSCTRL_MODE_OTHER)
|
||||
{
|
||||
_nav_capabilities.turn_distance = 0.0f;
|
||||
|
||||
|
@ -908,20 +910,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
/* no throttle limit as default */
|
||||
float throttle_max = 1.0f;
|
||||
|
||||
/* AUTONOMOUS FLIGHT */
|
||||
if (_control_mode.flag_control_auto_enabled &&
|
||||
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 */
|
||||
if (_mTecs.getEnabled()) {
|
||||
_mTecs.resetIntegrators();
|
||||
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
|
||||
}
|
||||
}
|
||||
|
||||
_was_pos_control_mode = true;
|
||||
_was_velocity_control_mode = false;
|
||||
_control_mode_current = FW_POSCTRL_MODE_AUTO;
|
||||
|
||||
/* reset hold altitude */
|
||||
_hold_alt = _global_pos.alt;
|
||||
|
@ -1248,35 +1249,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
|
||||
} else if (_control_mode.flag_control_velocity_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 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;
|
||||
_was_alt_control_mode = false;
|
||||
}
|
||||
_was_velocity_control_mode = true;
|
||||
_was_pos_control_mode = false;
|
||||
/* 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 */
|
||||
if (_mTecs.getEnabled()) {
|
||||
_mTecs.resetIntegrators();
|
||||
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
|
||||
}
|
||||
}
|
||||
_control_mode_current = FW_POSCTRL_MODE_POSITION;
|
||||
|
||||
// Get demanded airspeed
|
||||
float altctrl_airspeed = _parameters.airspeed_min +
|
||||
(_parameters.airspeed_max - _parameters.airspeed_min) *
|
||||
_manual.z;
|
||||
|
||||
// Get demanded vertical velocity from pitch control
|
||||
float pitch = 0.0f;
|
||||
static bool was_in_deadband = false;
|
||||
if (_manual.x > deadBand) {
|
||||
pitch = (_manual.x - deadBand) / factor;
|
||||
} else if (_manual.x < - deadBand) {
|
||||
pitch = (_manual.x + deadBand) / factor;
|
||||
}
|
||||
if (pitch > 0.0f) {
|
||||
float pitch = (_manual.x - deadBand) / factor;
|
||||
_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
|
||||
_was_alt_control_mode = false;
|
||||
} else if (pitch < 0.0f) {
|
||||
was_in_deadband = false;
|
||||
} else if (_manual.x < - deadBand) {
|
||||
float pitch = (_manual.x + deadBand) / factor;
|
||||
_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
|
||||
_was_alt_control_mode = false;
|
||||
} else if (!_was_alt_control_mode) {
|
||||
was_in_deadband = false;
|
||||
} 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;
|
||||
_was_alt_control_mode = true;
|
||||
was_in_deadband = true;
|
||||
}
|
||||
tecs_update_pitch_throttle(_hold_alt,
|
||||
altctrl_airspeed,
|
||||
|
@ -1292,8 +1303,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
|||
ground_speed,
|
||||
TECS_MODE_NORMAL);
|
||||
} else {
|
||||
_was_velocity_control_mode = false;
|
||||
_was_pos_control_mode = false;
|
||||
_control_mode_current = FW_POSCTRL_MODE_OTHER;
|
||||
|
||||
/** MANUAL FLIGHT **/
|
||||
|
||||
|
|
Loading…
Reference in New Issue