From ccf0b7b342c55a6e23c6c859d47e7533fbaa54b6 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Thu, 5 May 2022 21:28:13 +0200 Subject: [PATCH] fw pos ctrl: centralize parameter and state resets --- .../FixedwingPositionControl.cpp | 42 ++++++------------- 1 file changed, 12 insertions(+), 30 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 1cc3e989ad..da781ae284 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -886,12 +886,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const float contr 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) { - update_in_air_states(now); - - _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); @@ -906,10 +900,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const float contr 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; @@ -1411,16 +1401,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { - update_in_air_states(now); - - _att_sp.roll_reset_integral = false; - _att_sp.pitch_reset_integral = false; - _att_sp.yaw_reset_integral = false; - - /* 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 */ @@ -1657,15 +1637,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { - update_in_air_states(now); - - _att_sp.roll_reset_integral = false; - _att_sp.pitch_reset_integral = false; - _att_sp.yaw_reset_integral = false; - - /* 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()); @@ -2400,6 +2371,8 @@ FixedwingPositionControl::Run() set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid); + update_in_air_states(_local_pos.timestamp); + // update lateral guidance timesteps for slewrates if (_param_fw_use_npfg.get()) { _npfg.setDt(control_interval); @@ -2408,7 +2381,16 @@ FixedwingPositionControl::Run() _l1_control.set_dt(control_interval); } - _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder + // restore nominal 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()); + + _att_sp.roll_reset_integral = false; + _att_sp.pitch_reset_integral = false; + _att_sp.yaw_reset_integral = false; + + // by default we don't want yaw to be contoller directly with rudder + _att_sp.fw_control_yaw = false; switch (_control_mode_current) { case FW_POSCTRL_MODE_AUTO: {