From e29353c3c49b7c13090e102d47bcfb336f73e23a Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 20 Dec 2023 17:38:52 +0100 Subject: [PATCH] atune: remove input scale No difference in practice --- .../mc_autotune_attitude_control.cpp | 12 +++--------- .../mc_autotune_attitude_control.hpp | 7 ------- 2 files changed, 3 insertions(+), 16 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 c0fd90a5cc..8cad06f6f1 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 @@ -141,15 +141,15 @@ void McAutotuneAttitudeControl::Run() // Send data to the filters at maximum frequency if (_state == state::roll) { - _sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[0], + _sys_id.updateFilters(vehicle_torque_setpoint.xyz[0], angular_velocity.xyz[0]); } else if (_state == state::pitch) { - _sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[1], + _sys_id.updateFilters(vehicle_torque_setpoint.xyz[1], angular_velocity.xyz[1]); } else if (_state == state::yaw) { - _sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[2], + _sys_id.updateFilters(vehicle_torque_setpoint.xyz[2], angular_velocity.xyz[2]); } @@ -170,9 +170,6 @@ void McAutotuneAttitudeControl::Run() updateStateMachine(now); Vector coeff = _sys_id.getCoefficients(); - coeff(2) *= _input_scale; - coeff(3) *= _input_scale; - coeff(4) *= _input_scale; const Vector3f num(coeff(2), coeff(3), coeff(4)); const Vector3f den(1.f, coeff(0), coeff(1)); @@ -287,7 +284,6 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) _state = state::roll; _state_start_time = now; _sys_id.reset(); - _input_scale = 1.f / (_param_mc_rollrate_p.get() * _param_mc_rollrate_k.get()); _gains_backup_available = false; } @@ -310,7 +306,6 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) _state = state::pitch; _state_start_time = now; _sys_id.reset(); - _input_scale = 1.f / (_param_mc_pitchrate_p.get() * _param_mc_pitchrate_k.get()); } break; @@ -330,7 +325,6 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) _state = state::yaw; _state_start_time = now; _sys_id.reset(); - _input_scale = 1.f / (_param_mc_yawrate_p.get() * _param_mc_yawrate_k.get()); } break; 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 8c5c2bb67b..48fb5ed2d9 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 @@ -152,13 +152,6 @@ private: bool _gains_backup_available{false}; // true if a backup of the parameters has been done - /** - * Scale factor applied to the input data to have the same input/output range - * When input and output scales are a lot different, some elements of the covariance - * matrix will collapse much faster than other ones, creating an ill-conditionned matrix - */ - float _input_scale{1.f}; - hrt_abstime _last_run{0}; hrt_abstime _last_publish{0}; hrt_abstime _last_model_update{0};