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

Parameter update - Rename variables in modules/mc_att_control 

using parameter_update.py script
This commit is contained in:
bresch 2019-03-25 14:01:04 +01:00 committed by Matthias Grob
parent 2197ac883d
commit e575f032e4
2 changed files with 83 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -216,68 +216,69 @@ private:
bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MC_ROLL_P>) _roll_p,
(ParamFloat<px4::params::MC_ROLLRATE_P>) _roll_rate_p,
(ParamFloat<px4::params::MC_ROLLRATE_I>) _roll_rate_i,
(ParamFloat<px4::params::MC_RR_INT_LIM>) _roll_rate_integ_lim,
(ParamFloat<px4::params::MC_ROLLRATE_D>) _roll_rate_d,
(ParamFloat<px4::params::MC_ROLLRATE_FF>) _roll_rate_ff,
(ParamFloat<px4::params::MC_ROLL_P>) _param_mc_roll_p,
(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,
(ParamFloat<px4::params::MC_RR_INT_LIM>) _param_mc_rr_int_lim,
(ParamFloat<px4::params::MC_ROLLRATE_D>) _param_mc_rollrate_d,
(ParamFloat<px4::params::MC_ROLLRATE_FF>) _param_mc_rollrate_ff,
(ParamFloat<px4::params::MC_PITCH_P>) _pitch_p,
(ParamFloat<px4::params::MC_PITCHRATE_P>) _pitch_rate_p,
(ParamFloat<px4::params::MC_PITCHRATE_I>) _pitch_rate_i,
(ParamFloat<px4::params::MC_PR_INT_LIM>) _pitch_rate_integ_lim,
(ParamFloat<px4::params::MC_PITCHRATE_D>) _pitch_rate_d,
(ParamFloat<px4::params::MC_PITCHRATE_FF>) _pitch_rate_ff,
(ParamFloat<px4::params::MC_PITCH_P>) _param_mc_pitch_p,
(ParamFloat<px4::params::MC_PITCHRATE_P>) _param_mc_pitchrate_p,
(ParamFloat<px4::params::MC_PITCHRATE_I>) _param_mc_pitchrate_i,
(ParamFloat<px4::params::MC_PR_INT_LIM>) _param_mc_pr_int_lim,
(ParamFloat<px4::params::MC_PITCHRATE_D>) _param_mc_pitchrate_d,
(ParamFloat<px4::params::MC_PITCHRATE_FF>) _param_mc_pitchrate_ff,
(ParamFloat<px4::params::MC_YAW_P>) _yaw_p,
(ParamFloat<px4::params::MC_YAWRATE_P>) _yaw_rate_p,
(ParamFloat<px4::params::MC_YAWRATE_I>) _yaw_rate_i,
(ParamFloat<px4::params::MC_YR_INT_LIM>) _yaw_rate_integ_lim,
(ParamFloat<px4::params::MC_YAWRATE_D>) _yaw_rate_d,
(ParamFloat<px4::params::MC_YAWRATE_FF>) _yaw_rate_ff,
(ParamFloat<px4::params::MC_YAW_P>) _param_mc_yaw_p,
(ParamFloat<px4::params::MC_YAWRATE_P>) _param_mc_yawrate_p,
(ParamFloat<px4::params::MC_YAWRATE_I>) _param_mc_yawrate_i,
(ParamFloat<px4::params::MC_YR_INT_LIM>) _param_mc_yr_int_lim,
(ParamFloat<px4::params::MC_YAWRATE_D>) _param_mc_yawrate_d,
(ParamFloat<px4::params::MC_YAWRATE_FF>) _param_mc_yawrate_ff,
(ParamFloat<px4::params::MC_DTERM_CUTOFF>) _d_term_cutoff_freq, /**< Cutoff frequency for the D-term filter */
(ParamFloat<px4::params::MC_DTERM_CUTOFF>) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */
(ParamFloat<px4::params::MC_TPA_BREAK_P>) _tpa_breakpoint_p, /**< Throttle PID Attenuation breakpoint */
(ParamFloat<px4::params::MC_TPA_BREAK_I>) _tpa_breakpoint_i, /**< Throttle PID Attenuation breakpoint */
(ParamFloat<px4::params::MC_TPA_BREAK_D>) _tpa_breakpoint_d, /**< Throttle PID Attenuation breakpoint */
(ParamFloat<px4::params::MC_TPA_RATE_P>) _tpa_rate_p, /**< Throttle PID Attenuation slope */
(ParamFloat<px4::params::MC_TPA_RATE_I>) _tpa_rate_i, /**< Throttle PID Attenuation slope */
(ParamFloat<px4::params::MC_TPA_RATE_D>) _tpa_rate_d, /**< Throttle PID Attenuation slope */
(ParamFloat<px4::params::MC_TPA_BREAK_P>) _param_mc_tpa_break_p, /**< Throttle PID Attenuation breakpoint */
(ParamFloat<px4::params::MC_TPA_BREAK_I>) _param_mc_tpa_break_i, /**< Throttle PID Attenuation breakpoint */
(ParamFloat<px4::params::MC_TPA_BREAK_D>) _param_mc_tpa_break_d, /**< Throttle PID Attenuation breakpoint */
(ParamFloat<px4::params::MC_TPA_RATE_P>) _param_mc_tpa_rate_p, /**< Throttle PID Attenuation slope */
(ParamFloat<px4::params::MC_TPA_RATE_I>) _param_mc_tpa_rate_i, /**< Throttle PID Attenuation slope */
(ParamFloat<px4::params::MC_TPA_RATE_D>) _param_mc_tpa_rate_d, /**< Throttle PID Attenuation slope */
(ParamFloat<px4::params::MC_ROLLRATE_MAX>) _roll_rate_max,
(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _pitch_rate_max,
(ParamFloat<px4::params::MC_YAWRATE_MAX>) _yaw_rate_max,
(ParamFloat<px4::params::MC_YAWRAUTO_MAX>) _yaw_auto_max,
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MC_ROLLRATE_MAX>) _param_mc_rollrate_max,
(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _param_mc_pitchrate_max,
(ParamFloat<px4::params::MC_YAWRATE_MAX>) _param_mc_yawrate_max,
(ParamFloat<px4::params::MC_YAWRAUTO_MAX>) _param_mc_yawrauto_max,
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MC_ACRO_R_MAX>) _acro_roll_max,
(ParamFloat<px4::params::MC_ACRO_P_MAX>) _acro_pitch_max,
(ParamFloat<px4::params::MC_ACRO_Y_MAX>) _acro_yaw_max,
(ParamFloat<px4::params::MC_ACRO_EXPO>) _acro_expo_rp, /**< expo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_EXPO_Y>) _acro_expo_y, /**< expo stick curve shape (yaw) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _acro_superexpo_rp, /**< superexpo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPOY>) _acro_superexpo_y, /**< superexpo stick curve shape (yaw) */
(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
(ParamFloat<px4::params::MC_ACRO_P_MAX>) _param_mc_acro_p_max,
(ParamFloat<px4::params::MC_ACRO_Y_MAX>) _param_mc_acro_y_max,
(ParamFloat<px4::params::MC_ACRO_EXPO>) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_EXPO_Y>) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPO>) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */
(ParamFloat<px4::params::MC_ACRO_SUPEXPOY>) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */
(ParamFloat<px4::params::MC_RATT_TH>) _rattitude_thres,
(ParamFloat<px4::params::MC_RATT_TH>) _param_mc_ratt_th,
(ParamBool<px4::params::MC_BAT_SCALE_EN>) _bat_scale_en,
(ParamBool<px4::params::MC_BAT_SCALE_EN>) _param_mc_bat_scale_en,
(ParamInt<px4::params::SENS_BOARD_ROT>) _board_rotation_param,
(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _board_offset_x,
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _board_offset_y,
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _board_offset_z,
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off,
/* Stabilized mode params */
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _man_tilt_max_deg, /**< maximum tilt allowed for manual flight */
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _man_throttle_min, /**< minimum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_MAX>) _throttle_max, /**< maximum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_HOVER>) _throttle_hover, /**< throttle at which vehicle is at hover equilibrium */
(ParamInt<px4::params::MPC_THR_CURVE>) _throttle_curve, /**< throttle curve behavior */
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min, /**< minimum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max, /**< maximum throttle for stabilized */
(ParamFloat<px4::params::MPC_THR_HOVER>)
_param_mpc_thr_hover, /**< throttle at which vehicle is at hover equilibrium */
(ParamInt<px4::params::MPC_THR_CURVE>) _param_mpc_thr_curve, /**< throttle curve behavior */
(ParamInt<px4::params::MC_AIRMODE>) _airmode
(ParamInt<px4::params::MC_AIRMODE>) _param_mc_airmode
)
matrix::Vector3f _rate_p; /**< P gain for angular rate error */

66
src/modules/mc_att_control/mc_att_control_main.cpp
View File

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