dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

atune: remove input scale 

No difference in practice
This commit is contained in:
bresch 2023-12-20 17:38:52 +01:00
parent 825c11fc8a
commit e29353c3c4
2 changed files with 3 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
View File

@ -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<float, 5> 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;

7
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp
View File

@ -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};