fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 00:09:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: removed special mode switch calculation

Browse Source 
because it is not needed anymore with feed forward thrust
it even produced aggressive twitches when used together with the feed forward thrust
This commit is contained in:
Matthias Grob 2017-01-25 16:08:41 +01:00 committed by Lorenz Meier
parent 63057d7b60
commit 34080be68b
1 changed files with 0 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -278,7 +278,6 @@ private:
float _vel_z_lp;
float _acc_z_lp;
float _takeoff_thrust_sp;
bool control_vel_enabled_prev; /**< previous loop was in velocity controlled mode (control_state.flag_control_velocity_enabled) */
// counters for reset events on position and velocity states
// they are used to identify a reset event
@ -441,7 +440,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_z_lp(0),
_acc_z_lp(0),
_takeoff_thrust_sp(0.0f),
control_vel_enabled_prev(false),
_z_reset_counter(0),
_xy_reset_counter(0),
_vz_reset_counter(0),
@ -1669,7 +1667,6 @@ MulticopterPositionControl::control_position(float dt)
_vel_sp_prev(1) = _vel(1);
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
control_vel_enabled_prev = false;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
@ -1730,27 +1727,6 @@ MulticopterPositionControl::control_position(float dt)
/* velocity error */
math::Vector<3> vel_err = _vel_sp - _vel;
// check if we have switched from a non-velocity controlled mode into a velocity controlled mode
// if yes, then correct xy velocity setpoint such that the attitude setpoint is continuous
if (!control_vel_enabled_prev && _control_mode.flag_control_velocity_enabled) {
matrix::Dcmf Rb = matrix::Quatf(_att_sp.q_d[0], _att_sp.q_d[1], _att_sp.q_d[2], _att_sp.q_d[3]);
// choose velocity xyz setpoint such that the resulting thrust setpoint has the direction
// given by the last attitude setpoint
_vel_sp(0) = _vel(0) + (-Rb(0,
2) * _att_sp.thrust - _thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0);
_vel_sp(1) = _vel(1) + (-Rb(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,
2) * _att_sp.thrust - _thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2);
_vel_sp_prev = _vel_sp;
control_vel_enabled_prev = true;
// compute updated velocity error
vel_err = _vel_sp - _vel;
}
/* thrust vector in NED frame */
math::Vector<3> thrust_sp;
@ -2291,7 +2267,6 @@ MulticopterPositionControl::task_main()
_mode_auto = false;
_reset_int_z = true;
_reset_int_xy = true;
control_vel_enabled_prev = false;
/* store last velocity in case a mode switch to position control occurs */
_vel_sp_prev = _vel;