From e575f032e4d6630d59d1f4556dbba678610491df Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 25 Mar 2019 14:01:04 +0100 Subject: [PATCH] Parameter update - Rename variables in modules/mc_att_control using parameter_update.py script --- src/modules/mc_att_control/mc_att_control.hpp | 99 ++++++++++--------- .../mc_att_control/mc_att_control_main.cpp | 66 ++++++------- 2 files changed, 83 insertions(+), 82 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 41e041dfef..72ce93ccf8 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -216,68 +216,69 @@ private: bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ DEFINE_PARAMETERS( - (ParamFloat) _roll_p, - (ParamFloat) _roll_rate_p, - (ParamFloat) _roll_rate_i, - (ParamFloat) _roll_rate_integ_lim, - (ParamFloat) _roll_rate_d, - (ParamFloat) _roll_rate_ff, + (ParamFloat) _param_mc_roll_p, + (ParamFloat) _param_mc_rollrate_p, + (ParamFloat) _param_mc_rollrate_i, + (ParamFloat) _param_mc_rr_int_lim, + (ParamFloat) _param_mc_rollrate_d, + (ParamFloat) _param_mc_rollrate_ff, - (ParamFloat) _pitch_p, - (ParamFloat) _pitch_rate_p, - (ParamFloat) _pitch_rate_i, - (ParamFloat) _pitch_rate_integ_lim, - (ParamFloat) _pitch_rate_d, - (ParamFloat) _pitch_rate_ff, + (ParamFloat) _param_mc_pitch_p, + (ParamFloat) _param_mc_pitchrate_p, + (ParamFloat) _param_mc_pitchrate_i, + (ParamFloat) _param_mc_pr_int_lim, + (ParamFloat) _param_mc_pitchrate_d, + (ParamFloat) _param_mc_pitchrate_ff, - (ParamFloat) _yaw_p, - (ParamFloat) _yaw_rate_p, - (ParamFloat) _yaw_rate_i, - (ParamFloat) _yaw_rate_integ_lim, - (ParamFloat) _yaw_rate_d, - (ParamFloat) _yaw_rate_ff, + (ParamFloat) _param_mc_yaw_p, + (ParamFloat) _param_mc_yawrate_p, + (ParamFloat) _param_mc_yawrate_i, + (ParamFloat) _param_mc_yr_int_lim, + (ParamFloat) _param_mc_yawrate_d, + (ParamFloat) _param_mc_yawrate_ff, - (ParamFloat) _d_term_cutoff_freq, /**< Cutoff frequency for the D-term filter */ + (ParamFloat) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */ - (ParamFloat) _tpa_breakpoint_p, /**< Throttle PID Attenuation breakpoint */ - (ParamFloat) _tpa_breakpoint_i, /**< Throttle PID Attenuation breakpoint */ - (ParamFloat) _tpa_breakpoint_d, /**< Throttle PID Attenuation breakpoint */ - (ParamFloat) _tpa_rate_p, /**< Throttle PID Attenuation slope */ - (ParamFloat) _tpa_rate_i, /**< Throttle PID Attenuation slope */ - (ParamFloat) _tpa_rate_d, /**< Throttle PID Attenuation slope */ + (ParamFloat) _param_mc_tpa_break_p, /**< Throttle PID Attenuation breakpoint */ + (ParamFloat) _param_mc_tpa_break_i, /**< Throttle PID Attenuation breakpoint */ + (ParamFloat) _param_mc_tpa_break_d, /**< Throttle PID Attenuation breakpoint */ + (ParamFloat) _param_mc_tpa_rate_p, /**< Throttle PID Attenuation slope */ + (ParamFloat) _param_mc_tpa_rate_i, /**< Throttle PID Attenuation slope */ + (ParamFloat) _param_mc_tpa_rate_d, /**< Throttle PID Attenuation slope */ - (ParamFloat) _roll_rate_max, - (ParamFloat) _pitch_rate_max, - (ParamFloat) _yaw_rate_max, - (ParamFloat) _yaw_auto_max, - (ParamFloat) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */ + (ParamFloat) _param_mc_rollrate_max, + (ParamFloat) _param_mc_pitchrate_max, + (ParamFloat) _param_mc_yawrate_max, + (ParamFloat) _param_mc_yawrauto_max, + (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ - (ParamFloat) _acro_roll_max, - (ParamFloat) _acro_pitch_max, - (ParamFloat) _acro_yaw_max, - (ParamFloat) _acro_expo_rp, /**< expo stick curve shape (roll & pitch) */ - (ParamFloat) _acro_expo_y, /**< expo stick curve shape (yaw) */ - (ParamFloat) _acro_superexpo_rp, /**< superexpo stick curve shape (roll & pitch) */ - (ParamFloat) _acro_superexpo_y, /**< superexpo stick curve shape (yaw) */ + (ParamFloat) _param_mc_acro_r_max, + (ParamFloat) _param_mc_acro_p_max, + (ParamFloat) _param_mc_acro_y_max, + (ParamFloat) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */ + (ParamFloat) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ - (ParamFloat) _rattitude_thres, + (ParamFloat) _param_mc_ratt_th, - (ParamBool) _bat_scale_en, + (ParamBool) _param_mc_bat_scale_en, - (ParamInt) _board_rotation_param, + (ParamInt) _param_sens_board_rot, - (ParamFloat) _board_offset_x, - (ParamFloat) _board_offset_y, - (ParamFloat) _board_offset_z, + (ParamFloat) _param_sens_board_x_off, + (ParamFloat) _param_sens_board_y_off, + (ParamFloat) _param_sens_board_z_off, /* Stabilized mode params */ - (ParamFloat) _man_tilt_max_deg, /**< maximum tilt allowed for manual flight */ - (ParamFloat) _man_throttle_min, /**< minimum throttle for stabilized */ - (ParamFloat) _throttle_max, /**< maximum throttle for stabilized */ - (ParamFloat) _throttle_hover, /**< throttle at which vehicle is at hover equilibrium */ - (ParamInt) _throttle_curve, /**< throttle curve behavior */ + (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ + (ParamFloat) _param_mpc_manthr_min, /**< minimum throttle for stabilized */ + (ParamFloat) _param_mpc_thr_max, /**< maximum throttle for stabilized */ + (ParamFloat) + _param_mpc_thr_hover, /**< throttle at which vehicle is at hover equilibrium */ + (ParamInt) _param_mpc_thr_curve, /**< throttle curve behavior */ - (ParamInt) _airmode + (ParamInt) _param_mc_airmode ) matrix::Vector3f _rate_p; /**< P gain for angular rate error */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index fae4d87964..5abc429a26 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -132,40 +132,40 @@ void MulticopterAttitudeControl::parameters_updated() { // Store some of the parameters in a more convenient way & precompute often-used values - _attitude_control.setProportionalGain(Vector3f(_roll_p.get(), _pitch_p.get(), _yaw_p.get())); + _attitude_control.setProportionalGain(Vector3f(_param_mc_roll_p.get(), _param_mc_pitch_p.get(), _param_mc_yaw_p.get())); // rate gains - _rate_p = Vector3f(_roll_rate_p.get(), _pitch_rate_p.get(), _yaw_rate_p.get()); - _rate_i = Vector3f(_roll_rate_i.get(), _pitch_rate_i.get(), _yaw_rate_i.get()); - _rate_int_lim = Vector3f(_roll_rate_integ_lim.get(), _pitch_rate_integ_lim.get(), _yaw_rate_integ_lim.get()); - _rate_d = Vector3f(_roll_rate_d.get(), _pitch_rate_d.get(), _yaw_rate_d.get()); - _rate_ff = Vector3f(_roll_rate_ff.get(), _pitch_rate_ff.get(), _yaw_rate_ff.get()); + _rate_p = Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get()); + _rate_i = Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get()); + _rate_int_lim = Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get()); + _rate_d = Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()); + _rate_ff = Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get()); - if (fabsf(_lp_filters_d.get_cutoff_freq() - _d_term_cutoff_freq.get()) > 0.01f) { - _lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get()); + if (fabsf(_lp_filters_d.get_cutoff_freq() - _param_mc_dterm_cutoff.get()) > 0.01f) { + _lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _param_mc_dterm_cutoff.get()); _lp_filters_d.reset(_rates_prev); } // angular rate limits using math::radians; - _attitude_control.setRateLimit(Vector3f(radians(_roll_rate_max.get()), radians(_pitch_rate_max.get()), radians(_yaw_rate_max.get()))); + _attitude_control.setRateLimit(Vector3f(radians(_param_mc_rollrate_max.get()), radians(_param_mc_pitchrate_max.get()), radians(_param_mc_yawrate_max.get()))); adapt_auto_yaw_rate_limit(); // manual rate control acro mode rate limits - _acro_rate_max = Vector3f(radians(_acro_roll_max.get()), radians(_acro_pitch_max.get()), radians(_acro_yaw_max.get())); + _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), radians(_param_mc_acro_y_max.get())); - _man_tilt_max = math::radians(_man_tilt_max_deg.get()); + _man_tilt_max = math::radians(_param_mpc_man_tilt_max.get()); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); /* get transformation matrix from sensor/board to body frame */ - _board_rotation = get_rot_matrix((enum Rotation)_board_rotation_param.get()); + _board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); /* fine tune the rotation */ Dcmf board_rotation_offset(Eulerf( - M_DEG_TO_RAD_F * _board_offset_x.get(), - M_DEG_TO_RAD_F * _board_offset_y.get(), - M_DEG_TO_RAD_F * _board_offset_z.get())); + M_DEG_TO_RAD_F * _param_sens_board_x_off.get(), + M_DEG_TO_RAD_F * _param_sens_board_y_off.get(), + M_DEG_TO_RAD_F * _param_sens_board_z_off.get())); _board_rotation = board_rotation_offset * _board_rotation; } @@ -202,9 +202,9 @@ MulticopterAttitudeControl::vehicle_control_mode_poll() void MulticopterAttitudeControl::adapt_auto_yaw_rate_limit() { if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) && !_v_control_mode.flag_control_manual_enabled) { - _attitude_control.setRateLimitYaw(math::radians(_yaw_auto_max.get())); + _attitude_control.setRateLimitYaw(math::radians(_param_mc_yawrauto_max.get())); } else { - _attitude_control.setRateLimitYaw(math::radians(_yaw_rate_max.get())); + _attitude_control.setRateLimitYaw(math::radians(_param_mc_yawrate_max.get())); } } @@ -378,16 +378,16 @@ float MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { // throttle_stick_input is in range [0, 1] - switch (_throttle_curve.get()) { + switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle - return _man_throttle_min.get() + throttle_stick_input * (_throttle_max.get() - _man_throttle_min.get()); + return _param_mpc_manthr_min.get() + throttle_stick_input * (_param_mpc_thr_max.get() - _param_mpc_manthr_min.get()); default: // 0 or other: rescale to hover throttle at 0.5 stick if (throttle_stick_input < 0.5f) { - return (_throttle_hover.get() - _man_throttle_min.get()) / 0.5f * throttle_stick_input + _man_throttle_min.get(); + return (_param_mpc_thr_hover.get() - _param_mpc_manthr_min.get()) / 0.5f * throttle_stick_input + _param_mpc_manthr_min.get(); } else { - return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle_stick_input - 1.0f) + _throttle_max.get(); + return (_param_mpc_thr_max.get() - _param_mpc_thr_hover.get()) / 0.5f * (throttle_stick_input - 1.0f) + _param_mpc_thr_max.get(); } } } @@ -425,9 +425,9 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ if (reset_yaw_sp) { _man_yaw_sp = yaw; - } else if (_manual_control_sp.z > 0.05f || _airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { + } else if (_manual_control_sp.z > 0.05f || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { - const float yaw_rate = math::radians(_yaw_rate_scaling.get()); + const float yaw_rate = math::radians(_param_mpc_man_y_max.get()); attitude_setpoint.yaw_sp_move_rate = _manual_control_sp.r * yaw_rate; _man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt); } @@ -598,9 +598,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) rates(1) -= _sensor_bias.gyro_y_bias; rates(2) -= _sensor_bias.gyro_z_bias; - Vector3f rates_p_scaled = _rate_p.emult(pid_attenuations(_tpa_breakpoint_p.get(), _tpa_rate_p.get())); - Vector3f rates_i_scaled = _rate_i.emult(pid_attenuations(_tpa_breakpoint_i.get(), _tpa_rate_i.get())); - Vector3f rates_d_scaled = _rate_d.emult(pid_attenuations(_tpa_breakpoint_d.get(), _tpa_rate_d.get())); + Vector3f rates_p_scaled = _rate_p.emult(pid_attenuations(_param_mc_tpa_break_p.get(), _param_mc_tpa_rate_p.get())); + Vector3f rates_i_scaled = _rate_i.emult(pid_attenuations(_param_mc_tpa_break_i.get(), _param_mc_tpa_rate_i.get())); + Vector3f rates_d_scaled = _rate_d.emult(pid_attenuations(_param_mc_tpa_break_d.get(), _param_mc_tpa_rate_d.get())); /* angular rates error */ Vector3f rates_err = _rates_sp - rates; @@ -699,7 +699,7 @@ MulticopterAttitudeControl::publish_actuator_controls() _actuators.timestamp_sample = _sensor_gyro.timestamp; /* scale effort by battery status */ - if (_bat_scale_en.get() && _battery_status.scale > 0.0f) { + if (_param_mc_bat_scale_en.get() && _battery_status.scale > 0.0f) { for (int i = 0; i < 4; i++) { _actuators.control[i] *= _battery_status.scale; } @@ -810,8 +810,8 @@ MulticopterAttitudeControl::run() * even bother running the attitude controllers */ if (_v_control_mode.flag_control_rattitude_enabled) { _v_control_mode.flag_control_attitude_enabled = - fabsf(_manual_control_sp.y) <= _rattitude_thres.get() && - fabsf(_manual_control_sp.x) <= _rattitude_thres.get(); + fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() && + fabsf(_manual_control_sp.x) <= _param_mc_ratt_th.get(); } bool attitude_setpoint_generated = false; @@ -837,9 +837,9 @@ MulticopterAttitudeControl::run() if (manual_control_updated) { /* manual rates control - ACRO mode */ Vector3f man_rate_sp( - math::superexpo(_manual_control_sp.y, _acro_expo_rp.get(), _acro_superexpo_rp.get()), - math::superexpo(-_manual_control_sp.x, _acro_expo_rp.get(), _acro_superexpo_rp.get()), - math::superexpo(_manual_control_sp.r, _acro_expo_y.get(), _acro_superexpo_y.get())); + math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())); _rates_sp = man_rate_sp.emult(_acro_rate_max); _thrust_sp = _manual_control_sp.z; publish_rates_setpoint(); @@ -883,7 +883,7 @@ MulticopterAttitudeControl::run() _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; dt_accumulator = 0; loop_counter = 0; - _lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get()); + _lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _param_mc_dterm_cutoff.get()); } }