fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 19:19:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw pos ctrl: fix state switching logic for takeoff and landing

Browse Source
This commit is contained in:
Thomas Stastny 2022-05-04 16:08:51 +02:00 committed by Daniel Agar
parent 90998837ec
commit 5e3c8d2fa0
2 changed files with 149 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -755,19 +755,28 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
_control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// TAKEOFF: handle like a regular POSITION setpoint if already flying
if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
_control_mode_current = FW_POSCTRL_MODE_AUTO;
// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
} else {
_control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF;
}
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND
&& !_vehicle_status.in_transition_mode) {
_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING;
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
if (!_vehicle_status.in_transition_mode) {
_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING;
} else {
// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
} else {
_control_mode_current = FW_POSCTRL_MODE_AUTO;
@ -827,58 +836,36 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
}
void
FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
{
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
MAX_AUTO_TIMESTEP);
_last_time_position_control_called = now;
if (_param_fw_use_npfg.get()) {
_npfg.setDt(dt);
} else {
_l1_control.set_dt(dt);
}
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // by default we don't use flaps
/* save time when airplane is in air */
if (!_was_in_air && !_landed) {
_was_in_air = true;
_time_went_in_air = now;
_takeoff_ground_alt = _current_altitude;
_takeoff_ground_alt = _current_altitude; // XXX: is this really a good idea?
}
/* reset flag when airplane landed */
if (_landed) {
_was_in_air = 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 */
_tecs.reset_state();
}
float
FixedwingPositionControl::update_position_control_mode_timestep(const hrt_abstime now)
{
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
MAX_AUTO_TIMESTEP);
_last_time_position_control_called = now;
/* reset hold yaw */
_hdg_hold_yaw = _yaw;
return dt;
}
/* get circle mode */
const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
/* Initialize attitude controller integrator reset flags to 0 */
_att_sp.roll_reset_integral = false;
_att_sp.pitch_reset_integral = false;
_att_sp.yaw_reset_integral = false;
position_setpoint_s current_sp = pos_sp_curr;
void
FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp)
{
// TODO: velocity, altitude, or just a heading hold position mode should be used for this, not position
// shifting hacks
if (_vehicle_status.in_transition_to_fw) {
@ -902,6 +889,25 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
_transition_waypoint(0) = static_cast<double>(NAN);
_transition_waypoint(1) = static_cast<double>(NAN);
}
}
void
FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
{
const float dt = update_position_control_mode_timestep(now);
update_in_air_states(now);
_hdg_hold_yaw = _yaw;
_att_sp.roll_reset_integral = false;
_att_sp.pitch_reset_integral = false;
_att_sp.yaw_reset_integral = false;
position_setpoint_s current_sp = pos_sp_curr;
move_position_setpoint_for_vtol_transition(current_sp);
const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp);
@ -912,11 +918,26 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
publishOrbitStatus(current_sp);
}
// update lateral guidance timesteps for slewrates
if (_param_fw_use_npfg.get()) {
_npfg.setDt(dt);
} else {
_l1_control.set_dt(dt);
}
const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
switch (position_sp_type) {
case position_setpoint_s::SETPOINT_TYPE_IDLE:
_att_sp.thrust_body[0] = 0.0f;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = radians(_param_fw_psp_off.get());
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
break;
case position_setpoint_s::SETPOINT_TYPE_POSITION:
@ -932,16 +953,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
break;
}
/* reset landing state */
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
reset_landing_state();
}
/* reset takeoff/launch state */
if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
reset_takeoff_state();
}
if (was_circle_mode && !_l1_control.circle_mode()) {
/* just kicked out of loiter, reset roll integrals */
_att_sp.roll_reset_integral = true;
@ -991,6 +1002,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &no
}
_att_sp.pitch_body = get_tecs_pitch();
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
}
void
@ -1019,6 +1032,8 @@ FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
_att_sp.pitch_body = get_tecs_pitch();
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
}
uint8_t
@ -1037,15 +1052,8 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
uint8_t position_sp_type = setpoint_type;
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// TAKEOFF: handle like a regular POSITION setpoint if already flying
if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
|| pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
|| pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
float dist_xy = -1.f;
float dist_z = -1.f;
@ -1082,11 +1090,6 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
}
}
// set to type position during VTOL transitions in Land mode (to not start FW landing logic)
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _vehicle_status.in_transition_mode) {
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
}
return position_sp_type;
}
@ -1145,8 +1148,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
// Altitude first order hold (FOH)
if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) &&
((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) ||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF))
(pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER))
) {
const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
pos_sp_prev.lat, pos_sp_prev.lon);
@ -1206,6 +1208,8 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
tecs_update_pitch_throttle(now, position_sp_alt,
target_airspeed,
radians(_param_fw_p_lim_min.get()),
@ -1269,6 +1273,8 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
_att_sp.yaw_body = _yaw;
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
tecs_update_pitch_throttle(now, position_sp_alt,
target_airspeed,
radians(_param_fw_p_lim_min.get()),
@ -1356,7 +1362,10 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
// have to do this switch (which can cause significant altitude errors) close to the ground.
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
airspeed_sp = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
_att_sp.apply_flaps = true;
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
} else {
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
}
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt);
@ -1415,10 +1424,17 @@ void
FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
MAX_AUTO_TIMESTEP);
_last_time_position_control_called = now;
const float dt = update_position_control_mode_timestep(now);
update_in_air_states(now);
_hdg_hold_yaw = _yaw;
_att_sp.roll_reset_integral = false;
_att_sp.pitch_reset_integral = false;
_att_sp.yaw_reset_integral = false;
// update lateral guidance timesteps for slewrates
if (_param_fw_use_npfg.get()) {
_npfg.setDt(dt);
@ -1426,6 +1442,10 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
_l1_control.set_dt(dt);
}
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
/* current waypoint (the one currently heading for) */
Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
Vector2d prev_wp{0, 0}; /* previous waypoint */
@ -1656,10 +1676,17 @@ void
FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
MAX_AUTO_TIMESTEP);
_last_time_position_control_called = now;
const float dt = update_position_control_mode_timestep(now);
update_in_air_states(now);
_hdg_hold_yaw = _yaw;
_att_sp.roll_reset_integral = false;
_att_sp.pitch_reset_integral = false;
_att_sp.yaw_reset_integral = false;
// update lateral guidance timesteps for slewrates
if (_param_fw_use_npfg.get()) {
_npfg.setDt(dt);
@ -1667,6 +1694,12 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
_l1_control.set_dt(dt);
}
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
// Enable tighter altitude control for landings
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
/* current waypoint (the one currently heading for) */
Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
Vector2d prev_wp{0, 0}; /* previous waypoint */
@ -2074,9 +2107,7 @@ void
FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos,
const Vector2f &ground_speed)
{
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
MAX_AUTO_TIMESTEP);
_last_time_position_control_called = now;
const float dt = update_position_control_mode_timestep(now);
// 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
@ -2391,6 +2422,8 @@ FixedwingPositionControl::Run()
set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
switch (_control_mode_current) {
case FW_POSCTRL_MODE_AUTO: {
control_auto(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
@ -2437,11 +2470,23 @@ FixedwingPositionControl::Run()
_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_tecs.reset_state();
break;
}
}
if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING) {
reset_landing_state();
}
if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF) {
reset_takeoff_state();
}
if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {
if (_control_mode.flag_control_manual_enabled) {

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

@ -350,6 +350,30 @@ private:
* @param dt Time step
*/
void update_desired_altitude(float dt);
/**
* @brief Updates timing information for landed and in-air states.
*
* @param now Current system time [us]
*/
void update_in_air_states(const hrt_abstime now);
/**
* @brief Updates the time since the last position control call.
*
* @param now Current system time [us]
* @return Time since last position control call [s]
*/
float update_position_control_mode_timestep(const hrt_abstime now);
/**
* @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in a VTOL
* transition.
*
* @param[in,out] current_sp current position setpoint
*/
void move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp);
uint8_t handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr);
void control_auto(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,