From d641d776f7cf841e38a226dc5e33ddc964d3aa7a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 24 Jun 2017 20:13:38 -0400
Subject: [PATCH] mc_pos_control delete unused velocity feed forwards
---
.../mc_pos_control/mc_pos_control_main.cpp | 56 +++++++------------
.../mc_pos_control/mc_pos_control_params.c | 24 --------
2 files changed, 19 insertions(+), 61 deletions(-)
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index befb2e00a8..1624851217 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -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;
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index 852b4e12be..a418bf4e50 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -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
*
--
GitLab