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

fw_pos_control_l1 don't store TECS and L1 parameters

Browse Source
This commit is contained in:
Daniel Agar 2018-03-30 18:13:08 -04:00
parent d2abd53692
commit a78ab02144
2 changed files with 140 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

210
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -76,23 +76,23 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_use_terrain_estimate = param_find("FW_LND_USETER");
_parameter_handles.land_airspeed_scale = param_find("FW_LND_AIRSPD_SC");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
_parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX");
_parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX");
_parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF");
_parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP");
_parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN");
_parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC");
_parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA");
_parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA");
_parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR");
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
_parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
_parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF");
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
_parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX");
_parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX");
_parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF");
_parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP");
_parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN");
_parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC");
_parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA");
_parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA");
_parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR");
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
_parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
_parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF");
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
// if vehicle is vtol these handles will be set when we get the vehicle status
_parameter_handles.airspeed_trans = PARAM_INVALID;
@ -113,9 +113,7 @@ FixedwingPositionControl::~FixedwingPositionControl()
int
FixedwingPositionControl::parameters_update()
{
/* L1 control parameters */
param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping));
param_get(_parameter_handles.l1_period, &(_parameters.l1_period));
updateParams();
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
@ -124,13 +122,11 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max));
param_get(_parameter_handles.roll_limit, &(_parameters.roll_limit));
param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
param_get(_parameter_handles.throttle_idle, &(_parameters.throttle_idle));
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
param_get(_parameter_handles.throttle_alt_scale, &(_parameters.throttle_alt_scale));
param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max));
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.man_roll_max_deg, &_parameters.man_roll_max_rad);
@ -143,35 +139,8 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.pitchsp_offset_deg, &_parameters.pitchsp_offset_rad);
_parameters.pitchsp_offset_rad = radians(_parameters.pitchsp_offset_rad);
param_get(_parameter_handles.time_const, &(_parameters.time_const));
param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt));
param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain));
param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit));
param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega));
param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega));
param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation));
param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff));
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
/* check if negative value for 2/3 of flare altitude is set for throttle cut */
if (_parameters.land_thrust_lim_alt_relative < 0.0f) {
_parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative;
}
param_get(_parameter_handles.land_heading_hold_horizontal_distance,
&(_parameters.land_heading_hold_horizontal_distance));
param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg));
@ -179,56 +148,141 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate));
param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale));
// VTOL parameter VTOL_TYPE
if (_parameter_handles.vtol_type != PARAM_INVALID) {
param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
}
// VTOL parameter VT_ARSP_TRANS
if (_parameter_handles.airspeed_trans != PARAM_INVALID) {
param_get(_parameter_handles.airspeed_trans, &_parameters.airspeed_trans);
}
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(radians(_parameters.roll_limit));
_tecs.set_time_const(_parameters.time_const);
_tecs.set_time_const_throt(_parameters.time_const_throt);
_tecs.set_min_sink_rate(_parameters.min_sink_rate);
float v = 0.0f;
// L1 control parameters
if (param_get(_parameter_handles.l1_damping, &v) == PX4_OK) {
_l1_control.set_l1_damping(v);
}
if (param_get(_parameter_handles.l1_period, &v) == PX4_OK) {
_l1_control.set_l1_period(v);
}
if (param_get(_parameter_handles.roll_limit, &v) == PX4_OK) {
_l1_control.set_l1_roll_limit(radians(v));
}
// TECS parameters
param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
_tecs.set_max_sink_rate(_parameters.max_sink_rate);
_tecs.set_throttle_damp(_parameters.throttle_damp);
_tecs.set_throttle_slewrate(_parameters.throttle_slew_max);
_tecs.set_integrator_gain(_parameters.integrator_gain);
_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
_tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega);
_tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation);
param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
_tecs.set_speed_weight(_parameters.speed_weight);
_tecs.set_pitch_damping(_parameters.pitch_damping);
_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
_tecs.set_heightrate_p(_parameters.heightrate_p);
_tecs.set_heightrate_ff(_parameters.heightrate_ff);
_tecs.set_speedrate_p(_parameters.speedrate_p);
/* Update the landing slope */
_landingslope.update(radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative,
_parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
if (param_get(_parameter_handles.time_const, &v) == PX4_OK) {
_tecs.set_time_const(v);
}
/* Update and publish the navigation capabilities */
if (param_get(_parameter_handles.time_const_throt, &v) == PX4_OK) {
_tecs.set_time_const_throt(v);
}
if (param_get(_parameter_handles.min_sink_rate, &v) == PX4_OK) {
_tecs.set_min_sink_rate(v);
}
if (param_get(_parameter_handles.throttle_damp, &v) == PX4_OK) {
_tecs.set_throttle_damp(v);
}
if (param_get(_parameter_handles.integrator_gain, &v) == PX4_OK) {
_tecs.set_integrator_gain(v);
}
if (param_get(_parameter_handles.throttle_slew_max, &v) == PX4_OK) {
_tecs.set_throttle_slewrate(v);
}
if (param_get(_parameter_handles.vertical_accel_limit, &v) == PX4_OK) {
_tecs.set_vertical_accel_limit(v);
}
if (param_get(_parameter_handles.height_comp_filter_omega, &v) == PX4_OK) {
_tecs.set_height_comp_filter_omega(v);
}
if (param_get(_parameter_handles.speed_comp_filter_omega, &v) == PX4_OK) {
_tecs.set_speed_comp_filter_omega(v);
}
if (param_get(_parameter_handles.roll_throttle_compensation, &v) == PX4_OK) {
_tecs.set_roll_throttle_compensation(v);
}
if (param_get(_parameter_handles.pitch_damping, &v) == PX4_OK) {
_tecs.set_pitch_damping(v);
}
if (param_get(_parameter_handles.heightrate_p, &v) == PX4_OK) {
_tecs.set_heightrate_p(v);
}
if (param_get(_parameter_handles.heightrate_ff, &v) == PX4_OK) {
_tecs.set_heightrate_ff(v);
}
if (param_get(_parameter_handles.speedrate_p, &v) == PX4_OK) {
_tecs.set_speedrate_p(v);
}
// Landing slope
float land_slope_angle = 0.0f;
param_get(_parameter_handles.land_slope_angle, &land_slope_angle);
float land_flare_alt_relative = 0.0f;
param_get(_parameter_handles.land_flare_alt_relative, &land_flare_alt_relative);
float land_thrust_lim_alt_relative = 0.0f;
param_get(_parameter_handles.land_thrust_lim_alt_relative, &land_thrust_lim_alt_relative);
float land_H1_virt = 0.0f;
param_get(_parameter_handles.land_H1_virt, &land_H1_virt);
/* check if negative value for 2/3 of flare altitude is set for throttle cut */
if (land_thrust_lim_alt_relative < 0.0f) {
land_thrust_lim_alt_relative = 0.66f * land_flare_alt_relative;
}
_landingslope.update(radians(land_slope_angle), land_flare_alt_relative, land_thrust_lim_alt_relative, land_H1_virt);
// Update and publish the navigation capabilities
_fw_pos_ctrl_status.landing_slope_angle_rad = _landingslope.landing_slope_angle_rad();
_fw_pos_ctrl_status.landing_horizontal_slope_displacement = _landingslope.horizontal_slope_displacement();
_fw_pos_ctrl_status.landing_flare_length = _landingslope.flare_length();
fw_pos_ctrl_status_publish();
updateParams();
/* sanity check parameters */
if (_parameters.airspeed_max < _parameters.airspeed_min ||
_parameters.airspeed_max < 5.0f ||
_parameters.airspeed_min > 100.0f ||
_parameters.airspeed_trim < _parameters.airspeed_min ||
_parameters.airspeed_trim > _parameters.airspeed_max) {
// sanity check parameters
if ((_parameters.airspeed_max < _parameters.airspeed_min) ||
(_parameters.airspeed_max < 5.0f) ||
(_parameters.airspeed_min > 100.0f) ||
(_parameters.airspeed_trim < _parameters.airspeed_min) ||
(_parameters.airspeed_trim > _parameters.airspeed_max)) {
mavlink_log_critical(&_mavlink_log_pub, "Airspeed parameters invalid");

35
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -263,42 +263,24 @@ private:
} _control_mode_current{FW_POSCTRL_MODE_OTHER}; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
struct {
float l1_period;
float l1_damping;
float time_const;
float time_const_throt;
float min_sink_rate;
float max_sink_rate;
float max_climb_rate;
float climbout_diff;
float heightrate_p;
float heightrate_ff;
float speedrate_p;
float throttle_damp;
float integrator_gain;
float vertical_accel_limit;
float height_comp_filter_omega;
float speed_comp_filter_omega;
float roll_throttle_compensation;
float max_climb_rate;
float max_sink_rate;
float speed_weight;
float pitch_damping;
float airspeed_min;
float airspeed_trim;
float airspeed_max;
float airspeed_trans;
int32_t airspeed_disabled;
float pitch_limit_min;
float pitch_limit_max;
float roll_limit;
float throttle_min;
float throttle_max;
float throttle_idle;
float throttle_cruise;
float throttle_slew_max;
float throttle_alt_scale;
float man_roll_max_rad;
@ -308,29 +290,29 @@ private:
float throttle_land_max;
float land_slope_angle;
float land_H1_virt;
float land_flare_alt_relative;
float land_thrust_lim_alt_relative;
float land_heading_hold_horizontal_distance;
float land_flare_pitch_min_deg;
float land_flare_pitch_max_deg;
int32_t land_use_terrain_estimate;
float land_airspeed_scale;
// VTOL
float airspeed_trans;
int32_t vtol_type;
} _parameters{}; ///< local copies of interesting parameters */
struct {
param_t climbout_diff;
param_t l1_period;
param_t l1_damping;
param_t roll_limit;
param_t time_const;
param_t time_const_throt;
param_t min_sink_rate;
param_t max_sink_rate;
param_t max_climb_rate;
param_t climbout_diff;
param_t heightrate_p;
param_t heightrate_ff;
param_t speedrate_p;
@ -351,7 +333,6 @@ private:
param_t pitch_limit_min;
param_t pitch_limit_max;
param_t roll_limit;
param_t throttle_min;
param_t throttle_max;