From bc4778ed9cfdf6273369de26716042109e34c526 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 7 Jul 2023 10:44:07 +0200 Subject: [PATCH] mc-autotune: change input signal to sine sweep Sine sweeps are covering a much wide range of frequencies than doublets, giving better identification results --- .../mc_autotune_attitude_control.cpp | 65 ++++++------------- .../mc_autotune_attitude_control.hpp | 20 +++--- .../mc_autotune_attitude_control_params.c | 50 ++++++++++++++ 3 files changed, 82 insertions(+), 53 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 3d2c0dab7f..ea370851a1 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 @@ -61,8 +61,6 @@ bool McAutotuneAttitudeControl::init() return false; } - _signal_filter.setParameters(_publishing_dt_s, .2f); // runs in the slow publishing loop - return true; } @@ -167,7 +165,7 @@ void McAutotuneAttitudeControl::Run() _model_update_counter = 0; } - if (hrt_elapsed_time(&_last_publish) > _publishing_dt_hrt || _last_publish == 0) { + if (hrt_elapsed_time(&_last_publish) > (_publishing_dt_s * 1_s) || _last_publish == 0) { const hrt_abstime now = hrt_absolute_time(); updateStateMachine(now); @@ -198,7 +196,7 @@ void McAutotuneAttitudeControl::Run() const Vector &coeff_var = _sys_id.getVariances(); const Vector3f rate_sp = _sys_id.areFiltersInitialized() - ? getIdentificationSignal() + ? getIdentificationSignal(now) : Vector3f(); autotune_attitude_control_status_s status{}; @@ -289,12 +287,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) _state = state::roll; _state_start_time = now; _sys_id.reset(); - // first step needs to be shorter to keep the drone centered - _steps_counter = 5; - _max_steps = 10; - _signal_sign = 1; _input_scale = 1.f / (_param_mc_rollrate_p.get() * _param_mc_rollrate_k.get()); - _signal_filter.reset(0.f); _gains_backup_available = false; } @@ -302,7 +295,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) case state::roll: if (areAllSmallerThan(_sys_id.getVariances(), converged_thr) - && ((now - _state_start_time) > 5_s)) { + && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) { copyGains(0); // wait for the drone to stabilize @@ -318,18 +311,13 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) _state_start_time = now; _sys_id.reset(); _input_scale = 1.f / (_param_mc_pitchrate_p.get() * _param_mc_pitchrate_k.get()); - _signal_filter.reset(0.f); - _signal_sign = 1; - // first step needs to be shorter to keep the drone centered - _steps_counter = 5; - _max_steps = 10; } break; case state::pitch: if (areAllSmallerThan(_sys_id.getVariances(), converged_thr) - && ((now - _state_start_time) > 5_s)) { + && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) { copyGains(1); _state = state::pitch_pause; _state_start_time = now; @@ -343,18 +331,13 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) _state_start_time = now; _sys_id.reset(); _input_scale = 1.f / (_param_mc_yawrate_p.get() * _param_mc_yawrate_k.get()); - _signal_filter.reset(0.f); - _signal_sign = 1; - // first step needs to be shorter to keep the drone centered - _steps_counter = 5; - _max_steps = 10; } break; case state::yaw: if (areAllSmallerThan(_sys_id.getVariances(), converged_thr) - && ((now - _state_start_time) > 5_s)) { + && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) { copyGains(2); _state = state::yaw_pause; _state_start_time = now; @@ -367,10 +350,6 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) _state = state::verification; _state_start_time = now; _sys_id.reset(); - _signal_filter.reset(0.f); - _signal_sign = 1; - _steps_counter = 5; - _max_steps = 10; } break; @@ -445,7 +424,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) if (_state != state::wait_for_disarm && _state != state::idle - && (((now - _state_start_time) > 20_s) + && (((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s + 2_s)) || (fabsf(manual_control_setpoint.roll) > 0.05f) || (fabsf(manual_control_setpoint.pitch) > 0.05f))) { _state = state::fail; @@ -585,27 +564,25 @@ void McAutotuneAttitudeControl::stopAutotune() _vehicle_torque_setpoint_sub.unregisterCallback(); } -const Vector3f McAutotuneAttitudeControl::getIdentificationSignal() +const Vector3f McAutotuneAttitudeControl::getIdentificationSignal(hrt_abstime now) { - if (_steps_counter > _max_steps) { - _signal_sign = (_signal_sign == 1) ? 0 : 1; - _steps_counter = 0; + const float t = static_cast(now - _state_start_time) * 1e-6f; - if (_max_steps > 1) { - _max_steps--; + float signal; - } else { - _max_steps = 5; - } + 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(), + _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(), + _param_mc_at_sysid_time.get(), t); + + } else { + signal = 0.f; } - _steps_counter++; - - const float step = float(_signal_sign) * _param_mc_at_sysid_amp.get(); - - Vector3f rate_sp{}; - - const float signal = step - _signal_filter.getState(); + Vector3f rate_sp; if (_state == state::roll) { rate_sp(0) = signal; @@ -621,8 +598,6 @@ const Vector3f McAutotuneAttitudeControl::getIdentificationSignal() rate_sp(1) = signal; } - _signal_filter.update(step); - return rate_sp; } 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 4307edfcd1..5f1987bfae 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 @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -100,7 +101,7 @@ private: void backupAndSaveGainsToParams(); void revertParamGains(); - const matrix::Vector3f getIdentificationSignal(); + const matrix::Vector3f getIdentificationSignal(hrt_abstime now); uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)}; uORB::SubscriptionCallbackWorkItem _parameter_update_sub{this, ORB_ID(parameter_update)}; @@ -131,10 +132,12 @@ private: wait_for_disarm = autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM } _state{state::idle}; + enum class SignalType : uint8_t { + kLinearSineSweep = 0, + kLogSineSweep + }; + hrt_abstime _state_start_time{0}; - uint8_t _steps_counter{0}; - uint8_t _max_steps{5}; - int8_t _signal_sign{0}; bool _armed{false}; @@ -167,8 +170,6 @@ private: float _filter_dt{0.01f}; bool _are_filters_initialized{false}; - AlphaFilter _signal_filter; ///< used to create a wash-out filter - static constexpr float _model_dt_min{2e-3f}; // 2ms = 500Hz static constexpr float _model_dt_max{10e-3f}; // 10ms = 100Hz int _model_update_scaler{1}; @@ -179,6 +180,10 @@ private: DEFINE_PARAMETERS( (ParamBool) _param_mc_at_start, (ParamFloat) _param_mc_at_sysid_amp, + (ParamFloat) _param_mc_at_sysid_f0, + (ParamFloat) _param_mc_at_sysid_f1, + (ParamFloat) _param_mc_at_sysid_time, + (ParamInt) _param_mc_at_sysid_type, (ParamInt) _param_mc_at_apply, (ParamFloat) _param_mc_at_rise_time, @@ -201,6 +206,5 @@ private: (ParamFloat) _param_mc_yaw_p ) - static constexpr float _publishing_dt_s = 100e-3f; - static constexpr hrt_abstime _publishing_dt_hrt = 100_ms; + static constexpr float _publishing_dt_s = 5e-3f; }; 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 0227743e40..e85e64a0ed 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 @@ -76,6 +76,56 @@ PARAM_DEFINE_INT32(MC_AT_START, 0); */ PARAM_DEFINE_FLOAT(MC_AT_SYSID_AMP, 0.7); +/** + * 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(MC_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(MC_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(MC_AT_SYSID_TIME, 10.f); + +/** + * Input signal type + * + * Type of signal used during system identification to excite the system. + * + * @value 0 Linear sine sweep + * @value 1 Logarithmic sine sweep + * @group Autotune + */ +PARAM_DEFINE_INT32(MC_AT_SYSID_TYPE, 1); + /** * Controls when to apply the new gains *