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

mc_pos_control: migrate MPC_*_VEL_* parameter to acceleration scaling

Browse Source 
Before #14212 the velocity control gains used in the multicopter
position controller were defined as a scale between velocity error in
one axis (or it's integral and derivative respectively) and the unit
thrust vector. The problem with this is that the normalization of the
unit thrust vector changes per vehicle or even vehicle configuration
as 0 and 100% thrust get a different physical response. That's why
the gains are now defined as scale between velocity error
(integral/derivative) and the output acceleration in m/s².
This commit is contained in:
Matthias Grob
2020-05-26 14:44:41 +02:00
parent e482081f91
commit d92e66863a
25 changed files with 123 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/Takeoff/Takeoff.cpp
+3 -2
View File
@@ -37,11 +37,12 @@
#include "Takeoff.hpp"
#include <mathlib/mathlib.h>
#include <lib/ecl/geo/geo.h>
void Takeoff::generateInitialRampValue(const float hover_thrust, float velocity_p_gain)
void Takeoff::generateInitialRampValue(float velocity_p_gain)
{
velocity_p_gain = math::max(velocity_p_gain, 0.01f);
_takeoff_ramp_vz_init = -hover_thrust / velocity_p_gain;
_takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain;
}
void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
src/modules/mc_pos_control/Takeoff/Takeoff.hpp
+1 -1
View File
@@ -68,7 +68,7 @@ public:
* @param hover_thrust normalized thrsut value with which the vehicle hovers
* @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust
*/
void generateInitialRampValue(const float hover_thrust, const float velocity_p_gain);
void generateInitialRampValue(const float velocity_p_gain);
/**
* Update the state for the takeoff.
src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp
+2 -1
View File
@@ -34,6 +34,7 @@
#include <gtest/gtest.h>
#include <Takeoff.hpp>
#include <drivers/drv_hrt.h>
#include <lib/ecl/geo/geo.h>
TEST(TakeoffTest, Initialization)
{
@@ -46,7 +47,7 @@ TEST(TakeoffTest, RegularTakeoffRamp)
Takeoff takeoff;
takeoff.setSpoolupTime(1.f);
takeoff.setTakeoffRampTime(2.0);
takeoff.generateInitialRampValue(.5f, 1.f);
takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f);
// disarmed, landed, don't want takeoff
takeoff.updateTakeoffState(false, true, false, 1.f, false, 0);
src/modules/mc_pos_control/mc_pos_control_main.cpp
+11 -13
View File
@@ -150,12 +150,12 @@ private:
// Position Control
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
(ParamFloat<px4::params::MPC_XY_VEL_P>) _param_mpc_xy_vel_p,
(ParamFloat<px4::params::MPC_XY_VEL_I>) _param_mpc_xy_vel_i,
(ParamFloat<px4::params::MPC_XY_VEL_D>) _param_mpc_xy_vel_d,
(ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p,
(ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,
(ParamFloat<px4::params::MPC_Z_VEL_D>) _param_mpc_z_vel_d,
(ParamFloat<px4::params::MPC_XY_VEL_P_ACC>) _param_mpc_xy_vel_p_acc,
(ParamFloat<px4::params::MPC_XY_VEL_I_ACC>) _param_mpc_xy_vel_i_acc,
(ParamFloat<px4::params::MPC_XY_VEL_D_ACC>) _param_mpc_xy_vel_d_acc,
(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>) _param_mpc_z_vel_p_acc,
(ParamFloat<px4::params::MPC_Z_VEL_I_ACC>) _param_mpc_z_vel_i_acc,
(ParamFloat<px4::params::MPC_Z_VEL_D_ACC>) _param_mpc_z_vel_d_acc,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
@@ -362,12 +362,10 @@ MulticopterPositionControl::parameters_update(bool force)
}
_control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get()));
// backwards compatibility scale for velocity gains to non-acceleration based control, needs to be overcome with configuration conversion
const float hover_scale = 20.f;
_control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(),
_param_mpc_z_vel_p.get()) * hover_scale,
Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()) * hover_scale,
Vector3f(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get()) * hover_scale);
_control.setVelocityGains(
Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()),
Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()),
Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get()));
_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
_control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get());
_control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians!
@@ -407,7 +405,7 @@ MulticopterPositionControl::parameters_update(bool force)
// set trigger time for takeoff delay
_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
_takeoff.generateInitialRampValue(_param_mpc_thr_hover.get(), _param_mpc_z_vel_p.get());
_takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get());
if (_wv_controller != nullptr) {
_wv_controller->update_parameters();
src/modules/mc_pos_control/mc_pos_control_params.c
+27 -16
View File
@@ -148,34 +148,40 @@ PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
/**
* Proportional gain for vertical velocity error
*
* @min 0.1
* @max 0.4
* defined as correction acceleration in m/s^2 per m/s velocity error
*
* @min 2.0
* @max 8.0
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.2f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_P_ACC, 4.0f);
/**
* Integral gain for vertical velocity error
*
* defined as correction acceleration in m/s^2 per m velocity integral
*
* Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff.
*
* @min 0.01
* @max 0.1
* @min 0.2
* @max 2.0
* @decimal 3
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.1f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.0f);
/**
* Differential gain for vertical velocity error
*
* defined as correction acceleration in m/s^2 per m/s^2 velocity derivative
*
* @min 0.0
* @max 0.1
* @max 2.0
* @decimal 3
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.0f);
/**
* Maximum vertical ascent velocity
@@ -215,34 +221,39 @@ PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f);
/**
* Proportional gain for horizontal velocity error
*
* @min 0.06
* @max 0.15
* defined as correction acceleration in m/s^2 per m/s velocity error
*
* @min 1.2
* @max 3.0
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.09f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_P_ACC, 1.8f);
/**
* Integral gain for horizontal velocity error
*
* defined as correction acceleration in m/s^2 per m velocity integral
* Non-zero value allows to eliminate steady state errors in the presence of disturbances like wind.
*
* @min 0.0
* @max 3.0
* @max 60.0
* @decimal 3
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_I_ACC, 0.4f);
/**
* Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
*
* @min 0.005
* @max 0.1
* defined as correction acceleration in m/s^2 per m/s^2 velocity derivative
*
* @min 0.1
* @max 2.0
* @decimal 3
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D_ACC, 0.2f);
/**
* Maximum horizontal velocity in mission