From 55f0860c3136094bbfb1b7686ad5b6ba9013824d Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 1 Oct 2021 17:09:55 +0200 Subject: [PATCH] fw atune: add fixed-wing auto-tuning module --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 6 + ROMFS/px4fmu_common/init.d/rc.vtol_apps | 2 + .../CMakeLists.txt | 47 ++ .../fw_autotune_attitude_control.cpp | 685 ++++++++++++++++++ .../fw_autotune_attitude_control.hpp | 202 ++++++ .../fw_autotune_attitude_control_params.c | 105 +++ src/modules/mavlink/mavlink_receiver.cpp | 3 +- 7 files changed, 1048 insertions(+), 2 deletions(-) create mode 100644 src/modules/fw_autotune_attitude_control/CMakeLists.txt create mode 100644 src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp create mode 100644 src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp create mode 100644 src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 95c5a365db..dd26867136 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -16,6 +16,12 @@ ekf2 start & fw_att_control start fw_pos_control_l1 start airspeed_selector start + +# +# Start attitude control auto-tuner +# +fw_autotune_attitude_control start + # # Start Land Detector. # diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index c6c1cacfbf..97ddee2b4b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -24,9 +24,11 @@ mc_att_control start vtol flight_mode_manager start mc_pos_control start vtol mc_hover_thrust_estimator start +mc_autotune_attitude_control start fw_att_control start vtol fw_pos_control_l1 start vtol +fw_autotune_attitude_control start vtol # Start Land Detector # Multicopter for now until we have something for VTOL diff --git a/src/modules/fw_autotune_attitude_control/CMakeLists.txt b/src/modules/fw_autotune_attitude_control/CMakeLists.txt new file mode 100644 index 0000000000..3c898a21ac --- /dev/null +++ b/src/modules/fw_autotune_attitude_control/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE fw_autotune_attitude_control + MAIN fw_autotune_attitude_control + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + fw_autotune_attitude_control.cpp + fw_autotune_attitude_control.hpp + DEPENDS + hysteresis + mathlib + px4_work_queue + SystemIdentification +) 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 new file mode 100644 index 0000000000..d58835f8b2 --- /dev/null +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -0,0 +1,685 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fw_autotune_attitude_control.cpp + * + * @author Mathieu Bresciani + */ + +#include "fw_autotune_attitude_control.hpp" + +using namespace matrix; + +FwAutotuneAttitudeControl::FwAutotuneAttitudeControl(bool is_vtol) : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + _actuator_controls_sub(this, is_vtol ? ORB_ID(actuator_controls_1) : ORB_ID(actuator_controls_0)), + _actuator_controls_status_sub(is_vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)) +{ + reset(); +} + +FwAutotuneAttitudeControl::~FwAutotuneAttitudeControl() +{ + perf_free(_cycle_perf); +} + +bool FwAutotuneAttitudeControl::init() +{ + if (!_parameter_update_sub.registerCallback()) { + PX4_ERR("parameter_update callback registration failed!"); + return false; + } + + _signal_filter.setParameters(_publishing_dt_s, .2f); // runs in the slow publishing loop + + return true; +} + +void FwAutotuneAttitudeControl::reset() +{ +} + +void FwAutotuneAttitudeControl::Run() +{ + if (should_exit()) { + _parameter_update_sub.unregisterCallback(); + _actuator_controls_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + updateStateMachine(hrt_absolute_time()); + } + + // new control data needed every iteration + if (_state == state::idle + || !_actuator_controls_sub.updated()) { + return; + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + } + } + + if (_actuator_controls_status_sub.updated()) { + actuator_controls_status_s controls_status; + + if (_actuator_controls_status_sub.copy(&controls_status)) { + _control_power = Vector3f(controls_status.control_power); + } + } + + actuator_controls_s controls; + vehicle_angular_velocity_s angular_velocity; + + if (!_actuator_controls_sub.copy(&controls) + || !_vehicle_angular_velocity_sub.copy(&angular_velocity)) { + return; + } + + perf_begin(_cycle_perf); + + const hrt_abstime timestamp_sample = controls.timestamp; + + // collect sample interval average for filters + if (_last_run > 0) { + // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((timestamp_sample - _last_run) * 1e-6f), 0.000125f, 0.02f); + _interval_sum += dt; + _interval_count++; + + } else { + _interval_sum = 0.f; + _interval_count = 0.f; + } + + _last_run = timestamp_sample; + + checkFilters(); + + if (_state == state::roll) { + _sys_id.update(_input_scale * controls.control[actuator_controls_s::INDEX_ROLL], + angular_velocity.xyz[0]); + + } else if (_state == state::pitch) { + _sys_id.update(_input_scale * controls.control[actuator_controls_s::INDEX_PITCH], + angular_velocity.xyz[1]); + + } else if (_state == state::yaw) { + _sys_id.update(_input_scale * controls.control[actuator_controls_s::INDEX_YAW], + angular_velocity.xyz[2]); + } + + if (hrt_elapsed_time(&_last_publish) > _publishing_dt_hrt || _last_publish == 0) { + const hrt_abstime now = hrt_absolute_time(); + 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)); + _kiff(2) = (1.f + coeff(0) + coeff(1)) / (coeff(2) + coeff(3) + coeff(4)); // inverse of the static gain + const Vector3f num_design = num * _kiff(2); // PID algorithm design works better with systems having unit static gain + Vector3f kid = pid_design::computePidGmvc(num_design, den, _sample_interval_avg, 0.2f, 0.f, 0.4f); + _kiff(0) = kid(0); + _kiff(1) = kid(1); + _attitude_p = 8.f / (M_PI_F * (_kiff(2) + _kiff(0))); // Maximum control power at an attitude error of pi/8 + + const Vector &coeff_var = _sys_id.getVariances(); + const Vector3f rate_sp = _sys_id.areFiltersInitialized() + ? getIdentificationSignal() + : Vector3f(); + + autotune_attitude_control_status_s status{}; + status.timestamp = now; + coeff.copyTo(status.coeff); + coeff_var.copyTo(status.coeff_var); + status.fitness = _sys_id.getFitness(); + status.dt_model = _sample_interval_avg; + status.innov = _sys_id.getInnovation(); + status.u_filt = _sys_id.getFilteredInputData(); + status.y_filt = _sys_id.getFilteredOutputData(); + status.kc = _kiff(0); + status.ki = _kiff(1); + status.kd = kid(2); // FW rate controller has no derivative gain + status.kff = _kiff(2); + status.att_p = _attitude_p; + rate_sp.copyTo(status.rate_sp); + status.state = static_cast(_state); + _autotune_attitude_control_status_pub.publish(status); + + _last_publish = now; + } + + perf_end(_cycle_perf); +} + +void FwAutotuneAttitudeControl::checkFilters() +{ + if (_interval_count > 1000) { + // calculate sensor update rate + _sample_interval_avg = _interval_sum / _interval_count; + const float update_rate_hz = 1.f / _sample_interval_avg; + + // check if sample rate error is greater than 1% + bool reset_filters = false; + + if ((fabsf(update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { + reset_filters = true; + } + + if (reset_filters) { + _are_filters_initialized = true; + _filter_sample_rate = update_rate_hz; + _sys_id.setLpfCutoffFrequency(_filter_sample_rate, _param_imu_gyro_cutoff.get()); + _sys_id.setHpfCutoffFrequency(_filter_sample_rate, .5f); + _sys_id.setForgettingFactor(60.f, _sample_interval_avg); + _sys_id.setFitnessLpfTimeConstant(1.f, _sample_interval_avg); + } + + // reset sample interval accumulator + _last_run = 0; + } +} + +void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) +{ + // when identifying an axis, check if the estimate has converged + const float converged_thr = 1.f; + + const float temp[5] = {0.f, 0.f, 0.f, 0.f, 0.f}; + const Vector sys_id_init(temp); + + switch (_state) { + case state::idle: + if (_param_fw_at_start.get()) { + if (registerActuatorControlsCallback()) { + _state = state::init; + + } else { + _state = state::fail; + } + + _state_start_time = now; + } + + break; + + case state::init: + if (_are_filters_initialized) { + _state = state::roll; + _state_start_time = now; + _sys_id.reset(sys_id_init); + // 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_fw_rr_p.get(); + _signal_filter.reset(0.f); + _gains_backup_available = false; + } + + break; + + case state::roll: + if (!(_param_fw_at_axes.get() & Axes::roll)) { + // Should not tune this axis, skip + _state = state::roll_pause; + } + + if ((_sys_id.getFitness() < converged_thr) + && ((now - _state_start_time) > 5_s)) { + copyGains(0); + + // wait for the drone to stabilize + _state = state::roll_pause; + _state_start_time = now; + } + + break; + + case state::roll_pause: + if ((now - _state_start_time) > 2_s) { + _state = state::pitch; + _state_start_time = now; + _sys_id.reset(sys_id_init); + _input_scale = 1.f / _param_fw_pr_p.get(); + _signal_sign = 1; + // first step needs to be shorter to keep the drone centered + _steps_counter = 5; + _max_steps = 10; + _signal_filter.reset(0.f); + } + + break; + + case state::pitch: + if (!(_param_fw_at_axes.get() & Axes::pitch)) { + // Should not tune this axis, skip + _state = state::pitch_pause; + } + + if ((_sys_id.getFitness() < converged_thr) + && ((now - _state_start_time) > 5_s)) { + copyGains(1); + _state = state::pitch_pause; + _state_start_time = now; + } + + break; + + case state::pitch_pause: + if ((now - _state_start_time) > 2_s) { + _state = state::yaw; + _state_start_time = now; + _sys_id.reset(sys_id_init); + _input_scale = 1.f / _param_fw_yr_p.get(); + _signal_sign = 1; + // 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; + } + + if ((_sys_id.getFitness() < converged_thr) + && ((now - _state_start_time) > 5_s)) { + copyGains(2); + _state = state::yaw_pause; + _state_start_time = now; + } + + break; + + case state::yaw_pause: + _signal_filter.reset(0.f); + _state = state::verification; + + // fallthrough + case state::verification: + _state = areGainsGood() + ? state::apply + : state::fail; + + _state_start_time = now; + break; + + case state::apply: + if ((_param_fw_at_apply.get() == 1)) { + _state = state::wait_for_disarm; + + } else if (_param_fw_at_apply.get() == 2) { + backupAndSaveGainsToParams(); + _state = state::test; + + } else { + _state = state::complete; + } + + _state_start_time = now; + + break; + + case state::wait_for_disarm: + if (!_armed) { + saveGainsToParams(); + _state = state::complete; + _state_start_time = now; + } + + break; + + case state::test: + if ((now - _state_start_time) > 4_s) { + _state = state::complete; + _state_start_time = now; + + } else if ((now - _state_start_time) < 4_s + && (now - _state_start_time) > 1_s + && _control_power.longerThan(0.1f)) { + _state = state::fail; + revertParamGains(); + _state_start_time = now; + } + + break; + + case state::complete: + + // fallthrough + case state::fail: + + // Wait a bit in that state to make sure + // the other components are aware of the final result + if ((now - _state_start_time) > 2_s) { + _state = state::idle; + stopAutotune(); + } + + break; + } + + // In case of convergence timeout or pilot intervention, + // the identification sequence is aborted immediately + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + if (_state != state::wait_for_disarm + && _state != state::idle + && (((now - _state_start_time) > 20_s) + || (fabsf(manual_control_setpoint.x) > 0.2f) + || (fabsf(manual_control_setpoint.y) > 0.2f))) { + _state = state::fail; + _state_start_time = now; + } +} + +bool FwAutotuneAttitudeControl::registerActuatorControlsCallback() +{ + if (!_actuator_controls_sub.registerCallback()) { + PX4_ERR("actuator_controls callback registration failed!"); + return false; + } + + return true; +} + +void FwAutotuneAttitudeControl::copyGains(int index) +{ + if (index <= 2) { + _rate_k(index) = _kiff(0); + _rate_i(index) = _kiff(1); + _rate_ff(index) = _kiff(2); + _att_p(index) = _attitude_p; + } +} + +bool FwAutotuneAttitudeControl::areGainsGood() const +{ + bool are_positive = true; + bool are_small_enough = true; + + for (int i = 0; i < 3; i++) { + if (((i == 0) && (_param_fw_at_axes.get() & Axes::roll)) + || ((i == 1) && (_param_fw_at_axes.get() & Axes::pitch)) + || ((i == 2) && (_param_fw_at_axes.get() & Axes::yaw))) { + are_positive &= _rate_k(i) > 0.f + && _rate_i(i) > 0.f + && _rate_ff(i) > 0.f; + + are_small_enough &= _rate_k(i) < 4.0f + && _rate_i(i) < 10.f + && _rate_ff(i) < 2.f; + + if (i < 3) { + // There is no yaw attitude controller + are_positive &= _att_p(i) > 0.f; + are_small_enough &= _att_p(i) < 12.f; + } + } + } + + return are_positive && are_small_enough; +} + +void FwAutotuneAttitudeControl::backupAndSaveGainsToParams() +{ + float backup_gains[11] = {}; + backup_gains[0] = _param_fw_rr_p.get(); + backup_gains[1] = _param_fw_rr_i.get(); + backup_gains[2] = _param_fw_rr_ff.get(); + backup_gains[3] = _param_fw_r_tc.get(); + backup_gains[4] = _param_fw_pr_p.get(); + backup_gains[5] = _param_fw_pr_i.get(); + backup_gains[6] = _param_fw_pr_ff.get(); + backup_gains[7] = _param_fw_p_tc.get(); + backup_gains[8] = _param_fw_yr_p.get(); + backup_gains[9] = _param_fw_yr_i.get(); + backup_gains[10] = _param_fw_yr_ff.get(); + + saveGainsToParams(); + + _rate_k(0) = backup_gains[0]; + _rate_i(0) = backup_gains[1] / _rate_k(0); + _rate_ff(0) = backup_gains[2]; + _att_p(0) = 1.f / backup_gains[3]; + _rate_k(1) = backup_gains[4]; + _rate_i(1) = backup_gains[5] / _rate_k(1); + _rate_ff(1) = backup_gains[6]; + _att_p(1) = 1.f / backup_gains[7]; + _rate_k(2) = backup_gains[8]; + _rate_i(2) = backup_gains[9] / _rate_k(2); + _rate_ff(2) = backup_gains[10]; + + _gains_backup_available = true; +} + +void FwAutotuneAttitudeControl::revertParamGains() +{ + if (_gains_backup_available) { + saveGainsToParams(); + } +} + +void FwAutotuneAttitudeControl::saveGainsToParams() +{ + if (_param_fw_at_axes.get() & Axes::roll) { + _param_fw_rr_p.set(_rate_k(0)); + _param_fw_rr_i.set(_rate_k(0) * _rate_i(0)); + _param_fw_rr_ff.set(_rate_ff(0)); + _param_fw_r_tc.set(1.f / _att_p(0)); + _param_fw_rr_p.commit_no_notification(); + _param_fw_rr_i.commit_no_notification(); + _param_fw_rr_ff.commit_no_notification(); + + if (_param_fw_at_axes.get() == Axes::roll) { + _param_fw_r_tc.commit(); + + } else { + _param_fw_r_tc.commit_no_notification(); + } + } + + if (_param_fw_at_axes.get() & Axes::pitch) { + _param_fw_pr_p.set(_rate_k(1)); + _param_fw_pr_i.set(_rate_k(1) * _rate_i(1)); + _param_fw_pr_ff.set(_rate_ff(1)); + _param_fw_p_tc.set(1.f / _att_p(1)); + _param_fw_pr_p.commit_no_notification(); + _param_fw_pr_i.commit_no_notification(); + _param_fw_pr_ff.commit_no_notification(); + + if (!(_param_fw_at_axes.get() & Axes::yaw)) { + _param_fw_p_tc.commit(); + + } else { + _param_fw_p_tc.commit_no_notification(); + } + } + + if (_param_fw_at_axes.get() & Axes::yaw) { + _param_fw_yr_p.set(_rate_k(2)); + _param_fw_yr_i.set(_rate_k(2) * _rate_i(2)); + _param_fw_yr_ff.set(_rate_ff(2)); + _param_fw_yr_p.commit_no_notification(); + _param_fw_yr_i.commit_no_notification(); + _param_fw_yr_ff.commit(); + } +} + +void FwAutotuneAttitudeControl::stopAutotune() +{ + _param_fw_at_start.set(false); + _param_fw_at_start.commit(); + _actuator_controls_sub.unregisterCallback(); +} + +const Vector3f FwAutotuneAttitudeControl::getIdentificationSignal() +{ + if (_steps_counter > _max_steps) { + _signal_sign = (_signal_sign == 1) ? 0 : 1; + _steps_counter = 0; + + if (_max_steps > 1) { + _max_steps--; + + } else { + _max_steps = 5; + } + } + + _steps_counter++; + + const float signal = float(_signal_sign) * _param_fw_at_sysid_amp.get(); + + Vector3f rate_sp{}; + + float signal_scaled = 0.f; + + if (_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 = signal * M_PI_F / (8.f * _param_fw_r_tc.get()); + rate_sp(0) = signal_scaled - _signal_filter.getState(); + } + + if (_state == state::pitch || _state == state::test) { + signal_scaled = signal * M_PI_F / (8.f * _param_fw_p_tc.get()); + rate_sp(1) = signal_scaled - _signal_filter.getState(); + + } + + if (_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())); + rate_sp(2) = signal_scaled - _signal_filter.getState(); + } + + _signal_filter.update(signal_scaled); + + return rate_sp; +} + +int FwAutotuneAttitudeControl::task_spawn(int argc, char *argv[]) +{ + bool is_vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + is_vtol = true; + } + } + + FwAutotuneAttitudeControl *instance = new FwAutotuneAttitudeControl(is_vtol); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int FwAutotuneAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FwAutotuneAttitudeControl::print_status() +{ + perf_print_counter(_cycle_perf); + + return 0; +} + +int FwAutotuneAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("fw_autotune_attitude_control", "autotune"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int fw_autotune_attitude_control_main(int argc, char *argv[]) +{ + return FwAutotuneAttitudeControl::main(argc, argv); +} 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 new file mode 100644 index 0000000000..10fa470658 --- /dev/null +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fw_autotune_attitude_control.hpp + * + * @author Mathieu Bresciani + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class FwAutotuneAttitudeControl : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + FwAutotuneAttitudeControl(bool is_vtol); + ~FwAutotuneAttitudeControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + enum Axes : int32_t { + roll = (1 << 0), + pitch = (1 << 1), + yaw = (1 << 2) + }; + + void Run() override; + + void reset(); + + void checkFilters(); + + void updateStateMachine(hrt_abstime now); + bool registerActuatorControlsCallback(); + void stopAutotune(); + void copyGains(int index); + bool areGainsGood() const; + void saveGainsToParams(); + void backupAndSaveGainsToParams(); + void revertParamGains(); + + const matrix::Vector3f getIdentificationSignal(); + + uORB::SubscriptionCallbackWorkItem _actuator_controls_sub; + uORB::SubscriptionCallbackWorkItem _parameter_update_sub{this, ORB_ID(parameter_update)}; + + uORB::Subscription _actuator_controls_status_sub; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::PublicationData _autotune_attitude_control_status_pub{ORB_ID(autotune_attitude_control_status)}; + + SystemIdentification _sys_id; + + enum class state { + idle = autotune_attitude_control_status_s::STATE_IDLE, + init = autotune_attitude_control_status_s::STATE_INIT, + roll = autotune_attitude_control_status_s::STATE_ROLL, + roll_pause = autotune_attitude_control_status_s::STATE_ROLL_PAUSE, + pitch = autotune_attitude_control_status_s::STATE_PITCH, + pitch_pause = autotune_attitude_control_status_s::STATE_PITCH_PAUSE, + 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, + test = autotune_attitude_control_status_s::STATE_TEST, + verification = autotune_attitude_control_status_s::STATE_VERIFICATION, + complete = autotune_attitude_control_status_s::STATE_COMPLETE, + fail = autotune_attitude_control_status_s::STATE_FAIL, + wait_for_disarm = autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM + } _state{state::idle}; + + hrt_abstime _state_start_time{0}; + uint8_t _steps_counter{0}; + uint8_t _max_steps{5}; + int8_t _signal_sign{0}; + + bool _armed{false}; + + matrix::Vector3f _kiff{}; + matrix::Vector3f _rate_k{}; + matrix::Vector3f _rate_i{}; + matrix::Vector3f _rate_ff{}; + + float _attitude_p{0.f}; + matrix::Vector3f _att_p{}; + + matrix::Vector3f _control_power{}; + + 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}; + + float _interval_sum{0.f}; + float _interval_count{0.f}; + float _sample_interval_avg{0.01f}; + float _filter_sample_rate{1.f}; + bool _are_filters_initialized{false}; + + AlphaFilter _signal_filter; ///< used to create a wash-out filter + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + + DEFINE_PARAMETERS( + (ParamInt) _param_fw_at_axes, + (ParamBool) _param_fw_at_start, + (ParamFloat) _param_fw_at_sysid_amp, + (ParamInt) _param_fw_at_apply, + + (ParamFloat) _param_imu_gyro_cutoff, + + (ParamFloat) _param_fw_rr_p, + (ParamFloat) _param_fw_rr_i, + (ParamFloat) _param_fw_rr_ff, + (ParamFloat) _param_fw_r_tc, + (ParamFloat) _param_fw_pr_p, + (ParamFloat) _param_fw_pr_i, + (ParamFloat) _param_fw_pr_ff, + (ParamFloat) _param_fw_p_tc, + (ParamFloat) _param_fw_yr_p, + (ParamFloat) _param_fw_yr_i, + (ParamFloat) _param_fw_yr_ff + ) + + static constexpr float _publishing_dt_s = 100e-3f; + static constexpr hrt_abstime _publishing_dt_hrt = 100_ms; +}; 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 new file mode 100644 index 0000000000..3d7b938357 --- /dev/null +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fw_autotune_attitude_control_params.c + * + * Parameters used by the attitude auto-tuner + * + * @author Mathieu Bresciani + */ + +/** + * Start the autotuning sequence + * + * WARNING: this will inject steps to the rate controller + * and can be dangerous. Only activate if you know what you + * are doing, and in a safe environment. + * + * Any motion of the remote stick will abord the signal + * 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 + * + * After the auto-tuning sequence is completed, + * a new set of gains is available and can be applied + * immediately or after landing. + * + * @value 0 Do not apply the new gains (logging only) + * @value 1 Apply the new gains after disarm + * @value 2 Apply the new gains in air + * @group Autotune + */ +PARAM_DEFINE_INT32(FW_AT_APPLY, 2); + +/** + * Tuning axes selection + * + * Defines which axes will be tuned during the auto-tuning sequence + * + * Set bits in the following positions to enable: + * 0 : Roll + * 1 : Pitch + * 2 : Yaw + * + * @bit 0 roll + * @bit 1 pitch + * @bit 2 yaw + * @min 1 + * @max 7 + * @group Autotune + */ +PARAM_DEFINE_INT32(FW_AT_AXES, 3); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c93094a2f3..ac6703d809 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -598,8 +598,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const switch (vehicle_status.vehicle_type) { case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: - /* atune_start = param_find("FW_AT_START"); */ - atune_start = PARAM_INVALID; + atune_start = param_find("FW_AT_START"); break;