Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 03:36:07 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

TECS: move to new control loop architecture

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2020-10-15 08:26:13 +03:00
committed by Roman Bapst
parent 447e14906c
commit 01f891618b
5 changed files with 89 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+6 -8
View File
@@ -98,8 +98,6 @@ FixedwingPositionControl::parameters_update()
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_indicated_airspeed_min(_param_fw_airspd_min.get());
_tecs.set_indicated_airspeed_max(_param_fw_airspd_max.get());
_tecs.set_time_const_throt(_param_fw_t_thro_const.get());
_tecs.set_time_const(_param_fw_t_time_const.get());
_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
_tecs.set_throttle_damp(_param_fw_t_thr_damp.get());
_tecs.set_integrator_gain(_param_fw_t_integ_gain.get());
@@ -108,9 +106,9 @@ FixedwingPositionControl::parameters_update()
_tecs.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get());
_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
_tecs.set_heightrate_p(_param_fw_t_hrate_p.get());
_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
_tecs.set_heightrate_ff(_param_fw_t_hrate_ff.get());
_tecs.set_speedrate_p(_param_fw_t_srate_p.get());
_tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
_tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get());
_tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get());
@@ -639,7 +637,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
_tecs.set_time_const_throt(_param_fw_t_thro_const.get());
//_tecs.set_time_const_throt(_param_fw_t_thro_const.get());
Vector2f curr_wp{0.0f, 0.0f};
Vector2f prev_wp{0.0f, 0.0f};
@@ -845,7 +843,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
// landing airspeed and potentially tighter throttle control) already such that we don't
// have to do this switch (which can cause significant altitude errors) close to the ground.
_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
//_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
mission_airspeed = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
_att_sp.apply_flaps = true;
}
@@ -865,7 +863,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
_att_sp.roll_body = 0.0f;
}
_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
//_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
}
tecs_update_pitch_throttle(now, alt_sp,
@@ -1331,7 +1329,7 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
// Enable tighter throttle control for landings
_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
//_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get());
// save time at which we started landing and reset abort_landing
if (_time_started_landing == 0) {
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+2 -4
View File
@@ -395,7 +395,7 @@ private:
(ParamFloat<px4::params::FW_T_CLMB_MAX>) _param_fw_t_clmb_max,
(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
(ParamFloat<px4::params::FW_T_HRATE_P>) _param_fw_t_hrate_p,
(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
(ParamFloat<px4::params::FW_T_INTEG_GAIN>) _param_fw_t_integ_gain,
(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,
(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
@@ -403,10 +403,8 @@ private:
(ParamFloat<px4::params::FW_T_SINK_MIN>) _param_fw_t_sink_min,
(ParamFloat<px4::params::FW_T_SPD_OMEGA>) _param_fw_t_spd_omega,
(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_fw_t_spdweight,
(ParamFloat<px4::params::FW_T_SRATE_P>) _param_fw_t_srate_p,
(ParamFloat<px4::params::FW_T_TAS_TC>) _param_fw_t_tas_error_tc,
(ParamFloat<px4::params::FW_T_THR_DAMP>) _param_fw_t_thr_damp,
(ParamFloat<px4::params::FW_T_THRO_CONST>) _param_fw_t_thro_const,
(ParamFloat<px4::params::FW_T_TIME_CONST>) _param_fw_t_time_const,
(ParamFloat<px4::params::FW_T_VERT_ACC>) _param_fw_t_vert_acc,
(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
(ParamFloat<px4::params::FW_T_TAS_R_TC>) _param_tas_rate_time_const,
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+9 -43
View File
@@ -520,38 +520,6 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
/**
* TECS time constant
*
* This is the time constant of the TECS control algorithm (in seconds).
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
* @unit s
* @min 1.0
* @max 10.0
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
/**
* TECS Throttle time constant
*
* This is the time constant of the TECS throttle control algorithm (in seconds).
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
* @unit s
* @min 1.0
* @max 10.0
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
/**
* Throttle damping factor
*
@@ -678,15 +646,14 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
/**
* Height rate proportional factor
* Altitude error time constant.
*
* @min 0.0
* @max 1.0
* @min 2.0
* @decimal 2
* @increment 0.05
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f);
/**
* Height rate feed forward
@@ -700,15 +667,14 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.8f);
/**
* Speed rate P factor
* True airspeed error time constant.
*
* @min 0.0
* @max 2.0
* @min 2.0
* @decimal 2
* @increment 0.01
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f);
PARAM_DEFINE_FLOAT(FW_T_TAS_TC, 10.0f);
/**
* Minimum groundspeed
@@ -737,7 +703,7 @@ PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f);
* @group FW L1 Control
*/
PARAM_DEFINE_INT32(FW_POSCTL_INV_ST, 0);
/**
* Specific total energy rate first order filter time constant.
*