From 3dc23afb3e1dca550ef017f17b278a88be749583 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 28 Oct 2019 21:15:38 -0400 Subject: [PATCH] fw_pos_control_l1: move to px4::params --- .../FixedwingPositionControl.cpp | 450 ++++++------------ .../FixedwingPositionControl.hpp | 174 +++---- 2 files changed, 211 insertions(+), 413 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 89dadaa52d..f7ba1af975 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -44,75 +44,18 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : _runway_takeoff(this) { if (vtol) { - _parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS"); + _param_handle_airspeed_trans = param_find("VT_ARSP_TRANS"); // VTOL parameter VTOL_TYPE int32_t vt_type = -1; param_get(param_find("VT_TYPE"), &vt_type); _vtol_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); - - } else { - _parameter_handles.airspeed_trans = PARAM_INVALID; } // limit to 50 Hz _global_pos_sub.set_interval_ms(20); - _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); - _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); - _parameter_handles.roll_slew_deg_sec = param_find("FW_L1_R_SLEW_MAX"); - - _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); - _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); - _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); - _parameter_handles.airspeed_disabled = param_find("FW_ARSP_MODE"); - - _parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN"); - _parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX"); - _parameter_handles.roll_limit = param_find("FW_R_LIM"); - _parameter_handles.throttle_min = param_find("FW_THR_MIN"); - _parameter_handles.throttle_max = param_find("FW_THR_MAX"); - _parameter_handles.throttle_idle = param_find("FW_THR_IDLE"); - _parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX"); - _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); - _parameter_handles.throttle_alt_scale = param_find("FW_THR_ALT_SCL"); - _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); - _parameter_handles.man_roll_max_deg = param_find("FW_MAN_R_MAX"); - _parameter_handles.man_pitch_max_deg = param_find("FW_MAN_P_MAX"); - _parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF"); - _parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF"); - - _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); - _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); - _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); - _parameter_handles.land_flare_pitch_min_deg = param_find("FW_LND_FL_PMIN"); - _parameter_handles.land_flare_pitch_max_deg = param_find("FW_LND_FL_PMAX"); - _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); - _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); - _parameter_handles.land_use_terrain_estimate = param_find("FW_LND_USETER"); - _parameter_handles.land_early_config_change = param_find("FW_LND_EARLYCFG"); - _parameter_handles.land_airspeed_scale = param_find("FW_LND_AIRSPD_SC"); - _parameter_handles.land_throtTC_scale = param_find("FW_LND_THRTC_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.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.loiter_radius = param_find("NAV_LOITER_RAD"); - /* fetch initial parameter values */ parameters_update(); } @@ -138,165 +81,56 @@ FixedwingPositionControl::parameters_update() { updateParams(); - param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); - param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); - param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); - param_get(_parameter_handles.airspeed_disabled, &(_parameters.airspeed_disabled)); - - 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.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_land_max, &(_parameters.throttle_land_max)); - - param_get(_parameter_handles.man_roll_max_deg, &_parameters.man_roll_max_rad); - _parameters.man_roll_max_rad = radians(_parameters.man_roll_max_rad); - param_get(_parameter_handles.man_pitch_max_deg, &_parameters.man_pitch_max_rad); - _parameters.man_pitch_max_rad = radians(_parameters.man_pitch_max_rad); - - param_get(_parameter_handles.rollsp_offset_deg, &_parameters.rollsp_offset_rad); - _parameters.rollsp_offset_rad = radians(_parameters.rollsp_offset_rad); - param_get(_parameter_handles.pitchsp_offset_deg, &_parameters.pitchsp_offset_rad); - _parameters.pitchsp_offset_rad = radians(_parameters.pitchsp_offset_rad); - - param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); - - 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)); - param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); - param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); - param_get(_parameter_handles.land_early_config_change, &(_parameters.land_early_config_change)); - param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale)); - param_get(_parameter_handles.land_throtTC_scale, &(_parameters.land_throtTC_scale)); - param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); - // VTOL parameter VT_ARSP_TRANS - if (_parameter_handles.airspeed_trans != PARAM_INVALID) { - param_get(_parameter_handles.airspeed_trans, &_parameters.airspeed_trans); + if (_param_handle_airspeed_trans != PARAM_INVALID) { + param_get(_param_handle_airspeed_trans, &_param_airspeed_trans); } - - 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)); - } - - if (param_get(_parameter_handles.roll_slew_deg_sec, &v) == PX4_OK) { - _l1_control.set_roll_slew_rate(radians(v)); - } + _l1_control.set_l1_damping(_param_fw_l1_damping.get()); + _l1_control.set_l1_period(_param_fw_l1_period.get()); + _l1_control.set_l1_roll_limit(radians(_param_fw_r_lim.get())); + _l1_control.set_roll_slew_rate(radians(_param_fw_l1_r_slew_max.get())); // 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); - - param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); - _tecs.set_speed_weight(_parameters.speed_weight); - - _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); - _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); - - if (param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt)) == PX4_OK) { - _tecs.set_time_const_throt(_parameters.time_const_throt); - } - - if (param_get(_parameter_handles.time_const, &v) == PX4_OK) { - _tecs.set_time_const(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.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); - } - + _tecs.set_max_climb_rate(_param_fw_t_clmb_max.get()); + _tecs.set_max_sink_rate(_param_fw_t_sink_max.get()); + _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_time_const.get()); + _tecs.set_time_const(_param_fw_t_thro_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()); + _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_heightrate_p(_param_fw_t_hrate_p.get()); + _tecs.set_heightrate_ff(_param_fw_t_hrate_ff.get()); + _tecs.set_speedrate_p(_param_fw_t_srate_p.get()); // 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 */ + float land_thrust_lim_alt_relative = _param_fw_lnd_tlalt.get(); + if (land_thrust_lim_alt_relative < 0.0f) { - land_thrust_lim_alt_relative = 0.66f * land_flare_alt_relative; + land_thrust_lim_alt_relative = 0.66f * _param_fw_lnd_flalt.get(); } - _landingslope.update(radians(land_slope_angle), land_flare_alt_relative, land_thrust_lim_alt_relative, land_H1_virt); + _landingslope.update(radians(_param_fw_lnd_ang.get()), _param_fw_lnd_flalt.get(), land_thrust_lim_alt_relative, + _param_fw_lnd_hvirt.get()); landing_status_publish(); // 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)) { + if ((_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) || + (_param_fw_airspd_max.get() < 5.0f) || + (_param_fw_airspd_min.get() > 100.0f) || + (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get()) || + (_param_fw_airspd_trim.get() > _param_fw_airspd_max.get())) { mavlink_log_critical(&_mavlink_log_pub, "Airspeed parameters invalid"); @@ -337,7 +171,7 @@ FixedwingPositionControl::airspeed_poll() { bool airspeed_valid = _airspeed_valid; - if (!_parameters.airspeed_disabled && _airspeed_validated_sub.update()) { + if ((_param_fw_arsp_mode.get() == 0) && _airspeed_validated_sub.update()) { const airspeed_validated_s &airspeed_validated = _airspeed_validated_sub.get(); _eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed @@ -397,14 +231,14 @@ FixedwingPositionControl::get_demanded_airspeed() // neutral throttle corresponds to trim airspeed if (_manual.z < 0.5f) { // lower half of throttle is min to trim airspeed - altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_trim - _parameters.airspeed_min) * + altctrl_airspeed = _param_fw_airspd_min.get() + + (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) * _manual.z * 2; } else { // upper half of throttle is trim to max airspeed - altctrl_airspeed = _parameters.airspeed_trim + - (_parameters.airspeed_max - _parameters.airspeed_trim) * + altctrl_airspeed = _param_fw_airspd_trim.get() + + (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) * (_manual.z * 2 - 1); } @@ -429,12 +263,12 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const * Vsacc = Vs * sqrt(n) * */ - float adjusted_min_airspeed = _parameters.airspeed_min; + float adjusted_min_airspeed = _param_fw_airspd_min.get(); if (_airspeed_valid && PX4_ISFINITE(_att_sp.roll_body)) { - adjusted_min_airspeed = constrain(_parameters.airspeed_min / sqrtf(cosf(_att_sp.roll_body)), - _parameters.airspeed_min, _parameters.airspeed_max); + adjusted_min_airspeed = constrain(_param_fw_airspd_min.get() / sqrtf(cosf(_att_sp.roll_body)), + _param_fw_airspd_min.get(), _param_fw_airspd_max.get()); } // groundspeed undershoot @@ -451,14 +285,14 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const * not exceeded) travels towards a waypoint (and is not pushed more and more away * by wind). Not countering this would lead to a fly-away. */ - if (ground_speed_body < _groundspeed_min.get()) { - airspeed_demand += max(_groundspeed_min.get() - ground_speed_body, 0.0f); + if (ground_speed_body < _param_fw_gnd_spd_min.get()) { + airspeed_demand += max(_param_fw_gnd_spd_min.get() - ground_speed_body, 0.0f); } } // add minimum ground speed undershoot (only non-zero in presence of sufficient wind) // sanity check: limit to range - return constrain(airspeed_demand, adjusted_min_airspeed, _parameters.airspeed_max); + return constrain(airspeed_demand, adjusted_min_airspeed, _param_fw_airspd_max.get()); } void @@ -649,13 +483,13 @@ FixedwingPositionControl::update_desired_altitude(float dt) if (_manual.x > deadBand) { /* pitching down */ float pitch = -(_manual.x - deadBand) / factor; - _hold_alt += (_parameters.max_sink_rate * dt) * pitch; + _hold_alt += (_param_fw_t_sink_max.get() * dt) * pitch; _was_in_deadband = false; } else if (_manual.x < - deadBand) { /* pitching up */ float pitch = -(_manual.x + deadBand) / factor; - _hold_alt += (_parameters.max_climb_rate * dt) * pitch; + _hold_alt += (_param_fw_t_clmb_max.get() * dt) * pitch; _was_in_deadband = false; climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH); @@ -689,7 +523,7 @@ FixedwingPositionControl::in_takeoff_situation() const hrt_abstime delta_takeoff = 10_s; return (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff) - && (_global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff); + && (_global_pos.alt <= _takeoff_ground_alt + _param_fw_clmbout_diff.get()); } void @@ -697,7 +531,7 @@ FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_lim { /* demand "climbout_diff" m above ground if user switched into this mode during takeoff */ if (in_takeoff_situation()) { - *hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff; + *hold_altitude = _takeoff_ground_alt + _param_fw_clmbout_diff.get(); *pitch_limit_min = radians(10.0f); } } @@ -779,8 +613,8 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto bool was_circle_mode = _l1_control.circle_mode(); /* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */ - _tecs.set_speed_weight(_parameters.speed_weight); - _tecs.set_time_const_throt(_parameters.time_const_throt); + _tecs.set_speed_weight(_param_fw_t_spdweight.get()); + _tecs.set_time_const_throt(_param_fw_t_thro_const.get()); /* current waypoint (the one currently heading for) */ Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon); @@ -806,7 +640,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto prev_wp(1) = (float)pos_sp_curr.lon; } - float mission_airspeed = _parameters.airspeed_trim; + float mission_airspeed = _param_fw_airspd_trim.get(); if (PX4_ISFINITE(pos_sp_curr.cruising_speed) && pos_sp_curr.cruising_speed > 0.1f) { @@ -814,7 +648,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto mission_airspeed = pos_sp_curr.cruising_speed; } - float mission_throttle = _parameters.throttle_cruise; + float mission_throttle = _param_fw_thr_cruise.get(); if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) && pos_sp_curr.cruising_throttle > 0.01f) { @@ -835,13 +669,13 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto tecs_update_pitch_throttle(pos_sp_curr.alt, calculate_target_airspeed(mission_airspeed, ground_speed), - radians(_parameters.pitch_limit_min) - _parameters.pitchsp_offset_rad, - radians(_parameters.pitch_limit_max) - _parameters.pitchsp_offset_rad, - _parameters.throttle_min, - _parameters.throttle_max, + radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()), + radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()), + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), mission_throttle, false, - radians(_parameters.pitch_limit_min)); + radians(_param_fw_p_lim_min.get())); } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { @@ -850,7 +684,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto uint8_t loiter_direction = pos_sp_curr.loiter_direction; if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) { - loiter_radius = _parameters.loiter_radius; + loiter_radius = _param_nav_loiter_rad.get(); loiter_direction = (loiter_radius > 0) ? 1 : -1; } @@ -863,22 +697,22 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto float alt_sp = pos_sp_curr.alt; if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid - && _l1_control.circle_mode() && _parameters.land_early_config_change == 1) { + && _l1_control.circle_mode() && _param_fw_lnd_earlycfg.get()) { // 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(_parameters.land_throtTC_scale * _parameters.time_const_throt); - mission_airspeed = _parameters.land_airspeed_scale * _parameters.airspeed_min; + _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; } if (in_takeoff_situation()) { - alt_sp = max(alt_sp, _takeoff_ground_alt + _parameters.climbout_diff); + alt_sp = max(alt_sp, _takeoff_ground_alt + _param_fw_clmbout_diff.get()); _att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f)); } if (_land_abort) { - if (pos_sp_curr.alt - _global_pos.alt < _parameters.climbout_diff) { + if (pos_sp_curr.alt - _global_pos.alt < _param_fw_clmbout_diff.get()) { // aborted landing complete, normal loiter over landing point abort_landing(false); @@ -887,18 +721,18 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _att_sp.roll_body = 0.0f; } - _tecs.set_time_const_throt(_parameters.land_throtTC_scale * _parameters.time_const_throt); + _tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get()); } tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(mission_airspeed, ground_speed), - radians(_parameters.pitch_limit_min) - _parameters.pitchsp_offset_rad, - radians(_parameters.pitch_limit_max) - _parameters.pitchsp_offset_rad, - _parameters.throttle_min, - _parameters.throttle_max, - _parameters.throttle_cruise, + radians(_param_fw_p_lim_min.get()) - radians(_param_fw_psp_off.get()), + radians(_param_fw_p_lim_max.get()) - radians(_param_fw_psp_off.get()), + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), + _param_fw_thr_cruise.get(), false, - radians(_parameters.pitch_limit_min)); + radians(_param_fw_p_lim_min.get())); } else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND) { control_landing(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); @@ -936,7 +770,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto /* reset setpoints from other modes (auto) otherwise we won't * level out without new manual input */ - _att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad; + _att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get()); _att_sp.yaw_body = 0; } @@ -950,11 +784,11 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto // if we assume that user is taking off then help by demanding altitude setpoint well above ground // and set limit to pitch angle to prevent steering into ground // this will only affect planes and not VTOL - float pitch_limit_min = _parameters.pitch_limit_min; + float pitch_limit_min = _param_fw_p_lim_min.get(); do_takeoff_help(&_hold_alt, &pitch_limit_min); /* throttle limiting */ - throttle_max = _parameters.throttle_max; + throttle_max = _param_fw_thr_max.get(); if (_vehicle_land_detected.landed && (fabsf(_manual.z) < THROTTLE_THRESH)) { throttle_max = 0.0f; @@ -962,11 +796,11 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, - radians(_parameters.pitch_limit_min), - radians(_parameters.pitch_limit_max), - _parameters.throttle_min, + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), throttle_max, - _parameters.throttle_cruise, + _param_fw_thr_cruise.get(), climbout_requested, climbout_requested ? radians(10.0f) : pitch_limit_min, tecs_status_s::TECS_MODE_NORMAL); @@ -1029,7 +863,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _hdg_hold_enabled = false; _yaw_lock_engaged = false; - _att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad; + _att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get()); _att_sp.yaw_body = 0; } @@ -1052,11 +886,11 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto // if we assume that user is taking off then help by demanding altitude setpoint well above ground // and set limit to pitch angle to prevent steering into ground // this will only affect planes and not VTOL - float pitch_limit_min = _parameters.pitch_limit_min; + float pitch_limit_min = _param_fw_p_lim_min.get(); do_takeoff_help(&_hold_alt, &pitch_limit_min); /* throttle limiting */ - throttle_max = _parameters.throttle_max; + throttle_max = _param_fw_thr_max.get(); if (_vehicle_land_detected.landed && (fabsf(_manual.z) < THROTTLE_THRESH)) { throttle_max = 0.0f; @@ -1064,16 +898,16 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, - radians(_parameters.pitch_limit_min), - radians(_parameters.pitch_limit_max), - _parameters.throttle_min, + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), throttle_max, - _parameters.throttle_cruise, + _param_fw_thr_cruise.get(), climbout_requested, climbout_requested ? radians(10.0f) : pitch_limit_min, tecs_status_s::TECS_MODE_NORMAL); - _att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad; + _att_sp.roll_body = _manual.y * radians(_param_fw_man_r_max.get()); _att_sp.yaw_body = 0; } else { @@ -1101,7 +935,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons. the pre-takeoff throttle and the idle throttle normally map to the same parameter. */ - _att_sp.thrust_body[0] = _parameters.throttle_idle; + _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && @@ -1115,13 +949,13 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _att_sp.thrust_body[0] = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { - _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _parameters.throttle_max); + _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get()); } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_land_detected.landed) { // when we are landed state we want the motor to spin at idle speed - _att_sp.thrust_body[0] = min(_parameters.throttle_idle, throttle_max); + _att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), throttle_max); } else { _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); @@ -1214,17 +1048,17 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector _l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, curr_pos, ground_speed); // update tecs - const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); + const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get()); tecs_update_pitch_throttle(pos_sp_curr.alt, - calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min, ground_speed), - radians(_parameters.pitch_limit_min), + calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed), + radians(_param_fw_p_lim_min.get()), radians(takeoff_pitch_max_deg), - _parameters.throttle_min, - _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? - _parameters.throttle_cruise, + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here? + _param_fw_thr_cruise.get(), _runway_takeoff.climbout(), - radians(_runway_takeoff.getMinPitch(pos_sp_curr.pitch_min, 10.0f, _parameters.pitch_limit_min)), + radians(_runway_takeoff.getMinPitch(pos_sp_curr.pitch_min, 10.0f, _param_fw_p_lim_min.get())), tecs_status_s::TECS_MODE_TAKEOFF); // assign values @@ -1273,27 +1107,27 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use * full throttle, otherwise we use idle throttle */ - float takeoff_throttle = _parameters.throttle_max; + float takeoff_throttle = _param_fw_thr_max.get(); if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - takeoff_throttle = _parameters.throttle_idle; + takeoff_throttle = _param_fw_thr_idle.get(); } /* select maximum pitch: the launchdetector may impose another limit for the pitch * depending on the state of the launch */ - const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_parameters.pitch_limit_max); + const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_param_fw_p_lim_max.get()); const float altitude_error = pos_sp_curr.alt - _global_pos.alt; /* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */ - if (_parameters.climbout_diff > 0.0f && altitude_error > _parameters.climbout_diff) { + if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(pos_sp_curr.alt, - _parameters.airspeed_trim, - radians(_parameters.pitch_limit_min), + _param_fw_airspd_trim.get(), + radians(_param_fw_p_lim_min.get()), radians(takeoff_pitch_max_deg), - _parameters.throttle_min, + _param_fw_thr_min.get(), takeoff_throttle, - _parameters.throttle_cruise, + _param_fw_thr_cruise.get(), true, max(radians(pos_sp_curr.pitch_min), radians(10.0f)), tecs_status_s::TECS_MODE_TAKEOFF); @@ -1303,14 +1137,14 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector } else { tecs_update_pitch_throttle(pos_sp_curr.alt, - calculate_target_airspeed(_parameters.airspeed_trim, ground_speed), - radians(_parameters.pitch_limit_min), - radians(_parameters.pitch_limit_max), - _parameters.throttle_min, + calculate_target_airspeed(_param_fw_airspd_trim.get(), ground_speed), + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), takeoff_throttle, - _parameters.throttle_cruise, + _param_fw_thr_cruise.get(), false, - radians(_parameters.pitch_limit_min)); + radians(_param_fw_p_lim_min.get())); } } else { @@ -1353,7 +1187,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND; // Enable tighter throttle control for landings - _tecs.set_time_const_throt(_parameters.land_throtTC_scale * _parameters.time_const_throt); + _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) { @@ -1400,8 +1234,8 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector // we want the plane to keep tracking the desired flight path until we start flaring // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds - if ((_parameters.land_heading_hold_horizontal_distance > 0.0f) && !_land_noreturn_horizontal && - ((wp_distance < _parameters.land_heading_hold_horizontal_distance) || _land_noreturn_vertical)) { + if ((_param_fw_lnd_hhdist.get() > 0.0f) && !_land_noreturn_horizontal && + ((wp_distance < _param_fw_lnd_hhdist.get()) || _land_noreturn_vertical)) { if (pos_sp_prev.valid) { /* heading hold, along the line connecting this and the last waypoint */ @@ -1438,7 +1272,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector // default to no terrain estimation, just use landing waypoint altitude float terrain_alt = pos_sp_curr.alt; - if (_parameters.land_use_terrain_estimate == 1) { + if (_param_fw_lnd_useter.get() == 1) { if (_global_pos.terrain_alt_valid) { // all good, have valid terrain altitude terrain_alt = _global_pos.terrain_alt; @@ -1485,7 +1319,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector // _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ - float throttle_max = _parameters.throttle_max; + float throttle_max = _param_fw_thr_max.get(); /* enable direct yaw control using rudder/wheel */ if (_land_noreturn_horizontal) { @@ -1496,7 +1330,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector if (((_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt()) && (wp_distance_save < _landingslope.flare_length() + 5.0f)) || // Only kill throttle when close to WP _land_motor_lim) { - throttle_max = min(throttle_max, _parameters.throttle_land_max); + throttle_max = min(throttle_max, _param_fw_thr_lnd_max.get()); if (!_land_motor_lim) { _land_motor_lim = true; @@ -1513,18 +1347,18 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector _land_stayonground = true; } - const float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min; - const float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); + const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f; tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land, ground_speed), - radians(_parameters.land_flare_pitch_min_deg), - radians(_parameters.land_flare_pitch_max_deg), + radians(_param_fw_lnd_fl_pmin.get()), + radians(_param_fw_lnd_fl_pmax.get()), 0.0f, throttle_max, throttle_land, false, - _land_motor_lim ? radians(_parameters.land_flare_pitch_min_deg) : radians(_parameters.pitch_limit_min), + _land_motor_lim ? radians(_param_fw_lnd_fl_pmin.get()) : radians(_param_fw_p_lim_min.get()), _land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!_land_noreturn_vertical) { @@ -1536,7 +1370,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector } else { if (_global_pos.vel_d > 0.1f) { - _flare_pitch_sp = radians(_parameters.land_flare_pitch_min_deg) * + _flare_pitch_sp = radians(_param_fw_lnd_fl_pmin.get()) * constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f); } @@ -1582,17 +1416,17 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector } } - const float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; + const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); tecs_update_pitch_throttle(altitude_desired, calculate_target_airspeed(airspeed_approach, ground_speed), - radians(_parameters.pitch_limit_min), - radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - _parameters.throttle_max, - _parameters.throttle_cruise, + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + _param_fw_thr_min.get(), + _param_fw_thr_max.get(), + _param_fw_thr_cruise.get(), false, - radians(_parameters.pitch_limit_min)); + radians(_param_fw_p_lim_min.get())); } } @@ -1713,12 +1547,14 @@ FixedwingPositionControl::Run() _att_sp.timestamp = hrt_absolute_time(); // add attitude setpoint offsets - _att_sp.roll_body += _parameters.rollsp_offset_rad; - _att_sp.pitch_body += _parameters.pitchsp_offset_rad; + _att_sp.roll_body += radians(_param_fw_rsp_off.get()); + _att_sp.pitch_body += radians(_param_fw_psp_off.get()); if (_control_mode.flag_control_manual_enabled) { - _att_sp.roll_body = constrain(_att_sp.roll_body, -_parameters.man_roll_max_rad, _parameters.man_roll_max_rad); - _att_sp.pitch_body = constrain(_att_sp.pitch_body, -_parameters.man_pitch_max_rad, _parameters.man_pitch_max_rad); + _att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_man_r_max.get()), + radians(_param_fw_man_r_max.get())); + _att_sp.pitch_body = constrain(_att_sp.pitch_body, -radians(_param_fw_man_p_max.get()), + radians(_param_fw_man_p_max.get())); } Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); @@ -1813,15 +1649,15 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee _was_in_transition = true; // set this to transition airspeed to init tecs correctly - if (_parameters.airspeed_disabled) { + if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) { // some vtols fly without airspeed sensor - _asp_after_transition = _parameters.airspeed_trans; + _asp_after_transition = _param_airspeed_trans; } else { _asp_after_transition = _airspeed; } - _asp_after_transition = constrain(_asp_after_transition, _parameters.airspeed_min, _parameters.airspeed_max); + _asp_after_transition = constrain(_asp_after_transition, _param_fw_airspd_min.get(), _param_fw_airspd_max.get()); } else if (_was_in_transition) { // after transition we ramp up desired airspeed from the speed we had coming out of the transition @@ -1861,7 +1697,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* Using tecs library */ - float pitch_for_tecs = _pitch - _parameters.pitchsp_offset_rad; + float pitch_for_tecs = _pitch - radians(_param_fw_psp_off.get()); /* filter speed and altitude for controller */ Vector3f accel_body(_vehicle_acceleration_sub.get().xyz); @@ -1887,14 +1723,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee _global_pos.alt, _local_pos.vz); /* scale throttle cruise by baro pressure */ - if (_parameters.throttle_alt_scale > FLT_EPSILON) { + if (_param_fw_thr_alt_scl.get() > FLT_EPSILON) { sensor_baro_s baro{}; if (_sensor_baro_sub.update(&baro)) { - if (PX4_ISFINITE(baro.pressure) && PX4_ISFINITE(_parameters.throttle_alt_scale)) { + if (PX4_ISFINITE(baro.pressure) && PX4_ISFINITE(_param_fw_thr_alt_scl.get())) { // scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature) const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_MBAR / baro.pressure); - const float scale = constrain((eas2tas - 1.0f) * _parameters.throttle_alt_scale + 1.0f, 1.0f, 2.0f); + const float scale = constrain((eas2tas - 1.0f) * _param_fw_thr_alt_scl.get() + 1.0f, 1.0f, 2.0f); throttle_max = constrain(throttle_max * scale, throttle_min, 1.0f); throttle_cruise = constrain(throttle_cruise * scale, throttle_min + 0.01f, throttle_max - 0.01f); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 454ed721c4..315eceac3e 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -264,112 +264,8 @@ private: FW_POSCTRL_MODE_OTHER } _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 climbout_diff; - - float max_climb_rate; - float max_sink_rate; - float speed_weight; - float time_const_throt; - - float airspeed_min; - float airspeed_trim; - float airspeed_max; - int32_t airspeed_disabled; - - float pitch_limit_min; - float pitch_limit_max; - - float throttle_min; - float throttle_max; - float throttle_idle; - float throttle_cruise; - float throttle_alt_scale; - - float man_roll_max_rad; - float man_pitch_max_rad; - float rollsp_offset_rad; - float pitchsp_offset_rad; - - float throttle_land_max; - float loiter_radius; - - float land_heading_hold_horizontal_distance; - float land_flare_pitch_min_deg; - float land_flare_pitch_max_deg; - int32_t land_use_terrain_estimate; - int32_t land_early_config_change; - float land_airspeed_scale; - float land_throtTC_scale; - - // VTOL - float airspeed_trans; - } _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 roll_slew_deg_sec; - - 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 heightrate_p; - param_t heightrate_ff; - param_t speedrate_p; - param_t throttle_damp; - param_t integrator_gain; - param_t vertical_accel_limit; - param_t speed_comp_filter_omega; - param_t roll_throttle_compensation; - param_t speed_weight; - param_t pitch_damping; - - param_t airspeed_min; - param_t airspeed_trim; - param_t airspeed_max; - param_t airspeed_trans; - param_t airspeed_disabled; - - param_t pitch_limit_min; - param_t pitch_limit_max; - - param_t throttle_min; - param_t throttle_max; - param_t throttle_idle; - param_t throttle_cruise; - param_t throttle_slew_max; - param_t throttle_alt_scale; - - param_t man_roll_max_deg; - param_t man_pitch_max_deg; - param_t rollsp_offset_deg; - param_t pitchsp_offset_deg; - - param_t throttle_land_max; - - param_t land_slope_angle; - param_t land_H1_virt; - param_t land_flare_alt_relative; - param_t land_thrust_lim_alt_relative; - param_t land_heading_hold_horizontal_distance; - param_t land_flare_pitch_min_deg; - param_t land_flare_pitch_max_deg; - param_t land_use_terrain_estimate; - param_t land_early_config_change; - param_t land_airspeed_scale; - param_t land_throtTC_scale; - param_t loiter_radius; - } _parameter_handles {}; ///< handles for interesting parameters - - DEFINE_PARAMETERS( - (ParamFloat) _groundspeed_min - ) + param_t _param_handle_airspeed_trans{PARAM_INVALID}; + float _param_airspeed_trans{NAN}; // Update our local parameter cache. int parameters_update(); @@ -454,6 +350,72 @@ private: bool climbout_mode, float climbout_pitch_min_rad, uint8_t mode = tecs_status_s::TECS_MODE_NORMAL); + DEFINE_PARAMETERS( + + (ParamFloat) _param_fw_airspd_max, + (ParamFloat) _param_fw_airspd_min, + (ParamFloat) _param_fw_airspd_trim, + + (ParamFloat) _param_fw_clmbout_diff, + + (ParamFloat) _param_fw_gnd_spd_min, + + (ParamFloat) _param_fw_l1_damping, + (ParamFloat) _param_fw_l1_period, + (ParamFloat) _param_fw_l1_r_slew_max, + (ParamFloat) _param_fw_r_lim, + + (ParamFloat) _param_fw_lnd_airspd_sc, + (ParamFloat) _param_fw_lnd_ang, + (ParamFloat) _param_fw_lnd_fl_pmax, + (ParamFloat) _param_fw_lnd_fl_pmin, + (ParamFloat) _param_fw_lnd_flalt, + (ParamFloat) _param_fw_lnd_hhdist, + (ParamFloat) _param_fw_lnd_hvirt, + (ParamFloat) _param_fw_thrtc_sc, + (ParamFloat) _param_fw_lnd_tlalt, + (ParamBool) _param_fw_lnd_earlycfg, + (ParamBool) _param_fw_lnd_useter, + + (ParamFloat) _param_fw_p_lim_max, + (ParamFloat) _param_fw_p_lim_min, + + (ParamFloat) _param_fw_t_clmb_max, + (ParamFloat) _param_fw_t_hrate_ff, + (ParamFloat) _param_fw_t_hrate_p, + (ParamFloat) _param_fw_t_integ_gain, + (ParamFloat) _param_fw_t_ptch_damp, + (ParamFloat) _param_fw_t_rll2thr, + (ParamFloat) _param_fw_t_sink_max, + (ParamFloat) _param_fw_t_sink_min, + (ParamFloat) _param_fw_t_spd_omega, + (ParamFloat) _param_fw_t_spdweight, + (ParamFloat) _param_fw_t_srate_p, + (ParamFloat) _param_fw_t_thr_damp, + (ParamFloat) _param_fw_t_thro_const, + (ParamFloat) _param_fw_t_time_const, + (ParamFloat) _param_fw_t_vert_acc, + + (ParamFloat) _param_fw_thr_alt_scl, + (ParamFloat) _param_fw_thr_cruise, + (ParamFloat) _param_fw_thr_idle, + (ParamFloat) _param_fw_thr_lnd_max, + (ParamFloat) _param_fw_thr_max, + (ParamFloat) _param_fw_thr_min, + (ParamFloat) _param_fw_thr_slew_max, + + // external parameters + (ParamInt) _param_fw_arsp_mode, + + (ParamFloat) _param_fw_psp_off, + (ParamFloat) _param_fw_rsp_off, + (ParamFloat) _param_fw_man_p_max, + (ParamFloat) _param_fw_man_r_max, + + (ParamFloat) _param_nav_loiter_rad + + ) + }; #endif // FIXEDWINGPOSITIONCONTROL_HPP_