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

fw pos ctrl: centralize parameter and state resets

Browse Source
This commit is contained in:
Thomas Stastny
2022-06-08 08:44:30 -05:00
parent ccfbbb553a
commit cbf0fe8803
1 changed files with 12 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+12 -30
View File
@@ -879,12 +879,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, 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 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; position_setpoint_s current_sp = pos_sp_curr;
move_position_setpoint_for_vtol_transition(current_sp); move_position_setpoint_for_vtol_transition(current_sp);
@@ -899,10 +893,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.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());
switch (position_sp_type) { switch (position_sp_type) {
case position_setpoint_s::SETPOINT_TYPE_IDLE: case position_setpoint_s::SETPOINT_TYPE_IDLE:
_att_sp.thrust_body[0] = 0.0f; _att_sp.thrust_body[0] = 0.0f;
@@ -1409,16 +1399,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 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_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) */ /* current waypoint (the one currently heading for) */
Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon); Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
Vector2d prev_wp{0, 0}; /* previous waypoint */ Vector2d prev_wp{0, 0}; /* previous waypoint */
@@ -1656,15 +1636,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 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_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 // Enable tighter altitude control for landings
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); _tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
@@ -2402,6 +2373,8 @@ FixedwingPositionControl::Run()
set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid); 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 // update lateral guidance timesteps for slewrates
if (_param_fw_use_npfg.get()) { if (_param_fw_use_npfg.get()) {
_npfg.setDt(control_interval); _npfg.setDt(control_interval);
@@ -2410,7 +2383,16 @@ FixedwingPositionControl::Run()
_l1_control.set_dt(control_interval); _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) { switch (_control_mode_current) {
case FW_POSCTRL_MODE_AUTO: { case FW_POSCTRL_MODE_AUTO: {