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

Rework TPA to have per-component setup and use more stable and intuitive function

Browse Source 
This also adds a ZMR250 config.
This commit is contained in:
Anton Matosov 2016-11-15 01:02:27 -08:00 committed by Lorenz Meier
parent 1bef1ae34a
commit a2c0391bcc
7 changed files with 222 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d/4002_qavr5
View File

@ -28,8 +28,8 @@ then
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.5
param set MC_TPA_BREAK 0.7
param set MC_TPA_SLOPE 1.0
param set MC_TPA_BREAK_P 0.7
param set MC_TPA_RATE_P 0.3
param set PWM_MIN 1075
param set MPC_THR_MIN 0.06
param set MPC_MANTHR_MIN 0.06

4
ROMFS/px4fmu_common/init.d/4009_qav250
View File

@ -39,6 +39,6 @@ then
param set MPC_MANTHR_MIN 0.06
param set CBRK_IO_SAFETY 22027
param set ATT_BIAS_MAX 0.0
param set MC_TPA_BREAK 0.5
param set MC_TPA_SLOPE 1.0
param set MC_TPA_BREAK_P 0.5
param set MC_TPA_RATE_P 0.5
fi

4
ROMFS/px4fmu_common/init.d/4050_generic_250
View File

@ -39,8 +39,8 @@ then
param set MC_YAWRATE_MAX 400.0
param set MC_ACRO_R_MAX 360.0
param set MC_ACRO_P_MAX 360.0
param set MC_TPA_BREAK 0.5
param set MC_TPA_SLOPE 1.0
param set MC_TPA_BREAK_P 0.5
param set MC_TPA_RATE_P 0.5
param set PWM_MIN 1075

55
ROMFS/px4fmu_common/init.d/4080_zmr250 Normal file
View File

@ -0,0 +1,55 @@
#!nsh
#
# @name ZMR250 Racer
#
# @type Quadrotor x
#
# @output AUX1 feed-through of RC AUX1 channel
# @output AUX2 feed-through of RC AUX2 channel
#
# @maintainer Anton Matosov <anton.matosov@gmail.com>
#
sh /etc/init.d/rc.mc_defaults
set MIXER zmr250
set PWM_OUT 1234
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 2.0
param set MC_ROLLRATE_P 0.05
param set MC_ROLLRATE_I 0.2
param set MC_ROLLRATE_D 0.0015
param set MC_ROLL_TC 0.18
param set MC_PITCH_P 2.0
param set MC_PITCHRATE_P 0.05
param set MC_PITCHRATE_I 0.2
param set MC_PITCHRATE_D 0.0015
param set MC_PITCH_TC 0.18
param set MC_YAW_P 1.0
param set MC_YAWRATE_P 0.15
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.5
param set MC_ACRO_R_MAX 1080.0
param set MC_ACRO_P_MAX 1080.0
param set MC_ACRO_Y_MAX 1080.0
param set MC_TPA_BREAK_P 0.5
param set MC_TPA_BREAK_D 0.7
param set MC_TPA_RATE_P 0.5
param set MC_TPA_RATE_D 0.5
param set PWM_MIN 1075
param set PWM_RATE 400
param set PWM_DISARMED 900
param set FAILSAFE 100
# param set NAV_RCL_ACT 6 # Lockdown
param set CBRK_IO_SAFETY 22027
fi

10
ROMFS/px4fmu_common/mixers/zmr250.main.mix Normal file
View File

@ -0,0 +1,10 @@
# R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
R: 4x 7654 10000 10000 0
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000

72
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -100,6 +100,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
#define YAW_DEADZONE 0.05f
#define MIN_TAKEOFF_THRUST 0.2f
#define RATES_I_LIMIT 0.3f
#define TPA_RATE_LOWER_LIMIT 0.05f
#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f
#define ATTITUDE_TC_DEFAULT 0.2f
@ -188,8 +189,12 @@ private:
param_t pitch_rate_i;
param_t pitch_rate_d;
param_t pitch_rate_ff;
param_t tpa_breakpoint;
param_t tpa_slope;
param_t tpa_breakpoint_p;
param_t tpa_breakpoint_i;
param_t tpa_breakpoint_d;
param_t tpa_rate_p;
param_t tpa_rate_i;
param_t tpa_rate_d;
param_t yaw_p;
param_t yaw_rate_p;
param_t yaw_rate_i;
@ -224,8 +229,12 @@ private:
math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */
float yaw_ff; /**< yaw control feed-forward */
float tpa_breakpoint; /**< Throttle PID Attenuation breakpoint */
float tpa_slope; /**< Throttle PID Attenuation slope */
float tpa_breakpoint_p; /**< Throttle PID Attenuation breakpoint */
float tpa_breakpoint_i; /**< Throttle PID Attenuation breakpoint */
float tpa_breakpoint_d; /**< Throttle PID Attenuation breakpoint */
float tpa_rate_p; /**< Throttle PID Attenuation slope */
float tpa_rate_i; /**< Throttle PID Attenuation slope */
float tpa_rate_d; /**< Throttle PID Attenuation slope */
float roll_rate_max;
float pitch_rate_max;
@ -289,6 +298,11 @@ private:
*/
void control_attitude_rates(float dt);
/**
* Throttle PID attenuation.
*/
math::Vector<3> pid_attenuations(float tpa_breakpoint, float tpa_rate);
/**
* Check for vehicle status updates.
*/
@ -398,8 +412,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I");
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
_params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF");
_params_handles.tpa_breakpoint = param_find("MC_TPA_BREAK");
_params_handles.tpa_slope = param_find("MC_TPA_SLOPE");
_params_handles.tpa_breakpoint_p = param_find("MC_TPA_BREAK_P");
_params_handles.tpa_breakpoint_i = param_find("MC_TPA_BREAK_I");
_params_handles.tpa_breakpoint_d = param_find("MC_TPA_BREAK_D");
_params_handles.tpa_rate_p = param_find("MC_TPA_RATE_P");
_params_handles.tpa_rate_i = param_find("MC_TPA_RATE_I");
_params_handles.tpa_rate_d = param_find("MC_TPA_RATE_D");
_params_handles.yaw_p = param_find("MC_YAW_P");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
@ -496,10 +514,12 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.pitch_rate_ff, &v);
_params.rate_ff(1) = v;
param_get(_params_handles.tpa_breakpoint, &v);
_params.tpa_breakpoint = v;
param_get(_params_handles.tpa_slope, &v);
_params.tpa_slope = v;
param_get(_params_handles.tpa_breakpoint_p, &_params.tpa_breakpoint_p);
param_get(_params_handles.tpa_breakpoint_i, &_params.tpa_breakpoint_i);
param_get(_params_handles.tpa_breakpoint_d, &_params.tpa_breakpoint_d);
param_get(_params_handles.tpa_rate_p, &_params.tpa_rate_p);
param_get(_params_handles.tpa_rate_i, &_params.tpa_rate_i);
param_get(_params_handles.tpa_rate_d, &_params.tpa_rate_d);
/* yaw gains */
param_get(_params_handles.yaw_p, &v);
@ -790,6 +810,27 @@ MulticopterAttitudeControl::control_attitude(float dt)
}
}
/*
* Throttle PID attenuation
* Function visualization available here https://www.desmos.com/calculator/gn4mfoddje
* Input: 'tpa_breakpoint', 'tpa_rate', '_thrust_sp'
* Output: 'pidAttenuationPerAxis' vector
*/
math::Vector<3>
MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rate)
{
/* throttle pid attenuation factor */
float tpa = 1.0f - tpa_rate * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint) / (1.0f - tpa_breakpoint);
tpa = fmaxf(TPA_RATE_LOWER_LIMIT, fminf(1.0f, tpa));
math::Vector<3> pidAttenuationPerAxis;
pidAttenuationPerAxis(AXIS_INDEX_ROLL) = tpa;
pidAttenuationPerAxis(AXIS_INDEX_PITCH) = tpa;
pidAttenuationPerAxis(AXIS_INDEX_YAW) = 1.0;
return pidAttenuationPerAxis;
}
/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
@ -809,13 +850,16 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* throttle pid attenuation factor */
float tpa = fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint)));
math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p));
math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i));
math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d));
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
_att_control = rates_p_scaled.emult(rates_err) +
_rates_int +
rates_d_scaled.emult(_rates_prev - rates) / dt +
_params.rate_ff.emult(_rates_sp);
_rates_sp_prev = _rates_sp;
@ -825,7 +869,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
float rate_i = _rates_int(i) + rates_i_scaled(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT &&

124
src/modules/mc_att_control/mc_att_control_params.c
View File

@ -72,6 +72,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_TC, 0.2f);
*
* Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
*
* @unit 1/s
* @min 0.0
* @max 8
* @decimal 2
@ -138,7 +139,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f);
* @min 0.0
* @max 10
* @decimal 2
* @increment 0.0005
* @increment 0.1
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f);
@ -377,36 +378,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f);
PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f);
/**
* Threshold for Throttle PID Attenuation (TPA)
*
* Magnitude of throttle setpoint at which to begin attenuating roll/pitch P gain
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.1
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_TPA_BREAK, 1.0f);
/**
* Slope for Throttle PID Attenuation (TPA)
*
* Rate at which to attenuate roll/pitch P gain
* Attenuation factor is 1.0 when throttle magnitude is below the setpoint
* Above the setpoint, the attenuation factor is (1 - slope*(abs(throttle)-breakpoint))
*
* @min 0.0
* @max 2.0
* @decimal 2
* @increment 0.1
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_TPA_SLOPE, 1.0f);
/**
* Whether to scale outputs by battery power level
* Battery power level scaler
*
* This compensates for voltage drop of the battery over time by attempting to
* normalize performance across the operating range of the battery. The copter
@ -418,3 +390,93 @@ PARAM_DEFINE_FLOAT(MC_TPA_SLOPE, 1.0f);
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0);
/**
* TPA P Breakpoint
*
* Throttle PID Attenuation (TPA)
* Magnitude of throttle setpoint at which to begin attenuating roll/pitch P gain
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.1
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_TPA_BREAK_P, 1.0f);
/**
* TPA I Breakpoint
*
* Throttle PID Attenuation (TPA)
* Magnitude of throttle setpoint at which to begin attenuating roll/pitch I gain
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.1
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_TPA_BREAK_I, 1.0f);
/**
* TPA D Breakpoint
*
* Throttle PID Attenuation (TPA)
* Magnitude of throttle setpoint at which to begin attenuating roll/pitch D gain
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.1
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_TPA_BREAK_D, 1.0f);
/**
* TPA Rate P
*
* Throttle PID Attenuation (TPA)
* Rate at which to attenuate roll/pitch P gain
* Attenuation factor is 1.0 when throttle magnitude is below the setpoint
* Above the setpoint, the attenuation factor is (1 - rate * (throttle - breakpoint) / (1.0 - breakpoint))
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_TPA_RATE_P, 0.0f);
/**
* TPA Rate I
*
* Throttle PID Attenuation (TPA)
* Rate at which to attenuate roll/pitch I gain
* Attenuation factor is 1.0 when throttle magnitude is below the setpoint
* Above the setpoint, the attenuation factor is (1 - rate * (throttle - breakpoint) / (1.0 - breakpoint))
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_TPA_RATE_I, 0.0f);
/**
* TPA Rate D
*
* Throttle PID Attenuation (TPA)
* Rate at which to attenuate roll/pitch D gain
* Attenuation factor is 1.0 when throttle magnitude is below the setpoint
* Above the setpoint, the attenuation factor is (1 - rate * (throttle - breakpoint) / (1.0 - breakpoint))
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_TPA_RATE_D, 0.0f);