From a78ab02144b96b307772d75587af65c8bc5e40bd Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 30 Mar 2018 18:13:08 -0400 Subject: [PATCH] fw_pos_control_l1 don't store TECS and L1 parameters --- .../FixedwingPositionControl.cpp | 210 +++++++++++------- .../FixedwingPositionControl.hpp | 35 +-- 2 files changed, 140 insertions(+), 105 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 8306b7b00d..1f918f8445 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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"); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 63b0820ce1..f53e9c2929 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -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;