diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 91ce68b481..067fa909ae 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -101,6 +101,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f #define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f +#define ATTITUDE_TC_DEFAULT 0.2f class MulticopterAttitudeControl { @@ -196,6 +197,8 @@ private: param_t rattitude_thres; param_t vtol_type; + param_t roll_tc; + param_t pitch_tc; } _params_handles; /**< handles for interesting parameters */ @@ -384,6 +387,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); _params_handles.rattitude_thres = param_find("MC_RATT_TH"); _params_handles.vtol_type = param_find("VT_TYPE"); + _params_handles.roll_tc = param_find("MC_ROLL_TC"); + _params_handles.pitch_tc = param_find("MC_PITCH_TC"); /* fetch initial parameter values */ parameters_update(); @@ -426,27 +431,32 @@ MulticopterAttitudeControl::parameters_update() { float v; + float roll_tc, pitch_tc; + + param_get(_params_handles.roll_tc, &roll_tc); + param_get(_params_handles.pitch_tc, &pitch_tc); + /* roll gains */ param_get(_params_handles.roll_p, &v); - _params.att_p(0) = v; + _params.att_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_p, &v); - _params.rate_p(0) = v; + _params.rate_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_i, &v); _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); - _params.rate_d(0) = v; + _params.rate_d(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_ff, &v); _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); - _params.att_p(1) = v; + _params.att_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_p, &v); - _params.rate_p(1) = v; + _params.rate_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_i, &v); _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); - _params.rate_d(1) = v; + _params.rate_d(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_ff, &v); _params.rate_ff(1) = v; diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 9e0532058f..011a6755ad 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -39,6 +39,28 @@ * @author Anton Babushkin */ +/** + * Roll time constant + * + * Reduce if the system is too twitchy, increase if the response is too slow and sluggish. + * + * @min 0.15 + * @max 0.25 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLL_TC, 0.2f); + +/** + * Pitch time constant + * + * Reduce if the system is too twitchy, increase if the response is too slow and sluggish. + * + * @min 0.15 + * @max 0.25 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCH_TC, 0.2f); + /** * Roll P gain *