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: refactoring only in manual velocity setpoint generation

Browse Source
This commit is contained in:
Matthias Grob 2017-03-21 10:45:05 +01:00 committed by Dennis Mannhart
parent 713ba45876
commit 470c3fdc06
1 changed files with 35 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -922,18 +922,15 @@ MulticopterPositionControl::control_manual(float dt)
/*
* Map from stick input to velocity setpoint
* xy in local frame, z in global frame
*/
* Map from stick input to velocity setpoint
*/
/* setpoint in normalized range [-1,1] */
math::Vector<2> req_vel_sp_xy;
req_vel_sp_xy.zero();
float req_vel_sp_z = 0.0f;
/* velocity setpoint commanded by user stick input */
matrix::Vector3f man_vel_sp;
if (_control_mode.flag_control_altitude_enabled) {
/* 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_sp();
@ -941,55 +938,53 @@ MulticopterPositionControl::control_manual(float dt)
if (_control_mode.flag_control_position_enabled) {
/* 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);
req_vel_sp_xy(1) = math::expo_deadzone(_manual.y, _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);
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 */
if (req_vel_sp_xy.length() > 1.0f) {
req_vel_sp_xy = req_vel_sp_xy.normalized();
const float man_vel_hor_length = ((matrix::Vector2f)man_vel_sp.slice<2, 1>(0, 0)).length();
/* 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_pos_sp();
}
/* scale requested velocity setpoint to cruisespeed and rotate around yaw */
math::Vector<3> cruising_scale(_params.vel_cruise_xy,
_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);
/* prepare yaw to rotate into NED frame */
float yaw_input_fame = _control_mode.flag_control_fixed_hdg_enabled ? _yaw_takeoff : _local_pos.yaw;
/* scale velocity setpoint to cruise speed (m/s) and rotate around yaw to NED frame */
math::Matrix<3, 3> R_input_fame;
/* prepare cruise speed (m/s) vector to scale the velocity setpoint */
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) {
R_input_fame.from_euler(0.0f, 0.0f, _yaw_takeoff);
/* setpoint in NED frame and scaled to cruise velocity */
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
*/
/* 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 */
if (_alt_hold_engaged) {
/* 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);
_alt_hold_engaged = alt_hold_desired;
} else {
/* check if we switch to alt_hold_engaged */
bool smooth_alt_transition = _control_mode.flag_control_altitude_enabled &&
fabsf(req_vel_sp_z) < FLT_EPSILON &&
bool smooth_alt_transition = alt_hold_desired &&
(_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z);
/* during transition predict setpoint forward */
@ -1011,17 +1006,13 @@ MulticopterPositionControl::control_manual(float dt)
/* check horizontal hold engaged flag */
if (_pos_hold_engaged) {
/* 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);
_pos_hold_engaged = pos_hold_desired;
} else {
/* check if we switch to pos_hold_engaged */
float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1));
bool smooth_pos_transition = _control_mode.flag_control_position_enabled &&
(fabsf(req_vel_sp_xy(0)) < FLT_EPSILON && fabsf(req_vel_sp_xy(1)) < FLT_EPSILON) &&
bool smooth_pos_transition = pos_hold_desired &&
(_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy);
/* during transition predict setpoint forward */
@ -1046,15 +1037,15 @@ MulticopterPositionControl::control_manual(float dt)
if (!_alt_hold_engaged) {
_pos_sp(2) = _pos(2);
_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) {
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
_vel_sp(0) = req_vel_sp_scaled(0);
_vel_sp(1) = req_vel_sp_scaled(1);
_vel_sp(0) = man_vel_sp(0);
_vel_sp(1) = man_vel_sp(1);
}
if (_vehicle_land_detected.landed) {