diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index 7f6e5b9c10..5e63924d3b 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -620,22 +620,52 @@ void FwAutotuneAttitudeControl::saveGainsToParams() const Vector3f FwAutotuneAttitudeControl::getIdentificationSignal() { - if (_steps_counter > _max_steps) { - _signal_sign = (_signal_sign == 1) ? 0 : 1; - _steps_counter = 0; - if (_max_steps > 1) { - _max_steps--; - } else { - _max_steps = 5; + const hrt_abstime now = hrt_absolute_time(); + const float t = static_cast(now - _state_start_time) * 1e-6f; + float signal = 0.0f; + + switch (_param_fw_sysid_signal_type.get()) { + case static_cast(SignalType::kStep): { + if (_steps_counter > _max_steps) { + _signal_sign = (_signal_sign == 1) ? 0 : 1; + _steps_counter = 0; + + if (_max_steps > 1) { + _max_steps--; + + } else { + _max_steps = 5; + } + } + + _steps_counter++; + signal = float(_signal_sign); } + break; + + case static_cast(SignalType::kLinearSineSweep): { + + signal = signal_generator::getLinearSineSweep(_param_fw_at_sysid_f0.get(), + _param_fw_at_sysid_f1.get(), + _param_fw_sysid_time.get(), t); + } + break; + + case static_cast(SignalType::kLogSineSweep): { + signal = signal_generator::getLogSineSweep(_param_fw_at_sysid_f0.get(), _param_fw_at_sysid_f1.get(), + _param_fw_sysid_time.get(), t); + } + break; + + default: + signal = 0.f; + break; } - _steps_counter++; - - const float signal = float(_signal_sign) * _param_fw_at_sysid_amp.get(); + signal *= _param_fw_at_sysid_amp.get(); Vector3f rate_sp{}; float signal_scaled = 0.f; diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp index 9be5f55596..4350724eed 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -64,6 +65,12 @@ using namespace time_literals; +enum class SignalType : uint8_t { + kStep = 0, + kLinearSineSweep, + kLogSineSweep +}; + class FwAutotuneAttitudeControl : public ModuleBase, public ModuleParams, public px4::WorkItem { @@ -204,7 +211,12 @@ private: (ParamFloat) _param_fw_yr_p, (ParamFloat) _param_fw_yr_i, (ParamFloat) _param_fw_yr_ff, - (ParamFloat) _param_fw_y_rmax + (ParamFloat) _param_fw_y_rmax, + + (ParamFloat) _param_fw_at_sysid_f0, + (ParamFloat) _param_fw_at_sysid_f1, + (ParamFloat) _param_fw_sysid_time, + (ParamInt) _param_fw_sysid_signal_type ) static constexpr float _publishing_dt_s = 100e-3f; diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c index 13609dbbb1..eb10036631 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c @@ -121,3 +121,54 @@ PARAM_DEFINE_INT32(FW_AT_AXES, 3); * @group Autotune */ PARAM_DEFINE_INT32(FW_AT_MAN_AUX, 0); + +/** + * Start frequency of the injected signal + * + * Can be set lower or higher than the end frequency + * + * @min 0.1 + * @max 30.0 + * @decimal 1 + * @unit Hz + * @group Autotune + */ +PARAM_DEFINE_FLOAT(FW_AT_SYSID_F0, 1.f); + +/** + * End frequency of the injected signal + * + * Can be set lower or higher than the start frequency + * + * @min 0.1 + * @max 30.0 + * @decimal 1 + * @unit Hz + * @group Autotune + */ +PARAM_DEFINE_FLOAT(FW_AT_SYSID_F1, 20.f); + +/** + * Maneuver time for each axis + * + * Duration of the input signal sent on each axis during system identification + * + * @min 5 + * @max 120 + * @decimal 0 + * @unit s + * @group Autotune + */ +PARAM_DEFINE_FLOAT(FW_AT_SYSID_TIME, 10.f); + +/** + * Input signal type + * + * Type of signal used during system identification to excite the system. + * + * @value 0 Step + * @value 1 Linear sine sweep + * @value 2 Logarithmic sine sweep + * @group Autotune + */ +PARAM_DEFINE_INT32(FW_AT_SYSID_TYPE, 0);