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

mc_pos_control: set position setpoint during transition based on veloicyt and acceleration

Browse Source
This commit is contained in:
Dennis Mannhart 2017-02-02 17:57:09 +01:00 committed by Lorenz Meier
parent c141d4ca3f
commit f0978fc9e9
1 changed files with 17 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1000,8 +1000,6 @@ MulticopterPositionControl::control_manual(float dt)
bool smooth_pos_transition = do_pos_hold && !_pos_hold_engaged;
/* update hold flags */
_alt_hold_engaged = do_alt_hold;
_pos_hold_engaged = do_pos_hold;
@ -1023,12 +1021,26 @@ MulticopterPositionControl::control_manual(float dt)
/* reset position setpoints when in smooth transition for position*/
if (smooth_pos_transition) {
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
/* time to travel from current velocity to zero velocity */
float delta_t = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)) / _params.acc_hor_max;
/* p pos_sp in xy from max acceleration and current velocity */
math::Vector<2> pos(_pos(0), _pos(1));
math::Vector<2> vel(_vel(0), _vel(1));
math::Vector<2> pos_sp = pos + vel * delta_t - vel.normalized() * 0.5f * _params.acc_hor_max * delta_t *delta_t;
_pos_sp(0) = pos_sp(0);
_pos_sp(1) = pos_sp(1);
}
if (smooth_alt_transition) {
_pos_sp(2) = _pos(2);
/* get max acceleration */
float max_acc_z = (_vel(2) < 0.0f ? _params.acc_down_max : -_params.acc_up_max);
/* time to travel from current velocity to zero velocity */
float delta_t = -_vel(2) / max_acc_z;
/* set desired position setpoint assuming max acceleraiton */
_pos_sp(2) = _pos(2) + _vel(2) * delta_t + 0.5f * max_acc_z * delta_t *delta_t;
}
if (_vehicle_land_detected.landed) {
@ -1749,17 +1761,6 @@ MulticopterPositionControl::control_position(float dt)
} else {
thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d)
+ _thrust_int - math::Vector<3>(0.0f, 0.0f, _params.thr_hover);
/* at low thrust in z direction while position hold is engaged, we set thrust in xy to 0
* until thrust in z again reaches large enough thrust. this helps to prevent the jerk when
* flying upward while position hold is engaged
*/
bool set_thrust_xy_to_0 = _pos_hold_engaged && _alt_hold_engaged && (-thrust_sp(2) < 2.0f * _params.thr_min);
if (set_thrust_xy_to_0) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
}
}
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF