fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 04:20:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Parameter update - Rename variables in modules/mc_att_control

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+33 -33
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());
}
}