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

mc pos control:

Browse Source 
-when switching into vel control mode (xy) choose vel sp such that attitude
setpoint is continuous
This commit is contained in:
tumbili
2016-03-04 08:45:13 +01:00
committed by Roman
parent dcf022f480
commit 989a4a0984
1 changed files with 46 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
+46 -18
View File
@@ -254,6 +254,7 @@ 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) */
/**
* Update our local parameter cache.
@@ -384,7 +385,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_takeoff_jumped(false),
_vel_z_lp(0),
_acc_z_lp(0),
_takeoff_thrust_sp(0.0f)
_takeoff_thrust_sp(0.0f),
control_vel_enabled_prev(false)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
@@ -704,13 +706,14 @@ MulticopterPositionControl::reset_pos_sp()
{
if (_reset_pos_sp) {
_reset_pos_sp = false;
/* shift position setpoint to make attitude setpoint continuous */
_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)
- _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)
- _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1);
//mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
if (_mode_auto) {
// in auto mode we are controlling position directly and we need to reset it such that the attitude setpoint is continuous
_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)) / _params.pos_p(1);
} else {
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
}
}
}
@@ -718,9 +721,13 @@ void
MulticopterPositionControl::reset_alt_sp()
{
if (_reset_alt_sp) {
if (_mode_auto) {
// in auto mode we are controlling altitude directly and we need to reset it such that the attitude setpoint is continuous
_pos_sp(2) = _pos(2) + (_vel(2)) / _params.pos_p(2);
} else {
_pos_sp(2) = _pos(2);
}
_reset_alt_sp = false;
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _vel_sp(2)) / _params.pos_p(2);
//mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
}
}
@@ -930,17 +937,15 @@ void MulticopterPositionControl::control_auto(float dt)
{
/* reset position setpoint on AUTO mode activation or when reentering MC mode */
if (!_mode_auto || _vehicle_status.in_transition_mode || !_vehicle_status.is_rotary_wing) {
_mode_auto = true;
if (_vehicle_status.in_transition_mode || !_vehicle_status.is_rotary_wing) {
_reset_pos_sp = true;
_reset_alt_sp = true;
if (!_mode_auto) {
_mode_auto = true;
}
_reset_pos_sp = true;
_reset_alt_sp = true;
reset_pos_sp();
reset_alt_sp();
/* set current velocity as last target velocity to smooth out transition */
_vel_sp_prev = _vel;
}
//Poll position setpoint
@@ -1161,6 +1166,8 @@ MulticopterPositionControl::task_main()
math::Vector<3> thrust_int;
thrust_int.zero();
math::Matrix<3, 3> R;
R.identity();
@@ -1385,6 +1392,7 @@ MulticopterPositionControl::task_main()
_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) {
@@ -1435,6 +1443,8 @@ MulticopterPositionControl::task_main()
_takeoff_thrust_sp = 0.0f;
}
// limit total horizontal acceleration
math::Vector<2> acc_hor;
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
@@ -1510,8 +1520,25 @@ MulticopterPositionControl::task_main()
/* 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) {
// choose velocity xyz setpoint such that the resulting thrust setpoint has the direction
// given by the last attitude setpoint
_vel_sp(0) = _vel(0) + (-PX4_R(_att_sp.R_body, 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) + (-PX4_R(_att_sp.R_body, 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) + (-PX4_R(_att_sp.R_body, 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(1) = _vel_sp(1);
_vel_sp_prev(2) = _vel_sp(2);
control_vel_enabled_prev = true;
// compute updated velocity error
vel_err = _vel_sp - _vel;
}
/* 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;
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
@@ -1813,6 +1840,7 @@ 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;