fw atune: add fixed-wing auto-tuning module

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.
 #

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

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
+)

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 <mathieu@auterion.com>
+ */
+
+#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<float, 5> 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<float, 5> &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<int>(_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<float, 5> 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);
+}

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 <mathieu@auterion.com>
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <lib/perf/perf_counter.h>
+#include <lib/pid_design/pid_design.hpp>
+#include <lib/system_identification/system_identification.hpp>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/autotune_attitude_control_status.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_status.h>
+#include <mathlib/mathlib.h>
+
+using namespace time_literals;
+
+class FwAutotuneAttitudeControl : public ModuleBase<FwAutotuneAttitudeControl>, 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_s> _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<float> _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<px4::params::FW_AT_AXES>) _param_fw_at_axes,
+		(ParamBool<px4::params::FW_AT_START>) _param_fw_at_start,
+		(ParamFloat<px4::params::FW_AT_SYSID_AMP>) _param_fw_at_sysid_amp,
+		(ParamInt<px4::params::FW_AT_APPLY>) _param_fw_at_apply,
+
+		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
+
+		(ParamFloat<px4::params::FW_RR_P>) _param_fw_rr_p,
+		(ParamFloat<px4::params::FW_RR_I>) _param_fw_rr_i,
+		(ParamFloat<px4::params::FW_RR_FF>) _param_fw_rr_ff,
+		(ParamFloat<px4::params::FW_R_TC>) _param_fw_r_tc,
+		(ParamFloat<px4::params::FW_PR_P>) _param_fw_pr_p,
+		(ParamFloat<px4::params::FW_PR_I>) _param_fw_pr_i,
+		(ParamFloat<px4::params::FW_PR_FF>) _param_fw_pr_ff,
+		(ParamFloat<px4::params::FW_P_TC>) _param_fw_p_tc,
+		(ParamFloat<px4::params::FW_YR_P>) _param_fw_yr_p,
+		(ParamFloat<px4::params::FW_YR_I>) _param_fw_yr_i,
+		(ParamFloat<px4::params::FW_YR_FF>) _param_fw_yr_ff
+	)
+
+	static constexpr float _publishing_dt_s = 100e-3f;
+	static constexpr hrt_abstime _publishing_dt_hrt = 100_ms;
+};

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 <mathieu@auterion.com>
+ */
+
+/**
+ * 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);

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;