From 5e3c8d2fa006d3cb316af58778bdd26a6b75001a 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 | 205 +++++++++++------- .../FixedwingPositionControl.hpp | 24 ++ 2 files changed, 149 insertions(+), 80 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 2607a5ca7b..8c0a5e0310 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,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 ¤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) { @@ -902,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); @@ -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) { 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,