diff --git a/msg/AutotuneAttitudeControlStatus.msg b/msg/AutotuneAttitudeControlStatus.msg index 021a8c345b..15e9478e6d 100644 --- a/msg/AutotuneAttitudeControlStatus.msg +++ b/msg/AutotuneAttitudeControlStatus.msg @@ -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 diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index f9328635e6..f658eb56c2 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -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)) { 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 8bb783d3d1..3bb8ba57c9 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 @@ -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 &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(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(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())); 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 4350724eed..9373039457 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 @@ -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) _param_fw_at_axes, (ParamBool) _param_fw_at_start, (ParamInt) _param_fw_at_man_aux, - (ParamFloat) _param_fw_at_sysid_amp, (ParamInt) _param_fw_at_apply, (ParamFloat) _param_imu_gyro_cutoff, 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 53c85457dd..339d003818 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 @@ -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);