Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

mc_pos_control: use one method to wrap yaw speed instead of triplicate

Browse Source 
code
This commit is contained in:
Martina
2018-04-06 16:27:42 +02:00
committed by Daniel Agar
parent 8b20c66cfc
commit 59c7fc5d96
1 changed files with 24 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+24 -36
View File
@@ -375,6 +375,8 @@ private:
void vel_sp_slewrate();
void wrap_yaw_speed(float yaw_speed);
void update_velocity_derivative();
void do_control();
@@ -1618,18 +1620,8 @@ MulticopterPositionControl::control_offboard()
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_pos_sp_triplet.current.yawspeed_valid) {
float yaw_target = wrap_pi(_att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * _dt);
float yaw_offs = wrap_pi(yaw_target - _yaw);
const float yaw_rate_max = (_man_yaw_max < _global_yaw_max) ? _man_yaw_max : _global_yaw_max;
const float yaw_offset_max = yaw_rate_max / _mc_att_yaw_p.get();
// If the yaw offset became too big for the system to track stop
// shifting it, only allow if it would make the offset smaller again.
if (fabsf(yaw_offs) < yaw_offset_max ||
(_pos_sp_triplet.current.yawspeed > 0 && yaw_offs < 0) ||
(_pos_sp_triplet.current.yawspeed < 0 && yaw_offs > 0)) {
_att_sp.yaw_body = yaw_target;
}
wrap_yaw_speed(_pos_sp_triplet.current.yawspeed);
}
} else {
@@ -1860,21 +1852,8 @@ void MulticopterPositionControl::control_auto()
yaw_speed = _traj_wp_avoidance.point_0[trajectory_waypoint_s::YAW_SPEED];
}
/* we want to know the real constraint, and global overrides manual */
const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
_params.global_yaw_max;
const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;
wrap_yaw_speed(yaw_speed);
float yaw_target = _wrap_pi(_att_sp.yaw_body + yaw_speed * _dt);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it, only allow if it would make the offset smaller again.
if (fabsf(yaw_offs) < yaw_offset_max ||
(_att_sp.yaw_sp_move_rate > 0 && yaw_offs < 0) ||
(_att_sp.yaw_sp_move_rate < 0 && yaw_offs > 0)) {
_att_sp.yaw_body = yaw_target;
}
} else {
@@ -2326,6 +2305,25 @@ void MulticopterPositionControl::control_auto()
}
}
void
MulticopterPositionControl::wrap_yaw_speed(float yaw_speed)
{
/* we want to know the real constraint, and global overrides manual */
const float yaw_rate_max = (_man_yaw_max < _global_yaw_max) ? _man_yaw_max : _global_yaw_max;
const float yaw_offset_max = yaw_rate_max / _mc_att_yaw_p.get();
float yaw_target = _wrap_pi(_att_sp.yaw_body + yaw_speed * _dt);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it, only allow if it would make the offset smaller again.
if (fabsf(yaw_offs) < yaw_offset_max ||
(yaw_speed > 0 && yaw_offs < 0) ||
(yaw_speed < 0 && yaw_offs > 0)) {
_att_sp.yaw_body = yaw_target;
}
}
void
MulticopterPositionControl::update_velocity_derivative()
{
@@ -2869,7 +2867,6 @@ MulticopterPositionControl::generate_attitude_setpoint()
/* we want to know the real constraint, and global overrides manual */
const float yaw_rate_max = (_man_yaw_max < _global_yaw_max) ? _man_yaw_max : _global_yaw_max;
const float yaw_offset_max = yaw_rate_max / _mc_att_yaw_p.get();
_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
@@ -2878,16 +2875,7 @@ MulticopterPositionControl::generate_attitude_setpoint()
_att_sp.yaw_sp_move_rate = _traj_wp_avoidance.point_0[trajectory_waypoint_s::YAW_SPEED];
}
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * _dt);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it, only allow if it would make the offset smaller again.
if (fabsf(yaw_offs) < yaw_offset_max ||
(_att_sp.yaw_sp_move_rate > 0 && yaw_offs < 0) ||
(_att_sp.yaw_sp_move_rate < 0 && yaw_offs > 0)) {
_att_sp.yaw_body = yaw_target;
}
wrap_yaw_speed(_att_sp.yaw_sp_move_rate);
}
/* control throttle directly if no climb rate controller is active */