fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 03:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

removed jerk fix and reset prev horizontal vel sp with current velocity

Browse Source
This commit is contained in:
Andreas Antener
2015-12-18 15:55:17 +00:00
committed by Lorenz Meier
parent 35df66ce5d
commit 0f67d3fcd8
1 changed files with 3 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+3 -30
View File
@@ -1284,6 +1284,8 @@ MulticopterPositionControl::task_main()
} }
if (!_control_mode.flag_control_velocity_enabled) { if (!_control_mode.flag_control_velocity_enabled) {
_vel_sp_prev(0) = _vel(0);
_vel_sp_prev(1) = _vel(1);
_vel_sp(0) = 0.0f; _vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f; _vel_sp(1) = 0.0f;
} }
@@ -1366,6 +1368,7 @@ MulticopterPositionControl::task_main()
math::Vector<3> vel_err = _vel_sp - _vel; math::Vector<3> vel_err = _vel_sp - _vel;
/* thrust vector in NED frame */ /* thrust vector in NED frame */
// TODO?: + _vel_sp.emult(_params.vel_ff)
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int; math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
if (!_control_mode.flag_control_velocity_enabled) { if (!_control_mode.flag_control_velocity_enabled) {
@@ -1497,36 +1500,6 @@ MulticopterPositionControl::task_main()
} }
} }
/* limit max change rate */
math::Vector<2> jerk_hor;
math::Vector<3> thrust_diff = thrust_sp - _thrust_sp_prev;
/* this is jerk times dt
* we call it jerk so its clear what the intention is -
* to limit the jerk, not the total acceleration.
*/
jerk_hor(0) = thrust_diff(0);
jerk_hor(1) = thrust_diff(1);
/* because we compare with jerk times dt, we multiply here as well */
float max_hor_jerk = (_params.acc_hor_max / ONE_G) * dt;
if (jerk_hor.length() > max_hor_jerk) {
/* bring it to length one */
jerk_hor.normalize();
/* bring it to the max length for jerk * dt for the next step */
jerk_hor *= max_hor_jerk;
/* now use the previous trust setpoint and add the allowable delta */
math::Vector<2> thrust_sp_hor_prev(_thrust_sp_prev(0), _thrust_sp_prev(1));
math::Vector<2> thrust_sp_hor = thrust_sp_hor_prev + jerk_hor;
thrust_sp(0) = thrust_sp_hor(0);
thrust_sp(1) = thrust_sp_hor(1);
}
/* store last thrust vector */
_thrust_sp_prev = thrust_sp;
/* calculate attitude setpoint from thrust vector */ /* calculate attitude setpoint from thrust vector */
if (_control_mode.flag_control_velocity_enabled) { if (_control_mode.flag_control_velocity_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */ /* desired body_z axis = -normalize(thrust_vector) */