Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 14:47:44 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

mc_pos_control delete unused velocity feed forwards

Browse Source
This commit is contained in:
Daniel Agar
2017-06-24 20:13:38 -04:00
parent f947205cbe
commit d641d776f7
2 changed files with 19 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+19 -37
View File
@@ -159,7 +159,6 @@ private:
param_t z_vel_d;
param_t z_vel_max_up;
param_t z_vel_max_down;
param_t z_ff;
param_t slow_land_alt1;
param_t slow_land_alt2;
param_t xy_p;
@@ -168,7 +167,6 @@ private:
param_t xy_vel_d;
param_t xy_vel_max;
param_t xy_vel_cruise;
param_t xy_ff;
param_t tilt_max_air;
param_t land_speed;
param_t tko_speed;
@@ -215,8 +213,7 @@ private:
math::Vector<3> vel_p;
math::Vector<3> vel_i;
math::Vector<3> vel_d;
math::Vector<3> vel_ff;
} _params;
} _params{};
struct map_projection_reference_s _ref_pos;
float _ref_alt;
@@ -248,7 +245,6 @@ 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> _vel_sp_prev;
math::Vector<3> _vel_err_d; /**< derivative of current velocity */
math::Vector<3> _curr_pos_sp; /**< current setpoint of the triplets */
@@ -449,14 +445,13 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params.vel_p.zero();
_params.vel_i.zero();
_params.vel_d.zero();
_params.vel_ff.zero();
_pos.zero();
_pos_sp.zero();
_vel.zero();
_vel_sp.zero();
_vel_prev.zero();
_vel_ff.zero();
_vel_sp_prev.zero();
_vel_err_d.zero();
_curr_pos_sp.zero();
@@ -476,7 +471,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.z_vel_d = param_find("MPC_Z_VEL_D");
_params_handles.z_vel_max_up = param_find("MPC_Z_VEL_MAX_UP");
_params_handles.z_vel_max_down = param_find("MPC_Z_VEL_MAX_DN");
_params_handles.z_ff = param_find("MPC_Z_FF");
_params_handles.xy_p = param_find("MPC_XY_P");
_params_handles.xy_vel_p = param_find("MPC_XY_VEL_P");
@@ -484,7 +478,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.xy_vel_d = param_find("MPC_XY_VEL_D");
_params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX");
_params_handles.xy_vel_cruise = param_find("MPC_XY_CRUISE");
_params_handles.xy_ff = param_find("MPC_XY_FF");
_params_handles.slow_land_alt1 = param_find("MPC_LAND_ALT1");
_params_handles.slow_land_alt2 = param_find("MPC_LAND_ALT2");
@@ -591,13 +584,6 @@ MulticopterPositionControl::parameters_update(bool force)
_params.vel_max_xy = v;
param_get(_params_handles.xy_vel_cruise, &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;
_params.vel_ff(1) = v;
param_get(_params_handles.z_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(2) = v;
param_get(_params_handles.hold_max_xy, &v);
_params.hold_max_xy = math::max(0.f, v);
param_get(_params_handles.hold_max_z, &v);
@@ -859,8 +845,7 @@ MulticopterPositionControl::limit_altitude()
float delta_t = -_vel(2) / _params.acc_down_max;
/* predicted position */
float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f *
_params.acc_down_max * delta_t *delta_t;
float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * _params.acc_down_max * delta_t *delta_t;
if (pos_z_next <= -_vehicle_land_detected.alt_max) {
_pos_sp(2) = -_vehicle_land_detected.alt_max;
@@ -981,14 +966,14 @@ MulticopterPositionControl::control_manual(float dt)
}
/* prepare yaw to rotate into NED frame */
float yaw_input_fame = _control_mode.flag_control_fixed_hdg_enabled ? _yaw_takeoff : _att_sp.yaw_body;
float yaw_input_frame = _control_mode.flag_control_fixed_hdg_enabled ? _yaw_takeoff : _att_sp.yaw_body;
/* prepare cruise speed (m/s) vector to scale the velocity setpoint */
float vel_mag = (_velocity_hor_manual.get() < _vel_max_xy) ? _velocity_hor_manual.get() : _vel_max_xy;
matrix::Vector3f vel_cruise_scale(vel_mag, vel_mag, (man_vel_sp(2) > 0.0f) ? _params.vel_max_down : _params.vel_max_up);
/* 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);
man_vel_sp = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, yaw_input_frame)) * man_vel_sp.emult(vel_cruise_scale);
/*
* assisted velocity mode: user controls velocity, but if velocity is small enough, position
@@ -1021,12 +1006,11 @@ MulticopterPositionControl::control_manual(float dt)
/* time to travel from current velocity to zero velocity */
float delta_t = fabsf(_vel(2) / max_acc_z);
/* set desired position setpoint assuming max acceleraiton */
/* set desired position setpoint assuming max acceleration */
_pos_sp(2) = _pos(2) + _vel(2) * delta_t + 0.5f * max_acc_z * delta_t *delta_t;
_alt_hold_engaged = true;
}
}
/* check horizontal hold engaged flag */
@@ -1056,7 +1040,7 @@ MulticopterPositionControl::control_manual(float dt)
}
}
/* set requested velocity setpoitns */
/* set requested velocity setpoints */
if (!_alt_hold_engaged) {
_pos_sp(2) = _pos(2);
_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
@@ -1397,7 +1381,7 @@ void MulticopterPositionControl::control_auto(float dt)
}
}
//only project setpoints if they are finite, else use current position
// only project setpoints if they are finite, else use current position
if (PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
_curr_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
@@ -1424,7 +1408,6 @@ void MulticopterPositionControl::control_auto(float dt)
}
}
if (_pos_sp_triplet.next.valid) {
map_projection_project(&_ref_pos,
_pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
@@ -1588,17 +1571,14 @@ void MulticopterPositionControl::control_auto(float dt)
// During takeoff and landing, we better put it down again.
_att_sp.landing_gear = -1.0f;
} else {
// For the rest of the setpoint types, just leave it as is.
}
} else {
/* idle or triplet not valid, set velocity setpoint to zero */
_vel_sp.zero();
_run_pos_control = false;
_run_alt_control = false;
}
}
@@ -1651,8 +1631,6 @@ MulticopterPositionControl::update_velocity_derivative()
void
MulticopterPositionControl::do_control(float dt)
{
_vel_ff.zero();
/* by default, run position/altitude controller. the control_* functions
* can disable this and run velocity controllers directly in this cycle */
_run_pos_control = true;
@@ -1668,7 +1646,7 @@ MulticopterPositionControl::do_control(float dt)
control_manual(dt);
_mode_auto = false;
/* we set tiplets to false
/* we set triplets to false
* this ensures that when switching to auto, the position
* controller will not use the old triplets but waits until triplets
* have been updated */
@@ -1702,7 +1680,6 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
{
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) {
// If for any reason, we get a NaN position setpoint, we better just stay where we are.
if (PX4_ISFINITE(_pos_sp(0)) && PX4_ISFINITE(_pos_sp(1))) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
@@ -1754,6 +1731,7 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
if (_pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& !_control_mode.flag_control_manual_enabled) {
_vel_sp(2) = math::max(_vel_sp(2), -_params.tko_speed);
}
@@ -1772,8 +1750,10 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
/* special velocity setpoint limitation for smooth takeoff */
if (_in_takeoff) {
_in_takeoff = _takeoff_vel_limit < -_vel_sp(2);
/* ramp vertical velocity limit up to takeoff speed */
_takeoff_vel_limit += _params.tko_speed * dt / _takeoff_ramp_time.get();
/* limit vertical velocity to the current ramp value */
_vel_sp(2) = math::max(_vel_sp(2), -_takeoff_vel_limit);
}
@@ -1864,8 +1844,10 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
float tilt_max = _params.tilt_max_air;
float thr_max = _params.thr_max;
/* filter vel_z over 1/8sec */
_vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);
/* filter vel_z change over 1/8sec */
float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;
_acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;
@@ -1890,8 +1872,9 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
/* descend stabilized, we're landing */
if (!_in_landing && !_lnd_reached_ground
&& (float)fabsf(_acc_z_lp) < 0.1f
&& (fabsf(_acc_z_lp) < 0.1f)
&& _vel_z_lp > 0.6f * _params.land_speed) {
_in_landing = true;
}
@@ -1924,9 +1907,9 @@ MulticopterPositionControl::calculate_thrust_setpoint(float dt)
/* if we suddenly fall, reset landing logic and remove thrust limit */
if (_lnd_reached_ground
/* XXX: magic value, assuming free fall above 4m/s2 acceleration */
&& (_acc_z_lp > 4.0f
|| _vel_z_lp > 2.0f * _params.land_speed)) {
/* XXX: magic value, assuming free fall above 4 m/s^2 acceleration */
&& (_acc_z_lp > 4.0f || _vel_z_lp > 2.0f * _params.land_speed)) {
thr_max = _params.thr_max;
_in_landing = true;
_lnd_reached_ground = false;
@@ -2303,7 +2286,6 @@ MulticopterPositionControl::task_main()
_reset_int_xy = true;
_reset_yaw_sp = true;
_yaw_takeoff = _yaw;
}
was_armed = _control_mode.flag_armed;
src/modules/mc_pos_control/mc_pos_control_params.c
-24
View File
@@ -186,18 +186,6 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.0f);
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.0f);
/**
* Vertical velocity feed forward
*
* Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f);
/**
* Proportional gain for horizontal position error
*
@@ -299,18 +287,6 @@ PARAM_DEFINE_FLOAT(MPC_TARGET_THRE, 15.0f);
*/
PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 8.0f);
/**
* Horizontal velocity feed forward
*
* Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
/**
* Maximum tilt angle in air
*