diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 95efeb23ad..02d99689d1 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -45,6 +45,7 @@ set(msg_files airspeed.msg airspeed_validated.msg airspeed_wind.msg + autotune_attitude_control_status.msg battery_status.msg camera_capture.msg camera_status.msg diff --git a/msg/autotune_attitude_control_status.msg b/msg/autotune_attitude_control_status.msg new file mode 100644 index 0000000000..021a8c345b --- /dev/null +++ b/msg/autotune_attitude_control_status.msg @@ -0,0 +1,35 @@ +uint64 timestamp # time since system start (microseconds) + +float32[5] coeff # coefficients of the identified discrete-time model +float32[5] coeff_var # coefficients' variance of the identified discrete-time model +float32 fitness # fitness of the parameter estimate +float32 innov +float32 dt_model + +float32 kc +float32 ki +float32 kd +float32 kff +float32 att_p + +float32[3] rate_sp + +float32 u_filt +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 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index bd8f88f118..5a7af2de20 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -55,6 +55,7 @@ void LoggedTopics::add_default_topics() add_topic("actuator_controls_status_0", 300); add_topic("airspeed", 1000); add_topic("airspeed_validated", 200); + add_topic("autotune_attitude_control_status", 100); add_topic("camera_capture"); add_topic("camera_trigger"); add_topic("cellular_status", 200); diff --git a/src/modules/mc_autotune_attitude_control/CMakeLists.txt b/src/modules/mc_autotune_attitude_control/CMakeLists.txt new file mode 100644 index 0000000000..27f14b31b7 --- /dev/null +++ b/src/modules/mc_autotune_attitude_control/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# 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 mc_autotune_attitude_control + MAIN mc_autotune_attitude_control + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + SRCS + mc_autotune_attitude_control.cpp + mc_autotune_attitude_control.hpp + DEPENDS + hysteresis + mathlib + px4_work_queue + SystemIdentification + PidDesign +) diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp new file mode 100644 index 0000000000..bf71b0b1e6 --- /dev/null +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp @@ -0,0 +1,684 @@ +/**************************************************************************** + * + * 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 mc_autotune_attitude_control.cpp + * + * @author Mathieu Bresciani + */ + +#include "mc_autotune_attitude_control.hpp" + +using namespace matrix; + +McAutotuneAttitudeControl::McAutotuneAttitudeControl() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ + reset(); +} + +McAutotuneAttitudeControl::~McAutotuneAttitudeControl() +{ + perf_free(_cycle_perf); +} + +bool McAutotuneAttitudeControl::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 McAutotuneAttitudeControl::reset() +{ +} + +void McAutotuneAttitudeControl::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(); + + // Send data to the filters at maximum frequency + if (_state == state::roll) { + _sys_id.updateFilters(_input_scale * controls.control[actuator_controls_s::INDEX_ROLL], + angular_velocity.xyz[0]); + + } else if (_state == state::pitch) { + _sys_id.updateFilters(_input_scale * controls.control[actuator_controls_s::INDEX_PITCH], + angular_velocity.xyz[1]); + + } else if (_state == state::yaw) { + _sys_id.updateFilters(_input_scale * controls.control[actuator_controls_s::INDEX_YAW], + angular_velocity.xyz[2]); + } + + // Update the model at a lower frequency + _model_update_counter++; + + if (_model_update_counter >= _model_update_scaler) { + if ((_state == state::roll) || (_state == state::pitch) || (_state == state::yaw)) { + _sys_id.update(); + _last_model_update = hrt_absolute_time(); + } + + _model_update_counter = 0; + } + + 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)); + + const float model_dt = static_cast(_model_update_scaler) * _filter_dt; + + const float desired_rise_time = ((_state == state::yaw) + || (_state == state::yaw_pause)) ? 0.2f : _param_mc_at_rise_time.get(); + _kid = pid_design::computePidGmvc(num, den, model_dt, desired_rise_time, 0.f, 0.7f); + + // Prevent the D term from going just negative if it is not needed + if ((_kid(2) < 0.f) && (_kid(2) > -0.001f)) { + _kid(2) = 0.f; + } + + // To compute the attitude gain, use the following empirical rule: + // "An error of 60 degrees should produce the maximum control output" + // or K_att * K_rate * rad(60) = 1 + _attitude_p = math::constrain(1.f / (math::radians(60.f) * _kid(0)), 2.f, 6.5f); + + 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 = model_dt; + status.innov = _sys_id.getInnovation(); + status.u_filt = _sys_id.getFilteredInputData(); + status.y_filt = _sys_id.getFilteredOutputData(); + status.kc = _kid(0); + status.ki = _kid(1); + status.kd = _kid(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 McAutotuneAttitudeControl::checkFilters() +{ + if (_interval_count > 1000) { + // calculate sensor update rate + _sample_interval_avg = _interval_sum / _interval_count; + + // check if sample rate error is greater than 1% + bool reset_filters = false; + + if ((fabsf(_filter_dt - _sample_interval_avg) / _filter_dt) > 0.01f) { + reset_filters = true; + } + + if (reset_filters && !_are_filters_initialized) { + _filter_dt = _sample_interval_avg; + + const float filter_rate_hz = 1.f / _filter_dt; + + _sys_id.setLpfCutoffFrequency(filter_rate_hz, _param_imu_gyro_cutoff.get()); + _sys_id.setHpfCutoffFrequency(filter_rate_hz, .5f); + + // Set the model sampling time depending on the gyro cutoff frequency + // as this is a good indicator of the maximum control loop bandwidth + float model_dt = math::constrain(math::max(1.f / (2.f * _param_imu_gyro_cutoff.get()), _filter_dt), _model_dt_min, + _model_dt_max); + + _model_update_scaler = math::max(int(model_dt / _filter_dt), 1); + model_dt = _model_update_scaler * _filter_dt; + + _sys_id.setForgettingFactor(60.f, model_dt); + _sys_id.setFitnessLpfTimeConstant(1.f, model_dt); + + _are_filters_initialized = true; + } + + // reset sample interval accumulator + _last_run = 0; + } +} + +void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) +{ + // when identifying an axis, check if the estimate has converged + const float converged_thr = 50.f; + + switch (_state) { + case state::idle: + if (_param_mc_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(); + // 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; + } + + break; + + case state::roll: + if (areAllSmallerThan(_sys_id.getVariances(), 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(); + _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)) { + 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(); + _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)) { + copyGains(2); + _state = state::yaw_pause; + _state_start_time = now; + } + + break; + + case state::yaw_pause: + if ((now - _state_start_time) > 2_s) { + _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; + + case state::verification: + _state = areGainsGood() + ? state::apply + : state::fail; + + _state_start_time = now; + break; + + case state::apply: + if ((_param_mc_at_apply.get() == 1)) { + _state = state::wait_for_disarm; + + } else if (_param_mc_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.05f) + || (fabsf(manual_control_setpoint.y) > 0.05f))) { + _state = state::fail; + _state_start_time = now; + } +} + +void McAutotuneAttitudeControl::backupAndSaveGainsToParams() +{ + float backup_gains[15] = {}; + backup_gains[0] = _param_mc_rollrate_k.get(); + backup_gains[1] = _param_mc_rollrate_p.get(); + backup_gains[2] = _param_mc_rollrate_i.get(); + backup_gains[3] = _param_mc_rollrate_d.get(); + backup_gains[4] = _param_mc_roll_p.get(); + backup_gains[5] = _param_mc_pitchrate_k.get(); + backup_gains[6] = _param_mc_pitchrate_p.get(); + backup_gains[7] = _param_mc_pitchrate_i.get(); + backup_gains[8] = _param_mc_pitchrate_d.get(); + backup_gains[9] = _param_mc_pitch_p.get(); + backup_gains[10] = _param_mc_yawrate_k.get(); + backup_gains[11] = _param_mc_yawrate_p.get(); + backup_gains[12] = _param_mc_yawrate_i.get(); + backup_gains[13] = _param_mc_yawrate_d.get(); + backup_gains[14] = _param_mc_yaw_p.get(); + + saveGainsToParams(); + + _rate_k(0) = backup_gains[0] * backup_gains[1]; // convert and save as standard form + _rate_i(0) = backup_gains[2] / backup_gains[1]; + _rate_d(0) = backup_gains[3] / backup_gains[1]; + _att_p(0) = backup_gains[4]; + _rate_k(1) = backup_gains[5] * backup_gains[6]; + _rate_i(1) = backup_gains[7] / backup_gains[6]; + _rate_d(1) = backup_gains[8] / backup_gains[6]; + _att_p(1) = backup_gains[9]; + _rate_k(2) = backup_gains[10] * backup_gains[11]; + _rate_i(2) = backup_gains[12] / backup_gains[11]; + _rate_d(2) = backup_gains[13] / backup_gains[11]; + _att_p(2) = backup_gains[14]; + + _gains_backup_available = true; +} + +void McAutotuneAttitudeControl::revertParamGains() +{ + if (_gains_backup_available) { + saveGainsToParams(); + } +} + +bool McAutotuneAttitudeControl::registerActuatorControlsCallback() +{ + if (!_actuator_controls_sub.registerCallback()) { + PX4_ERR("actuator_controls callback registration failed!"); + return false; + } + + return true; +} + +bool McAutotuneAttitudeControl::areAllSmallerThan(const Vector &vect, float threshold) const +{ + return (vect(0) < threshold) + && (vect(1) < threshold) + && (vect(2) < threshold) + && (vect(3) < threshold) + && (vect(4) < threshold); +} + +void McAutotuneAttitudeControl::copyGains(int index) +{ + if (index <= 2) { + _rate_k(index) = _kid(0); + _rate_i(index) = _kid(1); + _rate_d(index) = _kid(2); + _att_p(index) = _attitude_p; + } +} + +bool McAutotuneAttitudeControl::areGainsGood() const +{ + const bool are_positive = _rate_k.min() > 0.f + && _rate_i.min() > 0.f + && _rate_d.min() >= 0.f + && _att_p.min() > 0.f; + + const bool are_small_enough = _rate_k.max() < 0.5f + && _rate_i.max() < 10.f + && _rate_d.max() < 0.1f + && _att_p.max() < 12.f; + + return are_positive && are_small_enough; +} + +void McAutotuneAttitudeControl::saveGainsToParams() +{ + // save as parallel form + _param_mc_rollrate_p.set(_rate_k(0)); + _param_mc_rollrate_k.set(1.f); + _param_mc_rollrate_i.set(_rate_k(0) * _rate_i(0)); + _param_mc_rollrate_d.set(_rate_k(0) * _rate_d(0)); + _param_mc_roll_p.set(_att_p(0)); + _param_mc_rollrate_p.commit_no_notification(); + _param_mc_rollrate_k.commit_no_notification(); + _param_mc_rollrate_i.commit_no_notification(); + _param_mc_rollrate_d.commit_no_notification(); + _param_mc_roll_p.commit_no_notification(); + + _param_mc_pitchrate_p.set(_rate_k(1)); + _param_mc_pitchrate_k.set(1.f); + _param_mc_pitchrate_i.set(_rate_k(1) * _rate_i(1)); + _param_mc_pitchrate_d.set(_rate_k(1) * _rate_d(1)); + _param_mc_pitch_p.set(_att_p(1)); + _param_mc_pitchrate_p.commit_no_notification(); + _param_mc_pitchrate_k.commit_no_notification(); + _param_mc_pitchrate_i.commit_no_notification(); + _param_mc_pitchrate_d.commit_no_notification(); + _param_mc_pitch_p.commit_no_notification(); + + _param_mc_yawrate_p.set(_rate_k(2)); + _param_mc_yawrate_k.set(1.f); + _param_mc_yawrate_i.set(_rate_k(2) * _rate_i(2)); + _param_mc_yawrate_d.set(_rate_k(2) * _rate_d(2)); + _param_mc_yaw_p.set(_att_p(2)); + _param_mc_yawrate_p.commit_no_notification(); + _param_mc_yawrate_k.commit_no_notification(); + _param_mc_yawrate_i.commit_no_notification(); + _param_mc_yawrate_d.commit_no_notification(); + _param_mc_yaw_p.commit(); +} + +void McAutotuneAttitudeControl::stopAutotune() +{ + _param_mc_at_start.set(false); + _param_mc_at_start.commit(); + _actuator_controls_sub.unregisterCallback(); +} + +const Vector3f McAutotuneAttitudeControl::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 step = float(_signal_sign) * _param_mc_at_sysid_amp.get(); + + Vector3f rate_sp{}; + + const float signal = step - _signal_filter.getState(); + + if (_state == state::roll) { + rate_sp(0) = signal; + + } else if (_state == state::pitch) { + rate_sp(1) = signal; + + } else if (_state == state::yaw) { + rate_sp(2) = signal; + + } else if (_state == state::test) { + rate_sp(0) = signal; + rate_sp(1) = signal; + } + + _signal_filter.update(step); + + return rate_sp; +} + +int McAutotuneAttitudeControl::task_spawn(int argc, char *argv[]) +{ + McAutotuneAttitudeControl *instance = new McAutotuneAttitudeControl(); + + 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 McAutotuneAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int McAutotuneAttitudeControl::print_status() +{ + perf_print_counter(_cycle_perf); + + return 0; +} + +int McAutotuneAttitudeControl::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("mc_autotune_attitude_control", "autotune"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int mc_autotune_attitude_control_main(int argc, char *argv[]) +{ + return McAutotuneAttitudeControl::main(argc, argv); +} diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp new file mode 100644 index 0000000000..3c5218bd19 --- /dev/null +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp @@ -0,0 +1,206 @@ +/**************************************************************************** + * + * 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 mc_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 + +using namespace time_literals; + +class McAutotuneAttitudeControl : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + McAutotuneAttitudeControl(); + ~McAutotuneAttitudeControl() 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: + void Run() override; + + void reset(); + + void checkFilters(); + + void updateStateMachine(hrt_abstime now); + bool registerActuatorControlsCallback(); + void stopAutotune(); + bool areAllSmallerThan(const matrix::Vector &vect, float threshold) const; + void copyGains(int index); + bool areGainsGood() const; + void saveGainsToParams(); + void backupAndSaveGainsToParams(); + void revertParamGains(); + + const matrix::Vector3f getIdentificationSignal(); + + uORB::SubscriptionCallbackWorkItem _actuator_controls_sub{this, ORB_ID(actuator_controls_0)}; + uORB::SubscriptionCallbackWorkItem _parameter_update_sub{this, ORB_ID(parameter_update)}; + + uORB::Subscription _actuator_controls_status_sub{ORB_ID(actuator_controls_status_0)}; + 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 _kid{}; + matrix::Vector3f _rate_k{}; + matrix::Vector3f _rate_i{}; + matrix::Vector3f _rate_d{}; + + 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}; + hrt_abstime _last_model_update{0}; + + float _interval_sum{0.f}; + float _interval_count{0.f}; + float _sample_interval_avg{0.01f}; + float _filter_dt{0.01f}; + bool _are_filters_initialized{false}; + + AlphaFilter _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}; + int _model_update_counter{0}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + + DEFINE_PARAMETERS( + (ParamBool) _param_mc_at_start, + (ParamFloat) _param_mc_at_sysid_amp, + (ParamInt) _param_mc_at_apply, + (ParamFloat) _param_mc_at_rise_time, + + (ParamFloat) _param_imu_gyro_cutoff, + + (ParamFloat) _param_mc_rollrate_p, + (ParamFloat) _param_mc_rollrate_k, + (ParamFloat) _param_mc_rollrate_i, + (ParamFloat) _param_mc_rollrate_d, + (ParamFloat) _param_mc_roll_p, + (ParamFloat) _param_mc_pitchrate_p, + (ParamFloat) _param_mc_pitchrate_k, + (ParamFloat) _param_mc_pitchrate_i, + (ParamFloat) _param_mc_pitchrate_d, + (ParamFloat) _param_mc_pitch_p, + (ParamFloat) _param_mc_yawrate_p, + (ParamFloat) _param_mc_yawrate_k, + (ParamFloat) _param_mc_yawrate_i, + (ParamFloat) _param_mc_yawrate_d, + (ParamFloat) _param_mc_yaw_p + ) + + static constexpr float _publishing_dt_s = 100e-3f; + static constexpr hrt_abstime _publishing_dt_hrt = 100_ms; +}; diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c new file mode 100644 index 0000000000..7ca4c727a0 --- /dev/null +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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 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 + * MC_AT_SYSID_AMP for more signal/noise ratio + * + * @boolean + * @group Autotune + */ +PARAM_DEFINE_INT32(MC_AT_START, 0); + +/** + * Amplitude of the injected signal + * + * @min 0.1 + * @max 6.0 + * @decimal 1 + * @group Autotune + */ +PARAM_DEFINE_FLOAT(MC_AT_SYSID_AMP, 0.7); + +/** + * 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. + * + * WARNING Applying the gains in air is dangerous as there is no + * guarantee that those new gains will be able to stabilize + * the drone properly. + * + * @value 0 Do not apply the new gains (logging only) + * @value 1 Apply the new gains after disarm + * @value 2 WARNING Apply the new gains in air + * @group Autotune + */ +PARAM_DEFINE_INT32(MC_AT_APPLY, 1); + +/** + * Desired angular rate closed-loop rise time + * + * @min 0.01 + * @max 0.5 + * @decimal 2 + * @unit s + * @group Autotune + */ +PARAM_DEFINE_FLOAT(MC_AT_RISE_TIME, 0.14);