fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 11:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc-autotune: change input signal to sine sweep

Browse Source 
Sine sweeps are covering a much wide range of frequencies than doublets,
giving better identification results
This commit is contained in:
bresch
2024-01-24 10:03:51 +01:00
parent 7f23eba46f
commit bc4778ed9c
3 changed files with 82 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
+20 -45
View File
@@ -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<float, 5> &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<float>(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<int32_t>(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<int32_t>(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;
}
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp
+12 -8
View File
@@ -43,6 +43,7 @@
#include <lib/perf/perf_counter.h>
#include <lib/pid_design/pid_design.hpp>
#include <lib/system_identification/system_identification.hpp>
#include <lib/system_identification/signal_generator.hpp>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
@@ -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<float> _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<px4::params::MC_AT_START>) _param_mc_at_start,
(ParamFloat<px4::params::MC_AT_SYSID_AMP>) _param_mc_at_sysid_amp,
(ParamFloat<px4::params::MC_AT_SYSID_F0>) _param_mc_at_sysid_f0,
(ParamFloat<px4::params::MC_AT_SYSID_F1>) _param_mc_at_sysid_f1,
(ParamFloat<px4::params::MC_AT_SYSID_TIME>) _param_mc_at_sysid_time,
(ParamInt<px4::params::MC_AT_SYSID_TYPE>) _param_mc_at_sysid_type,
(ParamInt<px4::params::MC_AT_APPLY>) _param_mc_at_apply,
(ParamFloat<px4::params::MC_AT_RISE_TIME>) _param_mc_at_rise_time,
@@ -201,6 +206,5 @@ private:
(ParamFloat<px4::params::MC_YAW_P>) _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;
};
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c
+50
View File
@@ -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
*