autotune: enable autotune in mission mode

When operating fully autonomously (i.e., without manual control or a guaranteed link to the ground station), tuning a vehicle can be difficult since the current autotune process requires either RC input or a GCS command. For these scenarios, it it useful to be able to start autotune inside a mission.

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp

@@ -46,7 +46,6 @@ McAutotuneAttitudeControl::McAutotuneAttitudeControl() :
 	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
 {
 	_autotune_attitude_control_status_pub.advertise();
-	reset();
 }
 
 McAutotuneAttitudeControl::~McAutotuneAttitudeControl()
@@ -56,7 +55,8 @@ McAutotuneAttitudeControl::~McAutotuneAttitudeControl()
 
 bool McAutotuneAttitudeControl::init()
 {
-	if (!_parameter_update_sub.registerCallback()) {
+
+	if (!_vehicle_torque_setpoint_sub.registerCallback()) {
 		PX4_ERR("callback registration failed");
 		return false;
 	}
@@ -66,11 +66,6 @@ bool McAutotuneAttitudeControl::init()
 	return true;
 }
 
-void McAutotuneAttitudeControl::reset()
-{
-	_param_mc_at_start.reset();
-}
-
 void McAutotuneAttitudeControl::Run()
 {
 	if (should_exit()) {
@@ -91,17 +86,12 @@ void McAutotuneAttitudeControl::Run()
 		updateStateMachine(hrt_absolute_time());
 	}
 
-	// new control data needed every iteration
-	if (_state == state::idle
-	    || !_vehicle_torque_setpoint_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);
+			_nav_state = vehicle_status.nav_state;
 		}
 	}
 
@@ -113,6 +103,24 @@ void McAutotuneAttitudeControl::Run()
 		}
 	}
 
+	if (_vehicle_command_sub.updated()) {
+		vehicle_command_s vehicle_command;
+
+		if (_vehicle_command_sub.copy(&vehicle_command)) {
+			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_AUTOTUNE_ENABLE) {
+				if (fabsf(vehicle_command.param1 - 1.0f) < FLT_EPSILON && fabsf(vehicle_command.param2) < FLT_EPSILON) {
+					_vehicle_cmd_start_autotune = true;
+				}
+			}
+		}
+	}
+
+	// new control data needed every iteration
+	if ((_state == state::idle && !_vehicle_cmd_start_autotune)
+	    || !_vehicle_torque_setpoint_sub.updated()) {
+		return;
+	}
+
 	vehicle_torque_setpoint_s vehicle_torque_setpoint;
 	vehicle_angular_velocity_s angular_velocity;
 
@@ -271,7 +279,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 
 	switch (_state) {
 	case state::idle:
-		if (_param_mc_at_start.get()) {
+		if (_vehicle_cmd_start_autotune) {
 			if (registerActuatorControlsCallback()) {
 				_state = state::init;
 
@@ -280,6 +288,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 			}
 
 			_state_start_time = now;
+			_start_flight_mode = _nav_state;
 		}
 
 		break;
@@ -438,17 +447,23 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		break;
 	}
 
-	// In case of convergence timeout or pilot intervention,
+	// In case of convergence timeout, pilot intervention or mode change,
 	// the identification sequence is aborted immediately
 	manual_control_setpoint_s manual_control_setpoint{};
 	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
 
+	const bool timeout = (now - _state_start_time) > 20_s;
+	const bool mode_changed = (_start_flight_mode != _nav_state);
+	const bool pilot_intervention = ((fabsf(manual_control_setpoint.roll) > 0.05f)
+					 || (fabsf(manual_control_setpoint.pitch) > 0.05f));
+
+	const bool should_abort = timeout || mode_changed || pilot_intervention;
+
 	if (_state != state::wait_for_disarm
-	    && _state != state::idle
-	    && (((now - _state_start_time) > 20_s)
-		|| (fabsf(manual_control_setpoint.roll) > 0.05f)
-		|| (fabsf(manual_control_setpoint.pitch) > 0.05f))) {
+	    && _state != state::idle && should_abort) {
+
 		_state = state::fail;
+		_start_flight_mode = _nav_state;
 		_state_start_time = now;
 	}
 }
@@ -580,8 +595,7 @@ void McAutotuneAttitudeControl::saveGainsToParams()
 
 void McAutotuneAttitudeControl::stopAutotune()
 {
-	_param_mc_at_start.set(false);
-	_param_mc_at_start.commit();
+	_vehicle_cmd_start_autotune = false;
 	_vehicle_torque_setpoint_sub.unregisterCallback();
 }
 

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp

@@ -56,6 +56,7 @@
 #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_command.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_torque_setpoint.h>
 #include <mathlib/mathlib.h>
@@ -86,8 +87,6 @@ public:
 private:
 	void Run() override;
 
-	void reset();
-
 	void checkFilters();
 
 	void updateStateMachine(hrt_abstime now);
@@ -109,6 +108,7 @@ private:
 	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::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 
 	uORB::PublicationData<autotune_attitude_control_status_s> _autotune_attitude_control_status_pub{ORB_ID(autotune_attitude_control_status)};
 
@@ -137,6 +137,9 @@ private:
 	int8_t _signal_sign{0};
 
 	bool _armed{false};
+	uint8_t _nav_state{0};
+	uint8_t _start_flight_mode{0};
+	bool _vehicle_cmd_start_autotune{false};
 
 	matrix::Vector3f _kid{};
 	matrix::Vector3f _rate_k{};
@@ -177,7 +180,6 @@ private:
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
 
 	DEFINE_PARAMETERS(
-		(ParamBool<px4::params::MC_AT_START>) _param_mc_at_start,
 		(ParamFloat<px4::params::MC_AT_SYSID_AMP>) _param_mc_at_sysid_amp,
 		(ParamInt<px4::params::MC_AT_APPLY>) _param_mc_at_apply,
 		(ParamFloat<px4::params::MC_AT_RISE_TIME>) _param_mc_at_rise_time,

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c

@@ -47,24 +47,6 @@
  */
 PARAM_DEFINE_INT32(MC_AT_EN, 0);
 
-/**
- * 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 abort 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