fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw-autotune: detect and limit amplitude of ID maneuver

Browse Source 
Increases signal ampltiude on a 1Hz sin input until a target rate (R/P/Y = 0.8/0.5/0.5 rad/s) is reached. Identification signal is then scaled with this ampltitude instead of the user-defined parameter.
This commit is contained in:
mahima-yoga 2025-09-24 15:41:39 +02:00 committed by Mahima Yoga
parent 97a97991c1
commit 614e15d5f4
5 changed files with 248 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
msg/AutotuneAttitudeControlStatus.msg
View File

@ -19,17 +19,20 @@ float32 y_filt
uint8 STATE_IDLE = 0
uint8 STATE_INIT = 1
uint8 STATE_ROLL = 2
uint8 STATE_ROLL_PAUSE = 3
uint8 STATE_PITCH = 4
uint8 STATE_PITCH_PAUSE = 5
uint8 STATE_YAW = 6
uint8 STATE_YAW_PAUSE = 7
uint8 STATE_VERIFICATION = 8
uint8 STATE_APPLY = 9
uint8 STATE_TEST = 10
uint8 STATE_COMPLETE = 11
uint8 STATE_FAIL = 12
uint8 STATE_WAIT_FOR_DISARM = 13
uint8 STATE_ROLL_AMPLITUDE_DETECTION = 2
uint8 STATE_ROLL = 3
uint8 STATE_ROLL_PAUSE = 4
uint8 STATE_PITCH_AMPLITUDE_DETECTION = 5
uint8 STATE_PITCH = 6
uint8 STATE_PITCH_PAUSE = 7
uint8 STATE_YAW_AMPLITUDE_DETECTION = 8
uint8 STATE_YAW = 9
uint8 STATE_YAW_PAUSE = 10
uint8 STATE_VERIFICATION = 11
uint8 STATE_APPLY = 12
uint8 STATE_TEST = 13
uint8 STATE_COMPLETE = 14
uint8 STATE_FAIL = 15
uint8 STATE_WAIT_FOR_DISARM = 16
uint8 state

3
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -318,6 +318,9 @@ void FixedwingAttitudeControl::Run()
if ((pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL_AMPLITUDE_DETECTION
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH_AMPLITUDE_DETECTION
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW_AMPLITUDE_DETECTION
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST)
&& ((hrt_absolute_time() - pid_autotune.timestamp) < 1_s)) {

230
src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
View File

@ -124,11 +124,18 @@ void FwAutotuneAttitudeControl::Run()
}
}
vehicle_torque_setpoint_s vehicle_torque_setpoint;
vehicle_angular_velocity_s angular_velocity;
if (!_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint)
|| !_vehicle_angular_velocity_sub.copy(&angular_velocity)) {
if (_vehicle_angular_velocity_sub.updated()) {
vehicle_angular_velocity_s vehicle_angular_velocity;
if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
_angular_velocity = Vector3f(vehicle_angular_velocity.xyz);
}
}
vehicle_torque_setpoint_s vehicle_torque_setpoint;
if (!_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint)) {
return;
}
@ -152,17 +159,17 @@ void FwAutotuneAttitudeControl::Run()
checkFilters();
if (_state == state::roll) {
if (_state == state::roll || _state == state::roll_amp_detection) {
_sys_id.update(_input_scale * vehicle_torque_setpoint.xyz[0],
angular_velocity.xyz[0]);
_angular_velocity(0));
} else if (_state == state::pitch) {
} else if (_state == state::pitch || _state == state::pitch_amp_detection) {
_sys_id.update(_input_scale * vehicle_torque_setpoint.xyz[1],
angular_velocity.xyz[1]);
_angular_velocity(1));
} else if (_state == state::yaw) {
} else if (_state == state::yaw || _state == state::yaw_amp_detection) {
_sys_id.update(_input_scale * vehicle_torque_setpoint.xyz[2],
angular_velocity.xyz[2]);
_angular_velocity(2));
}
if (hrt_elapsed_time(&_last_publish) > _publishing_dt_hrt || _last_publish == 0) {
@ -188,9 +195,16 @@ void FwAutotuneAttitudeControl::Run()
_attitude_p = math::constrain(1.f / (math::radians(60.f) * (_kiff(0) + _kiff(2))), 1.f, 5.f);
const Vector<float, 5> &coeff_var = _sys_id.getVariances();
const Vector3f rate_sp = _sys_id.areFiltersInitialized()
? getIdentificationSignal()
: Vector3f();
Vector3f rate_sp{};
if (_sys_id.areFiltersInitialized()) {
if (_state == state::roll_amp_detection || _state == state::pitch_amp_detection || _state == state::yaw_amp_detection) {
rate_sp = getAmplitudeDetectionSignal();
} else {
rate_sp = getIdentificationSignal();
}
}
autotune_attitude_control_status_s status{};
status.timestamp = now;
@ -308,7 +322,8 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
case state::init:
if (_are_filters_initialized) {
_state = state::roll;
_state = state::roll_amp_detection;
_amplitude_detection_state = amplitudeDetectionState::init;
_state_start_time = now;
_sys_id.reset(sys_id_init);
// first step needs to be shorter to keep the drone centered
@ -322,12 +337,29 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
break;
case state::roll:
if (!(_param_fw_at_axes.get() & Axes::roll)) {
// Should not tune this axis, skip
_state = state::roll_pause;
case state::roll_amp_detection: {
if (!(_param_fw_at_axes.get() & Axes::roll)) {
// Should not tune this axis, skip
_state = state::roll_pause;
break;
}
const float abs_roll_rate = fabsf(_angular_velocity(0));
const float target = min(kTargetRollRate, math::radians(_param_fw_r_rmax.get()));
updateAmplitudeDetectionState(now, abs_roll_rate, target);
if (_amplitude_detection_state == amplitudeDetectionState::complete) {
_state = state::roll;
_state_start_time = now;
}
break;
}
case state::roll:
if ((_sys_id.getFitness() < converged_thr)
&& ((now - _state_start_time) > 5_s)) {
copyGains(0);
@ -341,7 +373,8 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
case state::roll_pause:
if ((now - _state_start_time) > 2_s) {
_state = state::pitch;
_state = state::pitch_amp_detection;
_amplitude_detection_state = amplitudeDetectionState::init;
_state_start_time = now;
_sys_id.reset(sys_id_init);
_input_scale = 1.f / _param_fw_pr_p.get();
@ -354,12 +387,30 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
break;
case state::pitch:
if (!(_param_fw_at_axes.get() & Axes::pitch)) {
// Should not tune this axis, skip
_state = state::pitch_pause;
case state::pitch_amp_detection: {
if (!(_param_fw_at_axes.get() & Axes::pitch)) {
// Should not tune this axis, skip
_state = state::pitch_pause;
break;
}
const float abs_pitch_rate = fabsf(_angular_velocity(1));
const float max_pitch_rate = min(_param_fw_p_rmax_pos.get(), _param_fw_p_rmax_neg.get());
const float target = min(kTargetPitchRate, math::radians(max_pitch_rate));
updateAmplitudeDetectionState(now, abs_pitch_rate, target);
if (_amplitude_detection_state == amplitudeDetectionState::complete) {
_state = state::pitch;
_state_start_time = now;
}
break;
}
case state::pitch:
if ((_sys_id.getFitness() < converged_thr)
&& ((now - _state_start_time) > 5_s)) {
copyGains(1);
@ -371,7 +422,8 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
case state::pitch_pause:
if ((now - _state_start_time) > 2_s) {
_state = state::yaw;
_state = state::yaw_amp_detection;
_amplitude_detection_state = amplitudeDetectionState::init;
_state_start_time = now;
_sys_id.reset(sys_id_init);
_input_scale = 1.f / _param_fw_yr_p.get();
@ -379,19 +431,33 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
// first step needs to be shorter to keep the drone centered
_steps_counter = 5;
_max_steps = 10;
// reset yaw signal filter states
_signal_filter.reset(0.f);
}
break;
case state::yaw:
if (!(_param_fw_at_axes.get() & Axes::yaw)) {
// Should not tune this axis, skip
_state = state::yaw_pause;
case state::yaw_amp_detection: {
if (!(_param_fw_at_axes.get() & Axes::yaw)) {
// Should not tune this axis, skip
_state = state::yaw_pause;
break;
}
const float abs_yaw_rate = fabsf(_angular_velocity(2));
const float target = min(kTargetYawRate, math::radians(_param_fw_y_rmax.get()));
updateAmplitudeDetectionState(now, abs_yaw_rate, target);
if (_amplitude_detection_state == amplitudeDetectionState::complete) {
_state = state::yaw;
_state_start_time = now;
}
break;
}
case state::yaw:
if ((_sys_id.getFitness() < converged_thr)
&& ((now - _state_start_time) > 5_s)) {
copyGains(2);
@ -618,10 +684,94 @@ void FwAutotuneAttitudeControl::saveGainsToParams()
}
}
const Vector3f FwAutotuneAttitudeControl::getIdentificationSignal()
void FwAutotuneAttitudeControl::updateAmplitudeDetectionState(const hrt_abstime now,
const float rate, const float target_rate)
{
const hrt_abstime time_since_last_amplitude_increase = now - _time_last_amplitude_increase;
switch (_amplitude_detection_state) {
case amplitudeDetectionState::init:
_signal_amp = 0.1f; // start with an initial amplitude of 0.1 rad/s
_rate_reached = false;
_time_last_amplitude_increase = _state_start_time;
_amplitude_detection_state = amplitudeDetectionState::first_period;
break;
case amplitudeDetectionState::first_period:
if (!_rate_reached && rate >= target_rate) {
_rate_reached = true;
}
if (time_since_last_amplitude_increase >= 1_s) {
_amplitude_detection_state = _rate_reached ?
amplitudeDetectionState::second_period :
amplitudeDetectionState::increase_amplitude;
// reset flag
_rate_reached = false;
}
break;
case amplitudeDetectionState::second_period:
if (!_rate_reached && rate >= target_rate) {
_rate_reached = true;
}
if (time_since_last_amplitude_increase >= 2_s) {
_amplitude_detection_state = _rate_reached ?
amplitudeDetectionState::set_amplitude :
amplitudeDetectionState::increase_amplitude;
// reset flag
_rate_reached = false;
}
break;
case amplitudeDetectionState::increase_amplitude:
_signal_amp += kSignalAmpStep;
_time_last_amplitude_increase = now;
_amplitude_detection_state = (_signal_amp > kSignalAmpMax) ?
amplitudeDetectionState::set_amplitude :
amplitudeDetectionState::first_period;
break;
case amplitudeDetectionState::set_amplitude:
_signal_amp = math::constrain(_signal_amp - kSignalAmpStep, 0.1f, kSignalAmpMax);
_amplitude_detection_state = amplitudeDetectionState::complete;
break;
case amplitudeDetectionState::complete:
break;
}
}
const Vector3f FwAutotuneAttitudeControl::getAmplitudeDetectionSignal()
{
const hrt_abstime now = hrt_absolute_time();
const float t = static_cast<float>(now - _state_start_time) * 1e-6f;
float signal = sinf(2.0f * M_PI_F * t);
signal *= _signal_amp;
Vector3f rate_sp = scaleInputSignal(signal);
return rate_sp;
}
const Vector3f FwAutotuneAttitudeControl::getIdentificationSignal()
{
const hrt_abstime now = hrt_absolute_time();
const float t = static_cast<float>(now - _state_start_time) * 1e-6f;
float signal = 0.0f;
@ -665,26 +815,32 @@ const Vector3f FwAutotuneAttitudeControl::getIdentificationSignal()
}
signal *= _param_fw_at_sysid_amp.get();
signal *= _signal_amp;
Vector3f rate_sp = scaleInputSignal(signal);
return rate_sp;
}
const Vector3f FwAutotuneAttitudeControl::scaleInputSignal(const float signal)
{
float signal_scaled = 0.f;
Vector3f rate_sp{};
float signal_scaled = 0.f;
if (_state == state::roll || _state == state::test) {
if (_state == state::roll_amp_detection || _state == state::roll || _state == state::test) {
// Scale the signal such that the attitude controller is
// able to cancel it completely at an attitude error of pi/8
signal_scaled = math::min(signal * M_PI_F / (8.f * _param_fw_r_tc.get()), math::radians(_param_fw_r_rmax.get()));
rate_sp(0) = signal_scaled - _signal_filter.getState();
}
if (_state == state::pitch || _state == state::test) {
if (_state == state::pitch_amp_detection || _state == state::pitch || _state == state::test) {
const float pitch_rate_max_deg = math::min(_param_fw_p_rmax_pos.get(), _param_fw_p_rmax_neg.get());
signal_scaled = math::min(signal * M_PI_F / (8.f * _param_fw_p_tc.get()), math::radians(pitch_rate_max_deg));
rate_sp(1) = signal_scaled - _signal_filter.getState();
}
if (_state == state::yaw) {
if (_state == state::yaw_amp_detection || _state == state::yaw) {
// Do not send a signal that produces more than a full deflection of the rudder
signal_scaled = math::min(signal, 1.f / (_param_fw_yr_ff.get() + _param_fw_yr_p.get()),
math::radians(_param_fw_y_rmax.get()));

36
src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp
View File

@ -106,6 +106,7 @@ private:
void checkFilters();
void updateStateMachine(hrt_abstime now);
void updateAmplitudeDetectionState(hrt_abstime now, float rate, float target_rate);
void copyGains(int index);
bool areGainsGood() const;
void saveGainsToParams();
@ -114,6 +115,9 @@ private:
bool isAuxEnableSwitchEnabled();
const matrix::Vector3f getIdentificationSignal();
const matrix::Vector3f getAmplitudeDetectionSignal();
const matrix::Vector3f scaleInputSignal(const float signal);
uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub;
uORB::SubscriptionCallbackWorkItem _parameter_update_sub{this, ORB_ID(parameter_update)};
@ -130,10 +134,13 @@ private:
enum class state {
idle = autotune_attitude_control_status_s::STATE_IDLE,
init = autotune_attitude_control_status_s::STATE_INIT,
roll_amp_detection = autotune_attitude_control_status_s::STATE_ROLL_AMPLITUDE_DETECTION,
roll = autotune_attitude_control_status_s::STATE_ROLL,
roll_pause = autotune_attitude_control_status_s::STATE_ROLL_PAUSE,
pitch_amp_detection = autotune_attitude_control_status_s::STATE_PITCH_AMPLITUDE_DETECTION,
pitch = autotune_attitude_control_status_s::STATE_PITCH,
pitch_pause = autotune_attitude_control_status_s::STATE_PITCH_PAUSE,
yaw_amp_detection = autotune_attitude_control_status_s::STATE_YAW_AMPLITUDE_DETECTION,
yaw = autotune_attitude_control_status_s::STATE_YAW,
yaw_pause = autotune_attitude_control_status_s::STATE_YAW_PAUSE,
apply = autotune_attitude_control_status_s::STATE_APPLY,
@ -144,11 +151,39 @@ private:
wait_for_disarm = autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM
} _state{state::idle};
enum class amplitudeDetectionState {
init,
first_period,
second_period,
increase_amplitude,
set_amplitude,
complete
} _amplitude_detection_state{amplitudeDetectionState::init};
hrt_abstime _state_start_time{0};
uint8_t _steps_counter{0};
uint8_t _max_steps{5};
int8_t _signal_sign{0};
// Amplitude detection variables
float _signal_amp{0.1f};
bool _rate_reached{false};
hrt_abstime _time_last_amplitude_increase{0};
static constexpr float kSignalAmpMax{5.0f};
static constexpr float kSignalAmpStep{0.1f};
// Target maximum angular rates for the system identification signal.
// ~45 deg/s for roll, ~30 deg/s for pitch and yaw. These values are:
// - High enough to provide good signal-to-noise ratio for identification.
// - Low enough to keep pitch and yaw responses within the linear range
// for most vehicles.
static constexpr float kTargetRollRate{0.8f};
static constexpr float kTargetPitchRate{0.5f};
static constexpr float kTargetYawRate{0.5f};
matrix::Vector3f _angular_velocity{};
bool _armed{false};
uint8_t _nav_state{0};
uint8_t _start_flight_mode{0};
@ -192,7 +227,6 @@ private:
(ParamInt<px4::params::FW_AT_AXES>) _param_fw_at_axes,
(ParamBool<px4::params::FW_AT_START>) _param_fw_at_start,
(ParamInt<px4::params::FW_AT_MAN_AUX>) _param_fw_at_man_aux,
(ParamFloat<px4::params::FW_AT_SYSID_AMP>) _param_fw_at_sysid_amp,
(ParamInt<px4::params::FW_AT_APPLY>) _param_fw_at_apply,
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,

19
src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c
View File

@ -50,27 +50,12 @@
* injection and reset this parameter
* Best is to perform the identification in position or
* hold mode.
* Increase the amplitude of the injected signal using
* FW_AT_SYSID_AMP for more signal/noise ratio
*
* @boolean
* @group Autotune
*/
PARAM_DEFINE_INT32(FW_AT_START, 0);
/**
* Amplitude of the injected signal
*
* This parameter scales the signal sent to the
* rate controller during system identification.
*
* @min 0.1
* @max 6.0
* @decimal 1
* @group Autotune
*/
PARAM_DEFINE_FLOAT(FW_AT_SYSID_AMP, 1.0);
/**
* Controls when to apply the new gains
*
@ -146,7 +131,7 @@ PARAM_DEFINE_FLOAT(FW_AT_SYSID_F0, 1.f);
* @unit Hz
* @group Autotune
*/
PARAM_DEFINE_FLOAT(FW_AT_SYSID_F1, 20.f);
PARAM_DEFINE_FLOAT(FW_AT_SYSID_F1, 10.f);
/**
* Maneuver time for each axis
@ -171,4 +156,4 @@ PARAM_DEFINE_FLOAT(FW_AT_SYSID_TIME, 10.f);
* @value 2 Logarithmic sine sweep
* @group Autotune
*/
PARAM_DEFINE_INT32(FW_AT_SYSID_TYPE, 0);
PARAM_DEFINE_INT32(FW_AT_SYSID_TYPE, 1);