mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
mc_pos_control: manual and offboard control reorganization and cleanup
This commit is contained in:
Anton Babushkin
parent
a9d3e9854f
commit
faaeaeb113
@ -105,6 +105,7 @@ public:
|
||||
int start();
|
||||
|
||||
private:
|
||||
const float alt_ctl_dz = 0.2f;
|
||||
|
||||
bool _task_should_exit; /**< if true, task should exit */
|
||||
int _control_task; /**< task handle for task */
|
||||
@ -184,6 +185,8 @@ private:
|
||||
math::Vector<3> _vel;
|
||||
math::Vector<3> _vel_sp;
|
||||
math::Vector<3> _vel_prev; /**< velocity on previous step */
|
||||
math::Vector<3> _vel_ff;
|
||||
math::Vector<3> _sp_move_rate;
|
||||
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
@ -216,6 +219,16 @@ private:
|
||||
*/
|
||||
void reset_alt_sp();
|
||||
|
||||
/**
|
||||
* Set position setpoint using manual control
|
||||
*/
|
||||
void control_manual(float dt);
|
||||
|
||||
/**
|
||||
* Set position setpoint using offboard control
|
||||
*/
|
||||
void control_offboard(float dt);
|
||||
|
||||
/**
|
||||
* Select between barometric and global (AMSL) altitudes
|
||||
*/
|
||||
@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
||||
_vel.zero();
|
||||
_vel_sp.zero();
|
||||
_vel_prev.zero();
|
||||
_vel_ff.zero();
|
||||
_sp_move_rate.zero();
|
||||
|
||||
_params_handles.thr_min = param_find("MPC_THR_MIN");
|
||||
_params_handles.thr_max = param_find("MPC_THR_MAX");
|
||||
@ -515,6 +530,120 @@ MulticopterPositionControl::reset_alt_sp()
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::control_manual(float dt)
|
||||
{
|
||||
_sp_move_rate.zero();
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
/* move altitude setpoint with throttle stick */
|
||||
_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
/* move position setpoint with roll/pitch stick */
|
||||
_sp_move_rate(0) = _manual.x;
|
||||
_sp_move_rate(1) = _manual.y;
|
||||
}
|
||||
|
||||
/* limit setpoint move rate */
|
||||
float sp_move_norm = _sp_move_rate.length();
|
||||
|
||||
if (sp_move_norm > 1.0f) {
|
||||
_sp_move_rate /= sp_move_norm;
|
||||
}
|
||||
|
||||
/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
|
||||
math::Matrix<3, 3> R_yaw_sp;
|
||||
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
|
||||
_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
/* reset alt setpoint to current altitude if needed */
|
||||
reset_alt_sp();
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
/* reset position setpoint to current position if needed */
|
||||
reset_pos_sp();
|
||||
}
|
||||
|
||||
/* feed forward setpoint move rate with weight vel_ff */
|
||||
_vel_ff = _sp_move_rate.emult(_params.vel_ff);
|
||||
|
||||
/* move position setpoint */
|
||||
_pos_sp += _sp_move_rate * dt;
|
||||
|
||||
/* check if position setpoint is too far from actual position */
|
||||
math::Vector<3> pos_sp_offs;
|
||||
pos_sp_offs.zero();
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
|
||||
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
|
||||
}
|
||||
|
||||
float pos_sp_offs_norm = pos_sp_offs.length();
|
||||
|
||||
if (pos_sp_offs_norm > 1.0f) {
|
||||
pos_sp_offs /= pos_sp_offs_norm;
|
||||
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::control_offboard(float dt)
|
||||
{
|
||||
bool updated;
|
||||
orb_check(_pos_sp_triplet_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
|
||||
}
|
||||
|
||||
if (_pos_sp_triplet.current.valid) {
|
||||
if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
|
||||
/* control position */
|
||||
_pos_sp(0) = _pos_sp_triplet.current.x;
|
||||
_pos_sp(1) = _pos_sp_triplet.current.y;
|
||||
_pos_sp(2) = _pos_sp_triplet.current.z;
|
||||
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
|
||||
|
||||
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
|
||||
/* control velocity */
|
||||
/* reset position setpoint to current position if needed */
|
||||
reset_pos_sp();
|
||||
|
||||
/* set position setpoint move rate */
|
||||
_sp_move_rate(0) = _pos_sp_triplet.current.vx;
|
||||
_sp_move_rate(1) = _pos_sp_triplet.current.vy;
|
||||
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
/* reset alt setpoint to current altitude if needed */
|
||||
reset_alt_sp();
|
||||
|
||||
/* set altitude setpoint move rate */
|
||||
_sp_move_rate(2) = _pos_sp_triplet.current.vz;
|
||||
}
|
||||
|
||||
/* feed forward setpoint move rate with weight vel_ff */
|
||||
_vel_ff = _sp_move_rate.emult(_params.vel_ff);
|
||||
|
||||
/* move position setpoint */
|
||||
_pos_sp += _sp_move_rate * dt;
|
||||
|
||||
} else {
|
||||
reset_pos_sp();
|
||||
reset_alt_sp();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::task_main()
|
||||
{
|
||||
@ -553,13 +682,6 @@ MulticopterPositionControl::task_main()
|
||||
|
||||
hrt_abstime t_prev = 0;
|
||||
|
||||
const float alt_ctl_dz = 0.2f;
|
||||
|
||||
math::Vector<3> sp_move_rate;
|
||||
sp_move_rate.zero();
|
||||
|
||||
float yaw_sp_move_rate;
|
||||
|
||||
math::Vector<3> thrust_int;
|
||||
thrust_int.zero();
|
||||
math::Matrix<3, 3> R;
|
||||
@ -618,138 +740,17 @@ MulticopterPositionControl::task_main()
|
||||
_vel(1) = _local_pos.vy;
|
||||
_vel(2) = _local_pos.vz;
|
||||
|
||||
sp_move_rate.zero();
|
||||
_vel_ff.zero();
|
||||
_sp_move_rate.zero();
|
||||
|
||||
/* select control source */
|
||||
if (_control_mode.flag_control_manual_enabled) {
|
||||
/* manual control */
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
/* reset alt setpoint to current altitude if needed */
|
||||
reset_alt_sp();
|
||||
|
||||
/* move altitude setpoint with throttle stick */
|
||||
sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
/* reset position setpoint to current position if needed */
|
||||
reset_pos_sp();
|
||||
|
||||
/* move position setpoint with roll/pitch stick */
|
||||
sp_move_rate(0) = _manual.x;
|
||||
sp_move_rate(1) = _manual.y;
|
||||
}
|
||||
|
||||
/* limit setpoint move rate */
|
||||
float sp_move_norm = sp_move_rate.length();
|
||||
|
||||
if (sp_move_norm > 1.0f) {
|
||||
sp_move_rate /= sp_move_norm;
|
||||
}
|
||||
|
||||
/* scale to max speed and rotate around yaw */
|
||||
math::Matrix<3, 3> R_yaw_sp;
|
||||
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
|
||||
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
|
||||
|
||||
/* move position setpoint */
|
||||
_pos_sp += sp_move_rate * dt;
|
||||
|
||||
/* check if position setpoint is too far from actual position */
|
||||
math::Vector<3> pos_sp_offs;
|
||||
pos_sp_offs.zero();
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
|
||||
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
|
||||
}
|
||||
|
||||
float pos_sp_offs_norm = pos_sp_offs.length();
|
||||
|
||||
if (pos_sp_offs_norm > 1.0f) {
|
||||
pos_sp_offs /= pos_sp_offs_norm;
|
||||
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
|
||||
}
|
||||
control_manual(dt);
|
||||
|
||||
} else if (_control_mode.flag_control_offboard_enabled) {
|
||||
/* Offboard control */
|
||||
bool updated;
|
||||
orb_check(_pos_sp_triplet_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
|
||||
}
|
||||
|
||||
if (_pos_sp_triplet.current.valid) {
|
||||
|
||||
if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
|
||||
|
||||
_pos_sp(0) = _pos_sp_triplet.current.x;
|
||||
_pos_sp(1) = _pos_sp_triplet.current.y;
|
||||
_pos_sp(2) = _pos_sp_triplet.current.z;
|
||||
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
|
||||
|
||||
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
|
||||
/* reset position setpoint to current position if needed */
|
||||
reset_pos_sp();
|
||||
/* move position setpoint with roll/pitch stick */
|
||||
sp_move_rate(0) = _pos_sp_triplet.current.vx;
|
||||
sp_move_rate(1) = _pos_sp_triplet.current.vy;
|
||||
yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed;
|
||||
_att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt;
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
/* reset alt setpoint to current altitude if needed */
|
||||
reset_alt_sp();
|
||||
|
||||
/* move altitude setpoint with throttle stick */
|
||||
sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);;
|
||||
}
|
||||
|
||||
/* limit setpoint move rate */
|
||||
float sp_move_norm = sp_move_rate.length();
|
||||
|
||||
if (sp_move_norm > 1.0f) {
|
||||
sp_move_rate /= sp_move_norm;
|
||||
}
|
||||
|
||||
/* scale to max speed and rotate around yaw */
|
||||
math::Matrix<3, 3> R_yaw_sp;
|
||||
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
|
||||
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
|
||||
|
||||
/* move position setpoint */
|
||||
_pos_sp += sp_move_rate * dt;
|
||||
|
||||
/* check if position setpoint is too far from actual position */
|
||||
math::Vector<3> pos_sp_offs;
|
||||
pos_sp_offs.zero();
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
|
||||
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
|
||||
}
|
||||
|
||||
float pos_sp_offs_norm = pos_sp_offs.length();
|
||||
|
||||
if (pos_sp_offs_norm > 1.0f) {
|
||||
pos_sp_offs /= pos_sp_offs_norm;
|
||||
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
|
||||
}
|
||||
|
||||
} else {
|
||||
reset_pos_sp();
|
||||
reset_alt_sp();
|
||||
}
|
||||
/* offboard control */
|
||||
control_offboard(dt);
|
||||
|
||||
} else {
|
||||
/* AUTO */
|
||||
@ -823,7 +824,7 @@ MulticopterPositionControl::task_main()
|
||||
/* run position & altitude controllers, calculate velocity setpoint */
|
||||
math::Vector<3> pos_err = _pos_sp - _pos;
|
||||
|
||||
_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
|
||||
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
|
||||
|
||||
if (!_control_mode.flag_control_altitude_enabled) {
|
||||
_reset_alt_sp = true;
|
||||
@ -903,7 +904,7 @@ MulticopterPositionControl::task_main()
|
||||
math::Vector<3> vel_err = _vel_sp - _vel;
|
||||
|
||||
/* derivative of velocity error, not includes setpoint acceleration */
|
||||
math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
|
||||
math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
|
||||
_vel_prev = _vel;
|
||||
|
||||
/* thrust vector in NED frame */
|
||||
|
||||
@ -60,7 +60,7 @@ enum SETPOINT_TYPE
|
||||
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
|
||||
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */
|
||||
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
|
||||
SETPOINT_TYPE_OFFBOARD, /**< setpoint set by offboard */
|
||||
SETPOINT_TYPE_OFFBOARD, /**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
|
||||
};
|
||||
|
||||
struct position_setpoint_s
|
||||
@ -71,9 +71,9 @@ struct position_setpoint_s
|
||||
float y; /**< local position setpoint in m in NED */
|
||||
float z; /**< local position setpoint in m in NED */
|
||||
bool position_valid; /**< true if local position setpoint valid */
|
||||
float vx; /**< local velocity setpoint in m in NED */
|
||||
float vy; /**< local velocity setpoint in m in NED */
|
||||
float vz; /**< local velocity setpoint in m in NED */
|
||||
float vx; /**< local velocity setpoint in m/s in NED */
|
||||
float vy; /**< local velocity setpoint in m/s in NED */
|
||||
float vz; /**< local velocity setpoint in m/s in NED */
|
||||
bool velocity_valid; /**< true if local velocity setpoint valid */
|
||||
double lat; /**< latitude, in deg */
|
||||
double lon; /**< longitude, in deg */
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user