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

FixedWingPositionControl: push altitude setpoint locking into TECS 

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2021-10-22 11:48:12 +03:00 committed by Silvan Fuhrer
parent 296db60a58
commit 6e75b7cffd
2 changed files with 43 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -556,11 +556,11 @@ FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_
}
waypoint_prev = temp_prev;
waypoint_prev.alt = _hold_alt;
waypoint_prev.alt = _current_altitude;
waypoint_prev.valid = true;
waypoint_next = temp_next;
waypoint_next.alt = _hold_alt;
waypoint_next.alt = _current_altitude;
waypoint_next.valid = true;
}
@ -576,8 +576,8 @@ FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt)
return takeoff_alt;
}
void
FixedwingPositionControl::update_desired_altitude(float dt)
float
FixedwingPositionControl::getManualHeightRateSetpoint()
{
/*
* The complete range is -1..+1, so this is 6%
@ -592,17 +592,7 @@ FixedwingPositionControl::update_desired_altitude(float dt)
*/
const float factor = 1.0f - deadBand;
/*
* Reset the hold altitude to the current altitude if the uncertainty
* changes significantly.
* This is to guard against uncommanded altitude changes
* when the altitude certainty increases or decreases.
*/
if (fabsf(_althold_epv - _local_pos.epv) > ALTHOLD_EPV_RESET_THRESH) {
_hold_alt = _current_altitude;
_althold_epv = _local_pos.epv;
}
float height_rate_setpoint = 0.0f;
/*
* Manual control has as convention the rotation around
@ -612,38 +602,18 @@ FixedwingPositionControl::update_desired_altitude(float dt)
if (_manual_control_setpoint_altitude > deadBand) {
/* pitching down */
float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor;
if (pitch * _param_sinkrate_target.get() < _tecs.hgt_rate_setpoint()) {
_hold_alt += (_param_sinkrate_target.get() * dt) * pitch;
_manual_height_rate_setpoint_m_s = pitch * _param_sinkrate_target.get();
_was_in_deadband = false;
}
height_rate_setpoint = pitch * _param_sinkrate_target.get();
} else if (_manual_control_setpoint_altitude < - deadBand) {
/* pitching up */
float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor;
const float climb_rate_target = _param_climbrate_target.get();
if (pitch * _param_climbrate_target.get() > _tecs.hgt_rate_setpoint()) {
_hold_alt += (_param_climbrate_target.get() * dt) * pitch;
_manual_height_rate_setpoint_m_s = pitch * _param_climbrate_target.get();
_was_in_deadband = false;
}
height_rate_setpoint = pitch * climb_rate_target;
} 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 = _current_altitude;
_althold_epv = _local_pos.epv;
_was_in_deadband = true;
_manual_height_rate_setpoint_m_s = NAN;
}
if (_vehicle_status.is_vtol) {
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) {
_hold_alt = _current_altitude;
}
}
return height_rate_setpoint;
}
bool
@ -693,7 +663,6 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
&& !_vehicle_status.in_transition_mode) {
if (_control_mode_current != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
// Need to init because last loop iteration was in a different mode
_hold_alt = _current_altitude;
mavlink_log_critical(&_mavlink_log_pub, "Start loiter with fixed bank angle.\t");
events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical,
"Start loiter with fixed bank angle");
@ -714,7 +683,6 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
} else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) {
if (_control_mode_current != FW_POSCTRL_MODE_MANUAL_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _current_altitude;
_hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw
_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
_yaw_lock_engaged = false;
@ -728,11 +696,6 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
_control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION;
} else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_altitude_enabled) {
if (_control_mode_current != FW_POSCTRL_MODE_MANUAL_POSITION
&& _control_mode_current != FW_POSCTRL_MODE_MANUAL_ALTITUDE) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _current_altitude;
}
_control_mode_current = FW_POSCTRL_MODE_MANUAL_ALTITUDE;
@ -772,9 +735,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
_tecs.reset_state();
}
/* reset hold altitude */
_hold_alt = _current_altitude;
/* reset hold yaw */
_hdg_hold_yaw = _yaw;
@ -886,7 +846,7 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &no
{
// only control altitude and airspeed ("fixed-bank loiter")
tecs_update_pitch_throttle(now, _hold_alt,
tecs_update_pitch_throttle(now, _current_altitude,
_param_fw_airspd_trim.get(),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
@ -918,7 +878,7 @@ FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
// but not letting it drift too far away.
const float descend_rate = -0.5f;
tecs_update_pitch_throttle(now, _hold_alt,
tecs_update_pitch_throttle(now, _current_altitude,
_param_fw_airspd_trim.get(),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
@ -1735,21 +1695,28 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
const Vector2f &ground_speed)
{
/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
_control_position_last_called = now;
/* Get demanded airspeed */
float altctrl_airspeed = get_demanded_airspeed();
/* update desired altitude based on user pitch stick input */
update_desired_altitude(dt);
// if we assume that user is taking off then help by demanding altitude setpoint well above ground
// and set limit to pitch angle to prevent steering into ground
// this will only affect planes and not VTOL
float pitch_limit_min = _param_fw_p_lim_min.get();
do_takeoff_help(&_hold_alt, &pitch_limit_min);
float height_rate_sp = NAN;
float altitude_sp_amsl = _current_altitude;
if (in_takeoff_situation()) {
// if we assume that user is taking off then help by demanding altitude setpoint well above ground
// and set limit to pitch angle to prevent steering into ground
// this will only affect planes and not VTOL
altitude_sp_amsl = _takeoff_ground_alt + _param_fw_clmbout_diff.get();
pitch_limit_min = radians(10.0f);
} else {
height_rate_sp = getManualHeightRateSetpoint();
}
/* throttle limiting */
float throttle_max = _param_fw_thr_max.get();
@ -1758,7 +1725,7 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
throttle_max = 0.0f;
}
tecs_update_pitch_throttle(now, _hold_alt,
tecs_update_pitch_throttle(now, altitude_sp_amsl,
altctrl_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
@ -1768,7 +1735,7 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
false,
pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL,
_manual_height_rate_setpoint_m_s);
height_rate_sp);
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
@ -1792,14 +1759,23 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
const float dt = math::constrain((now - _control_position_last_called) * 1e-6f, 0.01f, 0.05f);
_control_position_last_called = now;
/* update desired altitude based on user pitch stick input */
update_desired_altitude(dt);
// if we assume that user is taking off then help by demanding altitude setpoint well above ground
// and set limit to pitch angle to prevent steering into ground
// this will only affect planes and not VTOL
float pitch_limit_min = _param_fw_p_lim_min.get();
do_takeoff_help(&_hold_alt, &pitch_limit_min);
float height_rate_sp = NAN;
float altitude_sp_amsl = _current_altitude;
if (in_takeoff_situation()) {
// if we assume that user is taking off then help by demanding altitude setpoint well above ground
// and set limit to pitch angle to prevent steering into ground
// this will only affect planes and not VTOL
altitude_sp_amsl = _takeoff_ground_alt + _param_fw_clmbout_diff.get();
pitch_limit_min = radians(10.0f);
} else {
height_rate_sp = getManualHeightRateSetpoint();
}
/* throttle limiting */
float throttle_max = _param_fw_thr_max.get();
@ -1808,7 +1784,7 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
throttle_max = 0.0f;
}
tecs_update_pitch_throttle(now, _hold_alt,
tecs_update_pitch_throttle(now, altitude_sp_amsl,
get_demanded_airspeed(),
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
@ -1818,7 +1794,7 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
false,
pitch_limit_min,
tecs_status_s::TECS_MODE_NORMAL,
_manual_height_rate_setpoint_m_s);
height_rate_sp);
/* heading control */
if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
@ -1962,7 +1938,6 @@ FixedwingPositionControl::Run()
// handle estimator reset events. we only adjust setpoins for manual modes
if (_control_mode.flag_control_manual_enabled) {
if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _alt_reset_counter) {
_hold_alt += -_local_pos.delta_z;
// make TECS accept step in altitude and demanded altitude
_tecs.handle_alt_step(-_local_pos.delta_z, _current_altitude);
}
@ -2075,10 +2050,6 @@ FixedwingPositionControl::Run()
}
case FW_POSCTRL_MODE_OTHER: {
/* do not publish the setpoint */
// reset hold altitude
_hold_alt = _current_altitude;
/* reset landing and takeoff state */
if (!_last_manual) {
reset_landing_state();

6
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -176,14 +176,10 @@ private:
map_projection_reference_s _global_local_proj_ref{};
float _global_local_alt0{NAN};
float _hold_alt{0.0f}; ///< hold altitude for altitude mode
float _manual_height_rate_setpoint_m_s{NAN};
float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched
float _hdg_hold_yaw{0.0f}; ///< hold heading for velocity mode
bool _hdg_hold_enabled{false}; ///< heading hold enabled
bool _yaw_lock_engaged{false}; ///< yaw is locked for heading hold
float _althold_epv{0.0f}; ///< the position estimate accuracy when engaging alt hold
bool _was_in_deadband{false}; ///< wether the last stick input was in althold deadband
float _min_current_sp_distance_xy{FLT_MAX};
@ -309,6 +305,8 @@ private:
*/
float get_terrain_altitude_takeoff(float takeoff_alt);
float getManualHeightRateSetpoint();
/**
* Check if we are in a takeoff situation
*/