From 936caafc0611e49d6e4b377f9648ec44e86b3bae Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 3 Jan 2024 11:10:50 +0100 Subject: [PATCH] atune: remove yaw D-term and add separate yaw max input freq We usually don't want the high frequency response in our reduced order model --- .../mc_autotune_attitude_control.cpp | 11 +++++++++-- .../mc_autotune_attitude_control.hpp | 1 + .../mc_autotune_attitude_control_params.c | 15 +++++++++++++++ 3 files changed, 25 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp index 493dd58ff9..d02c043589 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp @@ -198,6 +198,11 @@ void McAutotuneAttitudeControl::Run() _kid(2) = 0.f; } + // Do not use derivative on the yaw axis as it often only amplifies noise + if ((_state == state::yaw) || (_state == state::yaw_pause)) { + _kid(2) = 0.f; + } + // To compute the attitude gain, use the following empirical rule: // "An error of 60 degrees should produce the maximum control output" // or K_att * K_rate * rad(60) = 1 @@ -564,11 +569,13 @@ const Vector3f McAutotuneAttitudeControl::getIdentificationSignal(hrt_abstime no float signal; if (_param_mc_at_sysid_type.get() == static_cast(SignalType::kLinearSineSweep)) { - signal = signal_generator::getLinearSineSweep(_param_mc_at_sysid_f0.get(), _param_mc_at_sysid_f1.get(), + const float f_max = (_state == state::yaw) ? _param_mc_at_sysid_fyaw.get() : _param_mc_at_sysid_f1.get(); + signal = signal_generator::getLinearSineSweep(_param_mc_at_sysid_f0.get(), f_max, _param_mc_at_sysid_time.get(), t); } else if (_param_mc_at_sysid_type.get() == static_cast(SignalType::kLogSineSweep)) { - signal = signal_generator::getLogSineSweep(_param_mc_at_sysid_f0.get(), _param_mc_at_sysid_f1.get(), + const float f_max = (_state == state::yaw) ? _param_mc_at_sysid_fyaw.get() : _param_mc_at_sysid_f1.get(); + signal = signal_generator::getLogSineSweep(_param_mc_at_sysid_f0.get(), f_max, _param_mc_at_sysid_time.get(), t); } else { diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp index 694dfbc2ad..784edea1a7 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp @@ -174,6 +174,7 @@ private: (ParamFloat) _param_mc_at_sysid_amp, (ParamFloat) _param_mc_at_sysid_f0, (ParamFloat) _param_mc_at_sysid_f1, + (ParamFloat) _param_mc_at_sysid_fyaw, (ParamFloat) _param_mc_at_sysid_time, (ParamInt) _param_mc_at_sysid_type, (ParamInt) _param_mc_at_apply, diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c index e85e64a0ed..a903ab63eb 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c @@ -102,6 +102,21 @@ PARAM_DEFINE_FLOAT(MC_AT_SYSID_F0, 1.f); */ PARAM_DEFINE_FLOAT(MC_AT_SYSID_F1, 20.f); +/** + * Yaw axis maximum frequency + * + * End frequency of the identification for the yaw axis. + * This is usually set lower than the roll and pitch axes + * to only consider the yaw actuation produced by propeller drag. + * + * @min 0.1 + * @max 30.0 + * @decimal 1 + * @unit Hz + * @group Autotune + */ +PARAM_DEFINE_FLOAT(MC_AT_SYSID_FYAW, 5.f); + /** * Maneuver time for each axis *