From 1ab9fb22ee670902fab6d5d779441b8048cfadaf Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Wed, 4 May 2022 16:08:51 +0200 Subject: [PATCH] fw pos ctrl: fix state switching logic for takeoff and landing --- .../FixedwingPositionControl.cpp | 187 ++++++++++-------- .../FixedwingPositionControl.hpp | 24 +++ 2 files changed, 132 insertions(+), 79 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 941fa233d7..1c3d4355af 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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 ¤t_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(NAN); _transition_waypoint(1) = static_cast(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) { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index beeb910c8f..ba1b987c10 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -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 ¤t_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,