fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 14:57:35 +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-05-05 21:28:13 +02:00
parent 1af0c21ea9
commit ccf0b7b342
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
@@ -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: {