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

mc_pos_control: refactoring only in manual velocity setpoint generation

Browse Source
This commit is contained in:
Matthias Grob
2017-03-22 15:21:34 +01:00
committed by Dennis Mannhart
parent 713ba45876
commit 470c3fdc06
1 changed files with 35 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+35 -44
View File
@@ -922,18 +922,15 @@ MulticopterPositionControl::control_manual(float dt)
/* /*
* Map from stick input to velocity setpoint * Map from stick input to velocity setpoint
* xy in local frame, z in global frame */
*/
/* setpoint in normalized range [-1,1] */ /* velocity setpoint commanded by user stick input */
math::Vector<2> req_vel_sp_xy; matrix::Vector3f man_vel_sp;
req_vel_sp_xy.zero();
float req_vel_sp_z = 0.0f;
if (_control_mode.flag_control_altitude_enabled) { if (_control_mode.flag_control_altitude_enabled) {
/* set vertical velocity setpoint with throttle stick */ /* set vertical velocity setpoint with throttle stick */
req_vel_sp_z = -math::expo_deadzone((_manual.z - 0.5f) * 2.f, _z_vel_man_expo.get(), _params.hold_dz); man_vel_sp(2) = -math::expo_deadzone((_manual.z - 0.5f) * 2.f, _z_vel_man_expo.get(), _params.hold_dz);
/* reset alt setpoint to current altitude if needed */ /* reset alt setpoint to current altitude if needed */
reset_alt_sp(); reset_alt_sp();
@@ -941,55 +938,53 @@ MulticopterPositionControl::control_manual(float dt)
if (_control_mode.flag_control_position_enabled) { if (_control_mode.flag_control_position_enabled) {
/* set horizontal velocity setpoint with roll/pitch stick */ /* set horizontal velocity setpoint with roll/pitch stick */
req_vel_sp_xy(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_dz); man_vel_sp(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_dz);
req_vel_sp_xy(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_dz); man_vel_sp(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_dz);
/* saturarate such that magnitude is never larger than 1 */ const float man_vel_hor_length = ((matrix::Vector2f)man_vel_sp.slice<2, 1>(0, 0)).length();
if (req_vel_sp_xy.length() > 1.0f) {
req_vel_sp_xy = req_vel_sp_xy.normalized(); /* saturate such that magnitude is never larger than 1 */
if (man_vel_hor_length > 1.0f) {
man_vel_sp(0) /= man_vel_hor_length;
man_vel_sp(1) /= man_vel_hor_length;
} }
/* reset position setpoint to current position if needed */ /* reset position setpoint to current position if needed */
reset_pos_sp(); reset_pos_sp();
} }
/* scale requested velocity setpoint to cruisespeed and rotate around yaw */ /* prepare yaw to rotate into NED frame */
math::Vector<3> cruising_scale(_params.vel_cruise_xy, float yaw_input_fame = _control_mode.flag_control_fixed_hdg_enabled ? _yaw_takeoff : _local_pos.yaw;
_params.vel_cruise_xy,
(req_vel_sp_z > 0.0f) ? _params.vel_max_down : _params.vel_max_up);
math::Vector<3> req_vel_sp_scaled(req_vel_sp_xy(0), req_vel_sp_xy(1), req_vel_sp_z);
/* scale velocity setpoint to cruise speed (m/s) and rotate around yaw to NED frame */ /* prepare cruise speed (m/s) vector to scale the velocity setpoint */
math::Matrix<3, 3> R_input_fame; matrix::Vector3f vel_cruise_scale(_params.vel_cruise_xy,
_params.vel_cruise_xy,
(man_vel_sp(2) > 0.0f) ? _params.vel_max_down : _params.vel_max_up);
if (_control_mode.flag_control_fixed_hdg_enabled) { /* setpoint in NED frame and scaled to cruise velocity */
R_input_fame.from_euler(0.0f, 0.0f, _yaw_takeoff); man_vel_sp = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, yaw_input_fame)) * man_vel_sp.emult(vel_cruise_scale);
} else {
R_input_fame.from_euler(0.0f, 0.0f, _local_pos.yaw);
}
req_vel_sp_scaled = R_input_fame * req_vel_sp_scaled.emult(
cruising_scale); // in NED and scaled to actual velocity;
/* /*
* assisted velocity mode: user controls velocity, but if velocity is small enough, position * assisted velocity mode: user controls velocity, but if velocity is small enough, position
* hold is activated for the corresponding axis * hold is activated for the corresponding axis
*/ */
/* want to get/stay in altitude hold if user has z stick in the middle (accounted for deadzone already) */
const bool alt_hold_desired = _control_mode.flag_control_altitude_enabled && fabsf(man_vel_sp(2)) < FLT_EPSILON;
/* want to get/stay in position hold if user has xy stick in the middle (accounted for deadzone already) */
const bool pos_hold_desired = _control_mode.flag_control_position_enabled &&
fabsf(man_vel_sp(0)) < FLT_EPSILON && fabsf(man_vel_sp(1)) < FLT_EPSILON;
/* check vertical hold engaged flag */ /* check vertical hold engaged flag */
if (_alt_hold_engaged) { if (_alt_hold_engaged) {
_alt_hold_engaged = alt_hold_desired;
/* only switch back to velocity control if user moves stick */
_alt_hold_engaged = _control_mode.flag_control_altitude_enabled && (fabsf(req_vel_sp_z) < FLT_EPSILON);
} else { } else {
/* check if we switch to alt_hold_engaged */ /* check if we switch to alt_hold_engaged */
bool smooth_alt_transition = _control_mode.flag_control_altitude_enabled && bool smooth_alt_transition = alt_hold_desired &&
fabsf(req_vel_sp_z) < FLT_EPSILON &&
(_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z); (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z);
/* during transition predict setpoint forward */ /* during transition predict setpoint forward */
@@ -1011,17 +1006,13 @@ MulticopterPositionControl::control_manual(float dt)
/* check horizontal hold engaged flag */ /* check horizontal hold engaged flag */
if (_pos_hold_engaged) { if (_pos_hold_engaged) {
_pos_hold_engaged = pos_hold_desired;
/* only switch back to velocity control if user moves stick */
_pos_hold_engaged = _control_mode.flag_control_position_enabled && (fabsf(req_vel_sp_xy(0)) < FLT_EPSILON)
&& (fabsf(req_vel_sp_xy(1)) < FLT_EPSILON);
} else { } else {
/* check if we switch to pos_hold_engaged */ /* check if we switch to pos_hold_engaged */
float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)); float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1));
bool smooth_pos_transition = _control_mode.flag_control_position_enabled && bool smooth_pos_transition = pos_hold_desired &&
(fabsf(req_vel_sp_xy(0)) < FLT_EPSILON && fabsf(req_vel_sp_xy(1)) < FLT_EPSILON) &&
(_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy); (_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy);
/* during transition predict setpoint forward */ /* during transition predict setpoint forward */
@@ -1046,15 +1037,15 @@ MulticopterPositionControl::control_manual(float dt)
if (!_alt_hold_engaged) { if (!_alt_hold_engaged) {
_pos_sp(2) = _pos(2); _pos_sp(2) = _pos(2);
_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */ _run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
_vel_sp(2) = req_vel_sp_scaled(2); _vel_sp(2) = man_vel_sp(2);
} }
if (!_pos_hold_engaged) { if (!_pos_hold_engaged) {
_pos_sp(0) = _pos(0); _pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1); _pos_sp(1) = _pos(1);
_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
_vel_sp(0) = req_vel_sp_scaled(0); _vel_sp(0) = man_vel_sp(0);
_vel_sp(1) = req_vel_sp_scaled(1); _vel_sp(1) = man_vel_sp(1);
} }
if (_vehicle_land_detected.landed) { if (_vehicle_land_detected.landed) {