-
Notifications
You must be signed in to change notification settings - Fork 13.3k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Feature/TPA #5861
Closed
Closed
Feature/TPA #5861
Changes from 2 commits
Commits
Show all changes
20 commits
Select commit
Hold shift + click to select a range
9a08b77
Rework TPA to have per-component setup and use more stable and intuit…
anton-matosov 40a70f9
Add ZMR250 params
anton-matosov c4285a0
Remove debug code for RATE_I_MODE
anton-matosov 195b618
Fix formatting
anton-matosov 3a1c689
Remove code that should be in MC-Failsafe branch
anton-matosov 26cf17f
Rename TPA_SLOPE into TPA_RATE and remove unused params
anton-matosov a4a795c
Merge latest 'master' into feature/TPA
anton-matosov e127e4a
Merge branch 'master' into feature/TPA
anton-matosov 125cfb1
Merge commit 'b020be13f67838047d1c2532cf66a99ceb28cddf' into feature/TPA
anton-matosov fb7eb50
Merge branch 'master' into feature/TPA
anton-matosov 7141b0f
Rename to solve conflict
anton-matosov 4df4226
Update to the most recent config
anton-matosov 50ff01d
Merge commit 'fee75c61a18d8c8d6a32406575ed5ede79d01446' into feature/TPA
anton-matosov 2caf71e
Merge commit '64657900de1b306c509aeeb704ef897a00aa4183' into feature/TPA
anton-matosov 3f57f08
Merge branch 'master' into feature/TPA
anton-matosov 3adaf6a
Merge branch 'master' into feature/TPA
anton-matosov 62a9f28
Merge branch 'master' into feature/TPA
anton-matosov c19e37d
Merge branch 'master' into feature/TPA
anton-matosov e280259
Merge branch 'master' into feature/TPA
anton-matosov 983b745
Merge branch 'master' into feature/TPA
anton-matosov File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,58 @@ | ||
#!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.0003 | ||
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.0003 | ||
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_ROLLRATE_MAX 720.0 | ||
param set MC_PITCHRATE_MAX 720.0 | ||
param set MC_YAWRATE_MAX 400.0 | ||
param set MC_ACRO_R_MAX 720.0 | ||
param set MC_ACRO_P_MAX 720.0 | ||
|
||
param set MC_TPA_BREAK_P 0.7 | ||
param set MC_TPA_BREAK_D 0.7 | ||
param set MC_TPA_SLOPE_P 0.3 | ||
param set MC_TPA_SLOPE_D 0.3 | ||
|
||
param set PWM_MIN 1075 | ||
param set PWM_RATE 1000 | ||
|
||
param set MPC_THR_MIN 0.06 | ||
param set MPC_THR_MAX 0.9 | ||
param set MPC_MANTHR_MIN 0.06 | ||
param set MPC_MANTHR_MAX 0.9 | ||
|
||
param set CBRK_IO_SAFETY 22027 | ||
fi |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -99,6 +99,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_SLOPE_LOWER_LIMIT 0.05f | ||
#define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f | ||
#define ATTITUDE_TC_DEFAULT 0.2f | ||
|
||
|
@@ -186,7 +187,14 @@ class MulticopterAttitudeControl | |
param_t pitch_rate_d; | ||
param_t pitch_rate_ff; | ||
param_t tpa_breakpoint; | ||
param_t tpa_breakpoint_p; | ||
param_t tpa_breakpoint_i; | ||
param_t tpa_breakpoint_d; | ||
param_t tpa_slope; | ||
param_t tpa_slope_p; | ||
param_t tpa_slope_i; | ||
param_t tpa_slope_d; | ||
param_t rate_i_mode; | ||
param_t yaw_p; | ||
param_t yaw_rate_p; | ||
param_t yaw_rate_i; | ||
|
@@ -220,7 +228,14 @@ class MulticopterAttitudeControl | |
float yaw_ff; /**< yaw control feed-forward */ | ||
|
||
float tpa_breakpoint; /**< Throttle PID Attenuation breakpoint */ | ||
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_slope; /**< Throttle PID Attenuation slope */ | ||
float tpa_slope_p; /**< Throttle PID Attenuation slope */ | ||
float tpa_slope_i; /**< Throttle PID Attenuation slope */ | ||
float tpa_slope_d; /**< Throttle PID Attenuation slope */ | ||
int rate_i_mode; /**< Mode for I rate usage */ | ||
|
||
float roll_rate_max; | ||
float pitch_rate_max; | ||
|
@@ -282,6 +297,11 @@ class MulticopterAttitudeControl | |
*/ | ||
void control_attitude_rates(float dt); | ||
|
||
/** | ||
* Throttle PID attenuation. | ||
*/ | ||
math::Vector<3> pid_attenuations(float tpa_breakpoint, float tpa_slope); | ||
|
||
/** | ||
* Check for vehicle status updates. | ||
*/ | ||
|
@@ -386,8 +406,13 @@ 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_slope_p = param_find("MC_TPA_SLOPE_P"); | ||
_params_handles.tpa_slope_i = param_find("MC_TPA_SLOPE_I"); | ||
_params_handles.tpa_slope_d = param_find("MC_TPA_SLOPE_D"); | ||
_params_handles.rate_i_mode = param_find("MC_RATE_I_MODE"); | ||
_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"); | ||
|
@@ -484,10 +509,13 @@ 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_slope_p, &_params.tpa_slope_p); | ||
param_get(_params_handles.tpa_slope_i, &_params.tpa_slope_i); | ||
param_get(_params_handles.tpa_slope_d, &_params.tpa_slope_d); | ||
param_get(_params_handles.rate_i_mode, &_params.rate_i_mode); | ||
|
||
/* yaw gains */ | ||
param_get(_params_handles.yaw_p, &v); | ||
|
@@ -764,6 +792,27 @@ MulticopterAttitudeControl::control_attitude(float dt) | |
} | ||
} | ||
|
||
/* | ||
* Throttle PID attenuation | ||
* Function visualization available here https://www.desmos.com/calculator/gn4mfoddje | ||
* Input: 'tpa_breakpoint', 'tpa_slope', '_thrust_sp' | ||
* Output: 'pidAttenuationPerAxis' vector | ||
*/ | ||
math::Vector<3> | ||
MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_slope) | ||
{ | ||
/* throttle pid attenuation factor */ | ||
float tpa = 1.0f - tpa_slope * (fabsf(_v_rates_sp.thrust) - tpa_breakpoint)/(1.0f - tpa_breakpoint); | ||
tpa = fmaxf(TPA_SLOPE_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' | ||
|
@@ -783,28 +832,69 @@ 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_slope_p)); | ||
math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_slope_i)); | ||
math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_slope_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 + | ||
_params.rate_ff.emult(_rates_sp); | ||
_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; | ||
_rates_prev = rates; | ||
|
||
/* update integral only if not saturated on low limit and if motor commands are not saturated */ | ||
if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) { | ||
int rateIMode = _params.rate_i_mode; | ||
|
||
if (rateIMode == 0) { | ||
/* update integral only if not saturated on low limit and if motor commands are not saturated */ | ||
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) + 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 && | ||
/* if the axis is the yaw axis, do not update the integral if the limit is hit */ | ||
!((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) { | ||
_rates_int(i) = rate_i; | ||
} | ||
} | ||
} | ||
} | ||
} else if (rateIMode == 1) { | ||
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; | ||
|
||
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 && | ||
/* if the axis is the yaw axis, do not update the integral if the limit is hit */ | ||
!((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) { | ||
_rates_int(i) = rate_i; | ||
} | ||
} | ||
} | ||
} else if (rateIMode == 2) { | ||
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; | ||
|
||
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 && | ||
/* if the axis is the yaw axis, do not update the integral if the limit is hit */ | ||
!((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) { | ||
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { | ||
_rates_int(i) = rate_i; | ||
} | ||
} | ||
} | ||
} else if (rateIMode == 3) { | ||
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; | ||
|
||
if (PX4_ISFINITE(rate_i)) { | ||
_rates_int(i) = rate_i; | ||
} | ||
} | ||
|
@@ -1005,6 +1095,61 @@ MulticopterAttitudeControl::task_main() | |
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); | ||
} | ||
} | ||
|
||
if (_v_control_mode.flag_control_termination_enabled) { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This change should be in my RC failsafe PR #5863 |
||
if (!_vehicle_status.is_vtol) { | ||
|
||
_rates_sp.zero(); | ||
_rates_int.zero(); | ||
_thrust_sp = 0.0f; | ||
_att_control.zero(); | ||
|
||
|
||
/* publish actuator controls */ | ||
_actuators.control[0] = 0.0f; | ||
_actuators.control[1] = 0.0f; | ||
_actuators.control[2] = 0.0f; | ||
_actuators.control[3] = 0.0f; | ||
_actuators.timestamp = hrt_absolute_time(); | ||
_actuators.timestamp_sample = _ctrl_state.timestamp; | ||
|
||
if (!_actuators_0_circuit_breaker_enabled) { | ||
if (_actuators_0_pub != nullptr) { | ||
|
||
orb_publish(_actuators_id, _actuators_0_pub, &_actuators); | ||
perf_end(_controller_latency_perf); | ||
|
||
} else if (_actuators_id) { | ||
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators); | ||
} | ||
} | ||
|
||
_controller_status.roll_rate_integ = _rates_int(0); | ||
_controller_status.pitch_rate_integ = _rates_int(1); | ||
_controller_status.yaw_rate_integ = _rates_int(2); | ||
_controller_status.timestamp = hrt_absolute_time(); | ||
|
||
/* publish controller status */ | ||
if (_controller_status_pub != nullptr) { | ||
orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); | ||
|
||
} else { | ||
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); | ||
} | ||
/* publish attitude rates setpoint */ | ||
_v_rates_sp.roll = _rates_sp(0); | ||
_v_rates_sp.pitch = _rates_sp(1); | ||
_v_rates_sp.yaw = _rates_sp(2); | ||
_v_rates_sp.thrust = _thrust_sp; | ||
_v_rates_sp.timestamp = hrt_absolute_time(); | ||
|
||
if (_v_rates_sp_pub != nullptr) { | ||
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); | ||
} else if (_rates_sp_id) { | ||
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); | ||
} | ||
} | ||
} | ||
} | ||
|
||
perf_end(_loop_perf); | ||
|
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@anton-matosov I'm talking about the roll_scale parameter (7654) on this line
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ok, never mind my previous post then.
Mixer config is the best way to accommodate custom geometry. ZMR 250 is asymmetric and mixer compensates this.
For ZMR250 mixing for cleanflight/betaflight please refer to https://oscarliang.com/custom-motor-output-mix-quadcopter/