fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-12 17:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fxedwingPositionControl: Add slew rate at the end for all mode instead inside each

Browse Source
This commit is contained in:
Konrad
2023-11-29 17:16:45 +01:00
committed by Silvan Fuhrer
parent ae3aee3402
commit d1b8a2e8d5
4 changed files with 13 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+11 -14
View File
@@ -81,6 +81,9 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
parameters_update();
_airspeed_slew_rate_controller.setForcedValue(_performance_model.getCalibratedTrimAirspeed());
_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
_roll_slew_rate.setForcedValue(0.f);
}
FixedwingPositionControl::~FixedwingPositionControl()
@@ -106,6 +109,8 @@ FixedwingPositionControl::parameters_update()
_performance_model.updateParameters();
_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
// VTOL parameter VT_ARSP_TRANS
if (_param_handle_airspeed_trans != PARAM_INVALID) {
param_get(_param_handle_airspeed_trans, &_param_airspeed_trans);
@@ -124,7 +129,6 @@ FixedwingPositionControl::parameters_update()
_npfg.setRollTimeConst(_param_npfg_roll_time_const.get());
_npfg.setSwitchDistanceMultiplier(_param_npfg_switch_distance_multiplier.get());
_npfg.setRollLimit(radians(_param_fw_r_lim.get()));
_npfg.setRollSlewRate(radians(_param_fw_pn_r_slew_max.get()));
_npfg.setPeriodSafetyFactor(_param_npfg_period_safety_factor.get());
// TECS parameters
@@ -2120,16 +2124,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
_hdg_hold_enabled = false;
_yaw_lock_engaged = false;
// do slew rate limiting on roll if enabled
float roll_sp_new = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
const float roll_rate_slew_rad = radians(_param_fw_pn_r_slew_max.get());
if (control_interval > 0.f && roll_rate_slew_rad > 0.f) {
roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * control_interval,
_att_sp.roll_body + roll_rate_slew_rad * control_interval);
}
_att_sp.roll_body = roll_sp_new;
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
}
@@ -2349,9 +2344,6 @@ FixedwingPositionControl::Run()
update_in_air_states(_local_pos.timestamp);
// update lateral guidance timesteps for slewrates
_npfg.setDt(control_interval);
// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
@@ -2451,6 +2443,9 @@ FixedwingPositionControl::Run()
_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_climb_rate_enabled) {
// roll slew rate
_att_sp.roll_body = _roll_slew_rate.update(_att_sp.roll_body, control_interval);
const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
q.copyTo(_att_sp.q_d);
@@ -2465,6 +2460,8 @@ FixedwingPositionControl::Run()
}
}
} else {
_roll_slew_rate.setForcedValue(_roll);
}
// if there's any change in landing gear setpoint publish it