fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 14:29:07 +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
parent b3776134b8
commit 1ab9fb22ee
2 changed files with 132 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

187
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,55 +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);
}
/* 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) {
@ -899,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);
@ -909,6 +918,20 @@ 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;
@ -932,16 +955,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;
@ -1043,15 +1056,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;
@ -1088,11 +1094,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;
}
@ -1151,8 +1152,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);
@ -1432,10 +1432,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);
@ -1443,6 +1450,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 */
@ -1676,10 +1687,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);
@ -1687,6 +1705,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 */
@ -1704,9 +1728,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
prev_wp(1) = pos_sp_curr.lon;
}
// Enable tighter altitude control for landings
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
// save time at which we started landing and reset abort_landing
if (_time_started_landing == 0) {
reset_landing_state();
@ -2099,9 +2120,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
@ -2472,11 +2491,21 @@ FixedwingPositionControl::Run()
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_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,