dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fw_pos_control_l1: move to px4::params

This commit is contained in:
Daniel Agar 2019-10-28 21:15:38 -04:00
parent e0abe7090e
commit 3dc23afb3e
2 changed files with 211 additions and 413 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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<vtol_type>(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);

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

@ -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<px4::params::FW_GND_SPD_MIN>) _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<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
(ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _param_fw_clmbout_diff,
(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min,
(ParamFloat<px4::params::FW_L1_DAMPING>) _param_fw_l1_damping,
(ParamFloat<px4::params::FW_L1_PERIOD>) _param_fw_l1_period,
(ParamFloat<px4::params::FW_L1_R_SLEW_MAX>) _param_fw_l1_r_slew_max,
(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,
(ParamFloat<px4::params::FW_LND_AIRSPD_SC>) _param_fw_lnd_airspd_sc,
(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang,
(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,
(ParamFloat<px4::params::FW_LND_FL_PMIN>) _param_fw_lnd_fl_pmin,
(ParamFloat<px4::params::FW_LND_FLALT>) _param_fw_lnd_flalt,
(ParamFloat<px4::params::FW_LND_HHDIST>) _param_fw_lnd_hhdist,
(ParamFloat<px4::params::FW_LND_HVIRT>) _param_fw_lnd_hvirt,
(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
(ParamFloat<px4::params::FW_LND_TLALT>) _param_fw_lnd_tlalt,
(ParamBool<px4::params::FW_LND_EARLYCFG>) _param_fw_lnd_earlycfg,
(ParamBool<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
(ParamFloat<px4::params::FW_P_LIM_MAX>) _param_fw_p_lim_max,
(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,
(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_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,
(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_SRATE_P>) _param_fw_t_srate_p,
(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_THR_ALT_SCL>) _param_fw_thr_alt_scl,
(ParamFloat<px4::params::FW_THR_CRUISE>) _param_fw_thr_cruise,
(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
(ParamFloat<px4::params::FW_THR_LND_MAX>) _param_fw_thr_lnd_max,
(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
(ParamFloat<px4::params::FW_THR_SLEW_MAX>) _param_fw_thr_slew_max,
// external parameters
(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
(ParamFloat<px4::params::FW_RSP_OFF>) _param_fw_rsp_off,
(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad
)
};
#endif // FIXEDWINGPOSITIONCONTROL_HPP_