Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

mc_pos_control small refactoring while studying

Browse Source
This commit is contained in:
Matthias Grob
2016-12-20 17:01:53 +01:00
committed by Lorenz Meier
parent 3f545c270d
commit eda7848e16
1 changed files with 8 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+8 -18
View File
@@ -1314,6 +1314,7 @@ MulticopterPositionControl::limit_acceleration(float dt)
// limit vertical acceleration // limit vertical acceleration
float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt; float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
// TODO: vertical acceleration is not just 2 * horizontal acceleration
if (fabsf(acc_v) > 2 * _params.acc_hor_max) { if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
acc_v /= fabsf(acc_v); acc_v /= fabsf(acc_v);
_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2); _vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
@@ -1725,14 +1726,7 @@ MulticopterPositionControl::control_position(float dt)
float i = _params.thr_min; float i = _params.thr_min;
if (_reset_int_z_manual) { if (_reset_int_z_manual) {
i = _params.thr_hover; i = math::constrain(_params.thr_hover, _params.thr_min, _params.thr_max);
if (i < _params.thr_min) {
i = _params.thr_min;
} else if (i > _params.thr_max) {
i = _params.thr_max;
}
} }
_thrust_int(2) = -i; _thrust_int(2) = -i;
@@ -1770,9 +1764,7 @@ MulticopterPositionControl::control_position(float dt)
2) * _att_sp.thrust - _thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1); 2) * _att_sp.thrust - _thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1);
_vel_sp(2) = _vel(2) + (-Rb(2, _vel_sp(2) = _vel(2) + (-Rb(2,
2) * _att_sp.thrust - _thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2); 2) * _att_sp.thrust - _thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2);
_vel_sp_prev(0) = _vel_sp(0); _vel_sp_prev = _vel_sp;
_vel_sp_prev(1) = _vel_sp(1);
_vel_sp_prev(2) = _vel_sp(2);
control_vel_enabled_prev = true; control_vel_enabled_prev = true;
// compute updated velocity error // compute updated velocity error
@@ -2232,7 +2224,7 @@ MulticopterPositionControl::task_main()
parameters_update(false); parameters_update(false);
hrt_abstime t = hrt_absolute_time(); hrt_abstime t = hrt_absolute_time();
float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; float dt = t_prev != 0 ? (t - t_prev) / 1e6f : 0.0f;
t_prev = t; t_prev = t;
// set dt for control blocks // set dt for control blocks
@@ -2250,11 +2242,9 @@ MulticopterPositionControl::task_main()
} }
/* reset yaw and altitude setpoint for VTOL which are in fw mode */ /* reset yaw and altitude setpoint for VTOL which are in fw mode */
if (_vehicle_status.is_vtol) { if (_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing) {
if (!_vehicle_status.is_rotary_wing) { _reset_yaw_sp = true;
_reset_yaw_sp = true; _reset_alt_sp = true;
_reset_alt_sp = true;
}
} }
//Update previous arming state //Update previous arming state
@@ -2303,8 +2293,8 @@ MulticopterPositionControl::task_main()
} else { } else {
/* position controller disabled, reset setpoints */ /* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_pos_sp = true; _reset_pos_sp = true;
_reset_alt_sp = true;
_do_reset_alt_pos_flag = true; _do_reset_alt_pos_flag = true;
_mode_auto = false; _mode_auto = false;
_reset_int_z = true; _reset_int_z = true;