fw_autotune_attitude_control : Add better support for RC TX control while in FW AT (#20069)

Co-authored-by: Chris Seto <chris1seto@gmail.com>

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -65,6 +65,11 @@ bool FwAutotuneAttitudeControl::init()
 
 	_signal_filter.setParameters(_publishing_dt_s, .2f); // runs in the slow publishing loop
 
+	if (!_actuator_controls_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
 	return true;
 }
 
@@ -92,9 +97,12 @@ void FwAutotuneAttitudeControl::Run()
 		updateStateMachine(hrt_absolute_time());
 	}
 
+	_aux_switch_en = isAuxEnableSwitchEnabled();
+
 	// new control data needed every iteration
-	if (_state == state::idle
+	if ((_state == state::idle && !_aux_switch_en)
 	    || !_actuator_controls_sub.updated()) {
+
 		return;
 	}
 
@@ -103,6 +111,7 @@ void FwAutotuneAttitudeControl::Run()
 
 		if (_vehicle_status_sub.copy(&vehicle_status)) {
 			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+			_nav_state = vehicle_status.nav_state;
 		}
 	}
 
@@ -230,6 +239,48 @@ void FwAutotuneAttitudeControl::checkFilters()
 	}
 }
 
+bool FwAutotuneAttitudeControl::isAuxEnableSwitchEnabled()
+{
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+	float aux_enable_channel = 0;
+
+	switch (_param_fw_at_man_aux.get()) {
+	case 0:
+		return false;
+
+	case 1:
+		aux_enable_channel = manual_control_setpoint.aux1;
+		break;
+
+	case 2:
+		aux_enable_channel = manual_control_setpoint.aux2;
+		break;
+
+	case 3:
+		aux_enable_channel = manual_control_setpoint.aux3;
+		break;
+
+	case 4:
+		aux_enable_channel = manual_control_setpoint.aux4;
+		break;
+
+	case 5:
+		aux_enable_channel = manual_control_setpoint.aux5;
+		break;
+
+	case 6:
+		aux_enable_channel = manual_control_setpoint.aux6;
+		break;
+
+	default:
+		return false;
+	}
+
+	return aux_enable_channel > .5f;
+}
+
 void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 {
 	// when identifying an axis, check if the estimate has converged
@@ -240,15 +291,12 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 
 	switch (_state) {
 	case state::idle:
-		if (_param_fw_at_start.get()) {
-			if (registerActuatorControlsCallback()) {
-				_state = state::init;
-
-			} else {
-				_state = state::fail;
-			}
+		if (_param_fw_at_start.get() || _aux_switch_en) {
 
+			mavlink_log_info(&_mavlink_log_pub, "Autotune started");
+			_state = state::init;
 			_state_start_time = now;
+			_start_flight_mode = _nav_state;
 		}
 
 		break;
@@ -361,20 +409,23 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		_state_start_time = now;
 		break;
 
-	case state::apply:
-		if ((_param_fw_at_apply.get() == 1)) {
-			_state = state::wait_for_disarm;
+	case state::apply: {
+			mavlink_log_info(&_mavlink_log_pub, "Autotune finished successfully");
 
-		} else if (_param_fw_at_apply.get() == 2) {
-			backupAndSaveGainsToParams();
-			_state = state::test;
+			if ((_param_fw_at_apply.get() == 1)) {
+				_state = state::wait_for_disarm;
 
-		} else {
-			_state = state::complete;
+			} else if (_param_fw_at_apply.get() == 2) {
+				backupAndSaveGainsToParams();
+				_state = state::test;
+
+			} else {
+				_state = state::complete;
+			}
+
+			_state_start_time = now;
 		}
 
-		_state_start_time = now;
-
 		break;
 
 	case state::wait_for_disarm:
@@ -409,38 +460,36 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		// 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) {
+
+			// Don't reset until aux switch is back to disabled
+			if (_param_fw_at_man_aux.get() && _aux_switch_en) {
+				break;
+			}
+
+			orb_advert_t mavlink_log_pub = nullptr;
+			mavlink_log_info(&mavlink_log_pub, "Autotune returned to idle");
 			_state = state::idle;
-			stopAutotune();
+			_param_fw_at_start.set(false);
+			_param_fw_at_start.commit();
 		}
 
 		break;
 	}
 
-	// In case of convergence timeout or pilot intervention,
+	// In case of convergence timeout
 	// 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;
+	if (_state != state::wait_for_disarm && _state != state::idle && _state != state::fail && _state != state::complete) {
+		if (now - _state_start_time > 20_s
+		    || (_param_fw_at_man_aux.get() && !_aux_switch_en)
+		    || _start_flight_mode != _nav_state) {
+			orb_advert_t mavlink_log_pub = nullptr;
+			mavlink_log_critical(&mavlink_log_pub, "Autotune aborted before finishing");
+			_state = state::fail;
+			_state_start_time = now;
+		}
 	}
 }
 
-bool FwAutotuneAttitudeControl::registerActuatorControlsCallback()
-{
-	if (!_actuator_controls_sub.registerCallback()) {
-		PX4_ERR("callback registration failed");
-		return false;
-	}
-
-	return true;
-}
-
 void FwAutotuneAttitudeControl::copyGains(int index)
 {
 	if (index <= 2) {
@@ -564,13 +613,6 @@ void FwAutotuneAttitudeControl::saveGainsToParams()
 	}
 }
 
-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) {

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -60,6 +60,7 @@
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_status.h>
 #include <mathlib/mathlib.h>
+#include <lib/systemlib/mavlink_log.h>
 
 using namespace time_literals;
 
@@ -98,13 +99,12 @@ private:
 	void checkFilters();
 
 	void updateStateMachine(hrt_abstime now);
-	bool registerActuatorControlsCallback();
-	void stopAutotune();
 	void copyGains(int index);
 	bool areGainsGood() const;
 	void saveGainsToParams();
 	void backupAndSaveGainsToParams();
 	void revertParamGains();
+	bool isAuxEnableSwitchEnabled();
 
 	const matrix::Vector3f getIdentificationSignal();
 
@@ -143,6 +143,11 @@ private:
 	int8_t _signal_sign{0};
 
 	bool _armed{false};
+	uint8_t _nav_state{0};
+	uint8_t _start_flight_mode{0};
+	bool _aux_switch_en{false};
+
+	orb_advert_t _mavlink_log_pub{nullptr};
 
 	matrix::Vector3f _kiff{};
 	matrix::Vector3f _rate_k{};
@@ -179,6 +184,7 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::FW_AT_AXES>) _param_fw_at_axes,
 		(ParamBool<px4::params::FW_AT_START>) _param_fw_at_start,
+		(ParamInt<px4::params::FW_AT_MAN_AUX>) _param_fw_at_man_aux,
 		(ParamFloat<px4::params::FW_AT_SYSID_AMP>) _param_fw_at_sysid_amp,
 		(ParamInt<px4::params::FW_AT_APPLY>) _param_fw_at_apply,
 

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c

@@ -103,3 +103,21 @@ PARAM_DEFINE_INT32(FW_AT_APPLY, 2);
  * @group Autotune
  */
 PARAM_DEFINE_INT32(FW_AT_AXES, 3);
+
+/**
+ * Enable auto tuning enable on aux input
+ *
+ * Defines which aux input to enable auto tuning
+ *
+ * @value 0 Disable
+ * @value 1 Aux1
+ * @value 2 Aux2
+ * @value 3 Aux3
+ * @value 4 Aux4
+ * @value 5 Aux5
+ * @value 6 Aux6
+ * @min 0
+ * @max 6
+ * @group Autotune
+ */
+PARAM_DEFINE_INT32(FW_AT_MAN_AUX, 0);