mc stabilized mode: move from mc_pos_control to mc_att_control

- better in terms of dependencies:
  - the position controller code depended on position topics for yaw
  - mc_pos_control does not have to be run for Stabilized mode
- the code path is much simpler, and thus less error prone. This is
  especially important since Stabilized is often used as a fail-safe flight
  mode.

src/modules/mc_att_control/mc_att_control.hpp

@@ -114,6 +114,20 @@ private:
 	void		publish_rates_setpoint();
 	void		publish_rate_controller_status();
 
+	float		throttle_curve(float throttle_stick_input);
+
+	/**
+	 * Generate & publish an attitude setpoint from stick inputs
+	 */
+	void		generate_attitude_setpoint(float dt, bool reset_yaw_sp);
+
+	/**
+	 * Get the landing gear state based on the manual control switch position
+	 * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN
+	 */
+	float		get_landing_gear_state();
+
+
 	/**
 	 * Attitude controller.
 	 */
@@ -150,6 +164,7 @@ private:
 	orb_advert_t	_v_rates_sp_pub{nullptr};		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub{nullptr};		/**< attitude actuator controls publication */
 	orb_advert_t	_controller_status_pub{nullptr};	/**< controller status publication */
+	orb_advert_t	_vehicle_attitude_setpoint_pub{nullptr};
 
 	orb_id_t _rates_sp_id{nullptr};		/**< pointer to correct rates setpoint uORB metadata structure */
 	orb_id_t _actuators_id{nullptr};	/**< pointer to correct actuator controls0 uORB metadata structure */
@@ -186,6 +201,9 @@ private:
 
 	matrix::Dcmf _board_rotation;			/**< rotation matrix for the orientation that the board is mounted */
 
+	float _man_yaw_sp{0.f};				/**< current yaw setpoint in manual mode */
+	bool _gear_state_initialized{false};		/**< true if the gear state has been initialized */
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MC_ROLL_P>) _roll_p,
 		(ParamFloat<px4::params::MC_ROLLRATE_P>) _roll_rate_p,
@@ -221,6 +239,7 @@ private:
 		(ParamFloat<px4::params::MC_PITCHRATE_MAX>) _pitch_rate_max,
 		(ParamFloat<px4::params::MC_YAWRATE_MAX>) _yaw_rate_max,
 		(ParamFloat<px4::params::MC_YAWRAUTO_MAX>) _yaw_auto_max,
+		(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _yaw_rate_scaling,			/**< scaling factor from stick to yaw rate */
 
 		(ParamFloat<px4::params::MC_ACRO_R_MAX>) _acro_roll_max,
 		(ParamFloat<px4::params::MC_ACRO_P_MAX>) _acro_pitch_max,
@@ -238,7 +257,14 @@ private:
 
 		(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _board_offset_x,
 		(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _board_offset_y,
-		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _board_offset_z
+		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _board_offset_z,
+
+		/* Stabilized mode params */
+		(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _man_tilt_max_deg,			/**< maximum tilt allowed for manual flight */
+		(ParamFloat<px4::params::MPC_MANTHR_MIN>) _man_throttle_min,			/**< minimum throttle for stabilized */
+		(ParamFloat<px4::params::MPC_THR_MAX>) _throttle_max,				/**< maximum throttle for stabilized */
+		(ParamFloat<px4::params::MPC_THR_HOVER>) _throttle_hover,			/**< throttle at which vehicle is at hover equilibrium */
+		(ParamInt<px4::params::MPC_THR_CURVE>) _throttle_curve				/**< throttle curve behavior */
 	)
 
 	matrix::Vector3f _attitude_p;		/**< P gain for attitude control */
@@ -251,6 +277,7 @@ private:
 	matrix::Vector3f _mc_rate_max;		/**< attitude rate limits in stabilized modes */
 	matrix::Vector3f _auto_rate_max;	/**< attitude rate limits in auto modes */
 	matrix::Vector3f _acro_rate_max;	/**< max attitude rates in acro mode */
+	float _man_tilt_max;			/**< maximum tilt allowed for manual flight [rad] */
 
 };
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -177,6 +177,8 @@ MulticopterAttitudeControl::parameters_updated()
 	_acro_rate_max(1) = math::radians(_acro_pitch_max.get());
 	_acro_rate_max(2) = math::radians(_acro_yaw_max.get());
 
+	_man_tilt_max = math::radians(_man_tilt_max_deg.get());
+
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
 
 	/* get transformation matrix from sensor/board to body frame */
@@ -319,7 +321,15 @@ MulticopterAttitudeControl::vehicle_attitude_poll()
 	orb_check(_v_att_sub, &updated);
 
 	if (updated) {
+		uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter;
+
 		orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
+
+		// Check for a heading reset
+		if (prev_quat_reset_counter != _v_att.quat_reset_counter) {
+			// we only extract the heading change from the delta quaternion
+			_man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi();
+		}
 		return true;
 	}
 	return false;
@@ -368,6 +378,143 @@ MulticopterAttitudeControl::vehicle_land_detected_poll()
 
 }
 
+float
+MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
+{
+	// throttle_stick_input is in range [0, 1]
+	switch (_throttle_curve.get()) {
+	case 1: // no rescaling to hover throttle
+		return _man_throttle_min.get() + throttle_stick_input * (_throttle_max.get() - _man_throttle_min.get());
+
+	default: // 0 or other: rescale to hover throttle at 0.5 stick
+		if (throttle_stick_input < 0.5f) {
+			return (_throttle_hover.get() - _man_throttle_min.get()) / 0.5f * throttle_stick_input + _man_throttle_min.get();
+
+		} else {
+			return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle_stick_input - 1.0f) + _throttle_max.get();
+		}
+	}
+}
+
+float
+MulticopterAttitudeControl::get_landing_gear_state()
+{
+	// Only switch the landing gear up if we are not landed and if
+	// the user switched from gear down to gear up.
+	// If the user had the switch in the gear up position and took off ignore it
+	// until he toggles the switch to avoid retracting the gear immediately on takeoff.
+	if (_vehicle_land_detected.landed) {
+		_gear_state_initialized = false;
+	}
+	float landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; // default to down
+	if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
+		landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP;
+
+	} else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
+		// Switching the gear off does put it into a safe defined state
+		_gear_state_initialized = true;
+	}
+
+	return landing_gear;
+}
+
+void
+MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp)
+{
+	vehicle_attitude_setpoint_s attitude_setpoint{};
+	const float yaw = Eulerf(Quatf(_v_att.q)).psi();
+
+	/* reset yaw setpoint to current position if needed */
+	if (reset_yaw_sp) {
+		_man_yaw_sp = yaw;
+
+	} else if (_manual_control_sp.z > 0.05f) {
+
+		const float yaw_rate = math::radians(_yaw_rate_scaling.get());
+		attitude_setpoint.yaw_sp_move_rate = _manual_control_sp.r * yaw_rate;
+		_man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt);
+	}
+
+	/*
+	 * Input mapping for roll & pitch setpoints
+	 * ----------------------------------------
+	 * We control the following 2 angles:
+	 * - tilt angle, given by sqrt(x*x + y*y)
+	 * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion
+	 *
+	 * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick
+	 * points to, and changes of the stick input are linear.
+	 */
+	const float x = _manual_control_sp.x * _man_tilt_max;
+	const float y = _manual_control_sp.y * _man_tilt_max;
+
+	// we want to fly towards the direction of (x, y), so we use a perpendicular axis angle vector in the XY-plane
+	Vector2f v = Vector2f(y, -x);
+	float v_norm = v.norm(); // the norm of v defines the tilt angle
+
+	if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle
+		v *= _man_tilt_max / v_norm;
+	}
+
+	Quatf q_sp_rpy = AxisAnglef(v(0), v(1), 0.f);
+	Eulerf euler_sp = q_sp_rpy;
+	attitude_setpoint.roll_body = euler_sp(0);
+	attitude_setpoint.pitch_body = euler_sp(1);
+	// The axis angle can change the yaw as well (noticeable at higher tilt angles).
+	// This is the formula by how much the yaw changes:
+	//   let a := tilt angle, b := atan(y/x) (direction of maximum tilt)
+	//   yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))).
+	attitude_setpoint.yaw_body = _man_yaw_sp + euler_sp(2);
+
+	/* modify roll/pitch only if we're a VTOL */
+	if (_vehicle_status.is_vtol) {
+		// Construct attitude setpoint rotation matrix. Modify the setpoints for roll
+		// and pitch such that they reflect the user's intention even if a large yaw error
+		// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
+		// from the pure euler angle setpoints will lead to unexpected attitude behaviour from
+		// the user's view as the euler angle sequence uses the  yaw setpoint and not the current
+		// heading of the vehicle.
+		// However there's also a coupling effect that causes oscillations for fast roll/pitch changes
+		// at higher tilt angles, so we want to avoid using this on multicopters.
+		// The effect of that can be seen with:
+		// - roll/pitch into one direction, keep it fixed (at high angle)
+		// - apply a fast yaw rotation
+		// - look at the roll and pitch angles: they should stay pretty much the same as when not yawing
+
+		// calculate our current yaw error
+		float yaw_error = wrap_pi(attitude_setpoint.yaw_body - yaw);
+
+		// compute the vector obtained by rotating a z unit vector by the rotation
+		// given by the roll and pitch commands of the user
+		Vector3f zB = {0.0f, 0.0f, 1.0f};
+		Dcmf R_sp_roll_pitch = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, 0.0f);
+		Vector3f z_roll_pitch_sp = R_sp_roll_pitch * zB;
+
+		// transform the vector into a new frame which is rotated around the z axis
+		// by the current yaw error. this vector defines the desired tilt when we look
+		// into the direction of the desired heading
+		Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error);
+		z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
+
+		// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
+		// R_tilt is computed from_euler; only true if cos(roll) not equal zero
+		// -> valid if roll is not +-pi/2;
+		attitude_setpoint.roll_body = -asinf(z_roll_pitch_sp(1));
+		attitude_setpoint.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2));
+	}
+
+	/* copy quaternion setpoint to attitude setpoint topic */
+	Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body);
+	q_sp.copyTo(attitude_setpoint.q_d);
+	attitude_setpoint.q_d_valid = true;
+
+	attitude_setpoint.thrust = throttle_curve(_manual_control_sp.z);
+
+	attitude_setpoint.landing_gear = get_landing_gear_state();
+	attitude_setpoint.timestamp = hrt_absolute_time();
+	orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT);
+}
+
 /**
  * Attitude controller.
  * Input: 'vehicle_attitude_setpoint' topics (depending on mode)
@@ -666,6 +813,9 @@ MulticopterAttitudeControl::run()
 	float dt_accumulator = 0.f;
 	int loop_counter = 0;
 
+	bool reset_yaw_sp = true;
+	float attitude_dt = 0.f;
+
 	while (!should_exit()) {
 
 		poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
@@ -723,6 +873,7 @@ MulticopterAttitudeControl::run()
 			vehicle_land_detected_poll();
 			const bool manual_control_updated = vehicle_manual_poll();
 			const bool attitude_updated = vehicle_attitude_poll();
+			attitude_dt += dt;
 
 			/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
 			 * or roll (yaw can rotate 360 in normal att control).  If both are true don't
@@ -734,8 +885,19 @@ MulticopterAttitudeControl::run()
 				}
 			}
 
+			bool attitude_setpoint_generated = false;
+
 			if (_v_control_mode.flag_control_attitude_enabled) {
 				if (attitude_updated) {
+					// Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode
+					if (_v_control_mode.flag_control_manual_enabled &&
+							!_v_control_mode.flag_control_altitude_enabled &&
+							!_v_control_mode.flag_control_velocity_enabled &&
+							!_v_control_mode.flag_control_position_enabled) {
+						generate_attitude_setpoint(attitude_dt, reset_yaw_sp);
+						attitude_setpoint_generated = true;
+					}
+
 					control_attitude();
 					publish_rates_setpoint();
 				}
@@ -775,6 +937,13 @@ MulticopterAttitudeControl::run()
 				}
 			}
 
+			if (attitude_updated) {
+				reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) ||
+						_vehicle_land_detected.landed ||
+						(_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing); // VTOL in FW mode
+				attitude_dt = 0.f;
+			}
+
 			/* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */
 			if (!_v_control_mode.flag_armed || (now - task_start) < 3300000) {
 				dt_accumulator += dt;

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1003,25 +1003,6 @@ MulticopterPositionControl::start_flight_task()
 		}
 	}
 
-	// manual stabilized control
-	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL
-	    ||  _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || task_failure) {
-		should_disable_task = false;
-		int error = _flight_tasks.switchTask(FlightTaskIndex::ManualStabilized);
-
-		if (error != 0) {
-			if (prev_failure_count == 0) {
-				PX4_WARN("Stabilized-Ctrl failed with error: %s", _flight_tasks.errorToString(error));
-			}
-			task_failure = true;
-			_task_failure_count++;
-
-		} else {
-			check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_STAB);
-			task_failure = false;
-		}
-	}
-
 	// check task failure
 	if (task_failure) {