fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 11:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

TECS: Combine both airspeed and airspeed derivative filters in TECS into one MIMO filter using a steady state Kalman filter.

Browse Source
This commit is contained in:
Konrad
2022-11-08 16:44:58 +01:00
committed by Silvan Fuhrer
parent 08c36612b3
commit f5524fa605
6 changed files with 519 additions and 311 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+27 -25
View File
@@ -134,15 +134,16 @@ FixedwingPositionControl::parameters_update()
_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
_tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get());
_tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
_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_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
_tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.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());
_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
_tecs.set_airspeed_measurement_std_dev(_param_speed_standard_dev.get());
_tecs.set_airspeed_rate_measurement_std_dev(_param_speed_rate_standard_dev.get());
_tecs.set_airspeed_filter_process_std_dev(_param_process_noise_standard_dev.get());
int check_ret = PX4_OK;
@@ -457,11 +458,12 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
}
void
FixedwingPositionControl::tecs_status_publish()
FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
float true_airspeed_derivative_raw, float throttle_trim)
{
tecs_status_s t{};
TECS::DebugOutput debug_output{_tecs.getStatus()};
const TECS::DebugOutput &debug_output{_tecs.getStatus()};
switch (_tecs.tecs_mode()) {
case TECS::ECL_TECS_MODE_NORMAL:
@@ -481,25 +483,25 @@ FixedwingPositionControl::tecs_status_publish()
break;
}
t.altitude_sp = alt_sp;
t.altitude_filtered = debug_output.altitude_sp;
t.height_rate_setpoint = debug_output.control.altitude_rate_control;
t.height_rate = -_local_pos.vz;
t.equivalent_airspeed_sp = equivalent_airspeed_sp;
t.true_airspeed_sp = _eas2tas * equivalent_airspeed_sp;
t.true_airspeed_filtered = debug_output.true_airspeed_filtered;
t.height_rate_setpoint = debug_output.altitude_rate_setpoint;
t.height_rate = debug_output.altitude_rate;
t.true_airspeed_derivative_sp = debug_output.true_airspeed_derivative_control;
t.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
t.true_airspeed_derivative = debug_output.true_airspeed_derivative;
t.total_energy_rate_error = debug_output.total_energy_rate_error;
t.energy_distribution_rate_error = debug_output.energy_balance_rate_error;
t.total_energy_rate_sp = debug_output.total_energy_rate_sp;
t.total_energy_balance_rate_sp = debug_output.energy_balance_rate_sp;
t.true_airspeed_derivative_raw = true_airspeed_derivative_raw;
t.total_energy_rate = debug_output.control.total_energy_rate_estimate;
t.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate;
t.total_energy_rate_sp = debug_output.control.total_energy_rate_sp;
t.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp;
t.throttle_integ = debug_output.control.throttle_integrator;
t.pitch_integ = debug_output.control.pitch_integrator;
t.throttle_sp = _tecs.get_throttle_setpoint();
t.pitch_sp_rad = _tecs.get_pitch_setpoint();
t.throttle_trim = throttle_trim;
t.timestamp = hrt_absolute_time();
@@ -826,7 +828,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
_was_in_air = true;
_time_went_in_air = now;
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
}
/* reset flag when airplane landed */
@@ -834,7 +836,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
_was_in_air = false;
_completed_manual_takeoff = false;
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
}
}
@@ -2314,7 +2316,7 @@ FixedwingPositionControl::Run()
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
break;
}
@@ -2491,12 +2493,12 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
}
// We need an altitude lock to calculate the TECS control
if (!(_local_pos.timestamp > 0)) {
if (_local_pos.timestamp == 0) {
_reinitialize_tecs = true;
}
if (_reinitialize_tecs) {
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
_reinitialize_tecs = false;
}
@@ -2504,7 +2506,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
if (_landed) {
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
}
/* update TECS vehicle state estimates */
@@ -2530,7 +2532,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
-_local_pos.vz,
hgt_rate_sp);
tecs_status_publish();
tecs_status_publish(alt_sp, airspeed_sp, -_local_pos.vz, throttle_trim_comp);
}
float
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+5 -3
View File
@@ -437,7 +437,8 @@ private:
void status_publish();
void landing_status_publish();
void tecs_status_publish();
void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
float throttle_trim);
void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
/**
@@ -822,16 +823,17 @@ private:
(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
(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_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_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,
(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
(ParamFloat<px4::params::FW_T_SPD_STD>) _param_speed_standard_dev,
(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+39 -26
View File
@@ -709,22 +709,50 @@ PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f);
PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
/**
* Complementary filter "omega" parameter for speed
* Airspeed measurement standard deviation for airspeed filter.
*
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse longitudinal acceleration and airspeed to obtain an
* improved airspeed estimate. Increasing this frequency weights the solution
* more towards use of the airspeed sensor, whilst reducing it weights the
* solution more towards use of the accelerometer data.
* This is the measurement standard deviation for the airspeed used in the airspeed filter in TECS.
*
* @unit rad/s
* @min 1.0
* @unit m/s
* @min 0.01
* @max 10.0
* @decimal 1
* @increment 0.5
* @decimal 2
* @increment 0.1
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.2f);
/**
* Airspeed rate measurement standard deviation for airspeed filter.
*
* This is the measurement standard deviation for the airspeed rate used in the airspeed filter in TECS.
*
* @unit m/s^2
* @min 0.01
* @max 10.0
* @decimal 2
* @increment 0.1
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.05f);
/**
* Process noise standard deviation for the airspeed rate in the airspeed filter.
*
* This is the process noise standard deviation in the airspeed filter filter defining the noise in the
* airspeed rate for the constant airspeed rate model. This is used to define how much the airspeed and
* the airspeed rate are filtered. The smaller the value the more the measurements are smoothed with the
* drawback for delays.
*
* @unit m/s^2
* @min 0.01
* @max 10.0
* @decimal 2
* @increment 0.1
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f);
/**
* Roll -> Throttle feedforward
@@ -856,21 +884,6 @@ PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2);
*/
PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f);
/**
* True airspeed rate first order filter time constant.
*
* This filter is applied to the true airspeed rate.
*
* @min 0.0
* @max 2
* @decimal 2
* @increment 0.01
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_TAS_R_TC, 0.2f);
/**
* Specific total energy balance rate feedforward gain.
*