Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

Stick to velocity fix (#6825)

Browse Source 
* mc_pos_control: use just float for vel and cruise in xy

* mc_pos_control: stick map saturate magnitude to 1

* mc_pos_control: take minimum cruising speed for auto

* mc_pos_control: cruise speed triplet higher than from mc_pos_control

mc_pos_control: fix if for cruise in auto

* mc_pos_control: use PX4_ISFINITE criteria
This commit is contained in:
Dennis Mannhart
2017-03-21 07:58:47 +01:00
committed by GitHub
parent 7d62aa6a6d
commit 8f1e851911
1 changed files with 28 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+28 -28
View File
@@ -232,6 +232,8 @@ private:
float acc_hor_max;
float acc_up_max;
float acc_down_max;
float vel_max_xy;
float vel_cruise_xy;
float vel_max_up;
float vel_max_down;
float xy_vel_man_expo;
@@ -244,8 +246,6 @@ private:
math::Vector<3> vel_i;
math::Vector<3> vel_d;
math::Vector<3> vel_ff;
math::Vector<3> vel_max;
math::Vector<2> vel_cruise;
} _params;
struct map_projection_reference_s _ref_pos;
@@ -471,8 +471,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params.vel_p.zero();
_params.vel_i.zero();
_params.vel_d.zero();
_params.vel_max.zero();
_params.vel_cruise.zero();
_params.vel_ff.zero();
_pos.zero();
@@ -529,6 +527,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.opt_recover = param_find("VT_OPT_RECOV_EN");
_params_handles.xy_vel_man_expo = param_find("MPC_XY_MAN_EXPO");
/* fetch initial parameter values */
parameters_update(true);
}
@@ -610,16 +609,13 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.z_vel_d, &v);
_params.vel_d(2) = v;
param_get(_params_handles.xy_vel_max, &v);
_params.vel_max(0) = v;
_params.vel_max(1) = v;
_params.vel_max_xy = v;
param_get(_params_handles.z_vel_max_up, &v);
_params.vel_max_up = v;
_params.vel_max(2) = v;
param_get(_params_handles.z_vel_max_down, &v);
_params.vel_max_down = v;
param_get(_params_handles.xy_vel_cruise, &v);
_params.vel_cruise(0) = v;
_params.vel_cruise(1) = v;
_params.vel_cruise_xy = v;
param_get(_params_handles.xy_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(0) = v;
@@ -647,12 +643,14 @@ MulticopterPositionControl::parameters_update(bool force)
_params.xy_vel_man_expo = v;
/* make sure that vel_cruise_xy is always smaller than vel_max */
_params.vel_cruise_xy = math::min(_params.vel_cruise_xy, _params.vel_max_xy);
/*
* increase the maximum horizontal acceleration such that stopping
* within 1 s from full speed is feasible
*/
_params.acc_hor_max = math::max(_params.vel_cruise(0), _params.acc_hor_max);
_params.acc_hor_max = math::max(_params.vel_cruise_xy, _params.acc_hor_max);
param_get(_params_handles.alt_mode, &v_i);
_params.alt_mode = v_i;
@@ -978,14 +976,19 @@ MulticopterPositionControl::control_manual(float dt)
req_vel_sp_xy(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_xy_dz);
req_vel_sp_xy(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_xy_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();
}
/* reset position setpoint to current position if needed */
reset_pos_sp();
}
/* scale requested velocity setpoint to cruisespeed and rotate around yaw */
math::Vector<3> vel_cruise(_params.vel_cruise(0),
_params.vel_cruise(1),
(req_vel_sp_z > 0.0f) ? _params.vel_max_down : _params.vel_max_up);
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);
/* scale velocity setpoint to cruise speed (m/s) and rotate around yaw to NED frame */
@@ -1000,7 +1003,7 @@ MulticopterPositionControl::control_manual(float dt)
}
req_vel_sp_scaled = R_input_fame * req_vel_sp_scaled.emult(
vel_cruise); // in NED and scaled to actual velocity;
cruising_scale); // in NED and scaled to actual velocity;
/*
* assisted velocity mode: user controls velocity, but if velocity is small enough, position
@@ -1456,17 +1459,15 @@ void MulticopterPositionControl::control_auto(float dt)
if (current_setpoint_valid &&
(_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) {
float cruising_speed_xy = (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed)
&& (_pos_sp_triplet.current.cruising_speed > 0.1f)) ?
_pos_sp_triplet.current.cruising_speed : _params.vel_cruise_xy ;
float cruising_speed_z = (curr_sp(2) > _pos(2)) ? _params.vel_max_down : _params.vel_max_up;
/* scaled space: 1 == position error resulting max allowed speed */
math::Vector<3> cruising_speed(_params.vel_cruise(0),
_params.vel_cruise(1),
_params.vel_max_up);
if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) &&
_pos_sp_triplet.current.cruising_speed > 0.1f) {
cruising_speed(0) = _pos_sp_triplet.current.cruising_speed;
cruising_speed(1) = _pos_sp_triplet.current.cruising_speed;
}
math::Vector<3> cruising_speed(cruising_speed_xy,
cruising_speed_xy,
cruising_speed_z);
math::Vector<3> scale = _params.pos_p.edivide(cruising_speed);
@@ -1703,10 +1704,9 @@ MulticopterPositionControl::control_position(float dt)
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
_vel_sp(1) * _vel_sp(1));
if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
if (vel_norm_xy > _params.vel_max_xy) {
_vel_sp(0) = _vel_sp(0) * _params.vel_max_xy / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _params.vel_max_xy / vel_norm_xy;
}
/* make sure velocity setpoint is saturated in z*/