Mixer 6DoF (WIP): Added a special S mixer for 6-DoF multirotors

ROMFS/px4fmu_common/mixers/hexa_tilt_x.main.mix

@@ -1,3 +1,4 @@
 # Hexarotor with tilted arms
 
-R: 6xt 10000 10000 10000 0
+# R: 6xt 10000 10000 10000 0
+S: 6xt 10000 10000 10000 10000 10000 10000 0

src/lib/mixer/MixerGroup.cpp

@@ -200,6 +200,10 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle
 			m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid);
 			break;
 
+		case 'S':
+			m = MultirotorMixer6dof::from_text(control_cb, cb_handle, p, resid);
+			break;
+
 		case 'H':
 			m = HelicopterMixer::from_text(control_cb, cb_handle, p, resid);
 			break;

src/lib/mixer/MultirotorMixer/CMakeLists.txt

@@ -92,10 +92,12 @@ add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h ${CMA
 add_library(MultirotorMixer
 	MultirotorMixer.cpp
 	MultirotorMixer.hpp
+	MultirotorMixer6dof.cpp
 
 	${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h
 	${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h
 	${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
+	${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof_normalized.generated.h
 )
 target_link_libraries(MultirotorMixer PRIVATE Mixer)
 target_include_directories(MultirotorMixer PRIVATE ${CMAKE_CURRENT_BINARY_DIR})

src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp

@@ -258,3 +258,236 @@ private:
 	float 				*_outputs_prev{nullptr};
 	float 				*_tmp_array{nullptr};
 };
+
+/**
+ * 6-DoF multi-rotor mixer for pre-defined vehicle geometries.
+ *
+ * Collects six inputs (roll, pitch, yaw, x thrust, y thrust and z thrust) and mixes them to
+ * a set of outputs based on the configured geometry.
+ */
+class MultirotorMixer6dof : public Mixer
+{
+public:
+	/**
+	 * Precalculated rotor mix.
+	 */
+	struct Rotor {
+		float	roll_scale;	/**< scales roll for this rotor */
+		float	pitch_scale;	/**< scales pitch for this rotor */
+		float	yaw_scale;	/**< scales yaw for this rotor */
+		float	x_scale;	/**< scales x thrust for this rotor */
+		float	y_scale;	/**< scales y thrust for this rotor */
+		float	z_scale;	/**< scales z thrust for this rotor */
+		float	thrust_scale;	/**< TODO: Remove later after fixing the Airmod */
+	};
+
+	/**
+	 * Constructor.
+	 *
+	 * @param control_cb		Callback invoked to read inputs.
+	 * @param cb_handle		Passed to control_cb.
+	 * @param geometry		The selected geometry.
+	 * @param roll_scale		Scaling factor applied to roll inputs
+	 *				compared to thrust.
+	 * @param pitch_scale		Scaling factor applied to pitch inputs
+	 *				compared to thrust.
+	 * @param yaw_scale		Scaling factor applied to yaw inputs compared
+	 *				to thrust.
+	 * @param x_scale		Scaling factor applied to x thrust inputs
+	 *				compared to thrust.
+	 * @param y_scale		Scaling factor applied to y thrust inputs
+	 *				compared to thrust.
+	 * @param z_scale		Scaling factor applied to z thrust inputs
+	 *				compared to thrust.
+	 * @param idle_speed		Minimum rotor control output value; usually
+	 *				tuned to ensure that rotors never stall at the
+	 * 				low end of their control range.
+	 */
+	MultirotorMixer6dof(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry,
+			    float roll_scale, float pitch_scale, float yaw_scale,
+			    float x_scale, float y_scale, float z_scale, float idle_speed);
+
+	/**
+	 * Constructor (for testing).
+	 *
+	 * @param control_cb		Callback invoked to read inputs.
+	 * @param cb_handle		Passed to control_cb.
+	 * @param rotors		control allocation matrix
+	 * @param rotor_count		length of rotors array (= number of motors)
+	 */
+	MultirotorMixer6dof(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count);
+
+	virtual ~MultirotorMixer6dof();
+
+	// no copy, assignment, move, move assignment
+	MultirotorMixer6dof(const MultirotorMixer6dof &) = delete;
+	MultirotorMixer6dof &operator=(const MultirotorMixer6dof &) = delete;
+	MultirotorMixer6dof(MultirotorMixer6dof &&) = delete;
+	MultirotorMixer6dof &operator=(MultirotorMixer6dof &&) = delete;
+
+	/**
+	 * Factory method.
+	 *
+	 * Given a pointer to a buffer containing a text description of the mixer,
+	 * returns a pointer to a new instance of the mixer.
+	 *
+	 * @param control_cb		The callback to invoke when fetching a
+	 *				control value.
+	 * @param cb_handle		Handle passed to the control callback.
+	 * @param buf			Buffer containing a text description of
+	 *				the mixer.
+	 * @param buflen		Length of the buffer in bytes, adjusted
+	 *				to reflect the bytes consumed.
+	 * @return			A new MultirotorMixer6dof instance, or nullptr
+	 *				if the text format is bad.
+	 */
+	static MultirotorMixer6dof *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
+					      unsigned &buflen);
+
+	unsigned		mix(float *outputs, unsigned space) override;
+
+	uint16_t		get_saturation_status() override { return _saturation_status.value; }
+
+	void			groups_required(uint32_t &groups) override { groups |= (1 << 0); }
+
+	/**
+	 * @brief      Update slew rate parameter. This tells the multicopter mixer
+	 *             the maximum allowed change of the output values per cycle.
+	 *             The value is only valid for one cycle, in order to have continuous
+	 *             slew rate limiting this function needs to be called before every call
+	 *             to mix().
+	 *
+	 * @param[in]  delta_out_max  Maximum delta output.
+	 *
+	 */
+	void 			set_max_delta_out_once(float delta_out_max) override { _delta_out_max = delta_out_max; }
+
+	unsigned		set_trim(float trim) override { return _rotor_count; }
+	unsigned		get_trim(float *trim) override { return _rotor_count; }
+
+	/**
+	 * @brief      Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output.
+	 *
+	 * @param[in]  val   The value
+	 */
+	void			set_thrust_factor(float val) override { _thrust_factor = math::constrain(val, 0.0f, 1.0f); }
+
+	void 			set_airmode(Airmode airmode) override { _airmode = airmode; }
+
+	unsigned		get_multirotor_count() override { return _rotor_count; }
+
+	union saturation_status {
+		struct {
+			uint16_t valid		: 1; // 0 - true when the saturation status is used
+			uint16_t motor_pos	: 1; // 1 - true when any motor has saturated in the positive direction
+			uint16_t motor_neg	: 1; // 2 - true when any motor has saturated in the negative direction
+			uint16_t roll_pos	: 1; // 3 - true when a positive roll demand change will increase saturation
+			uint16_t roll_neg	: 1; // 4 - true when a negative roll demand change will increase saturation
+			uint16_t pitch_pos	: 1; // 5 - true when a positive pitch demand change will increase saturation
+			uint16_t pitch_neg	: 1; // 6 - true when a negative pitch demand change will increase saturation
+			uint16_t yaw_pos	: 1; // 7 - true when a positive yaw demand change will increase saturation
+			uint16_t yaw_neg	: 1; // 8 - true when a negative yaw demand change will increase saturation
+			uint16_t x_thrust_pos	: 1; // 9 - true when a positive x thrust demand change will increase saturation
+			uint16_t x_thrust_neg	: 1; //10 - true when a negative x thrust demand change will increase saturation
+			uint16_t y_thrust_pos	: 1; //11 - true when a positive y thrust demand change will increase saturation
+			uint16_t y_thrust_neg	: 1; //12 - true when a negative y thrust demand change will increase saturation
+			uint16_t z_thrust_pos	: 1; //13 - true when a positive z thrust demand change will increase saturation
+			uint16_t z_thrust_neg	: 1; //14 - true when a negative z thrust demand change will increase saturation
+		} flags;
+		uint16_t value;
+	};
+
+private:
+	/**
+	 * Computes the gain k by which desaturation_vector has to be multiplied
+	 * in order to unsaturate the output that has the greatest saturation.
+	 * @see also minimize_saturation().
+	 *
+	 * @return desaturation gain
+	 */
+	float compute_desaturation_gain(const float *desaturation_vector, const float *outputs, saturation_status &sat_status,
+					float min_output, float max_output) const;
+
+	/**
+	 * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector.
+	 * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular
+	 * acceleration on a specific axis.
+	 * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the
+	 * saturation will be minimized by shifting the vertical thrust setpoint, without changing the
+	 * roll/pitch/yaw accelerations.
+	 *
+	 * Note that as we only slide along the given axis, in extreme cases outputs can still contain values
+	 * outside of [min_output, max_output].
+	 *
+	 * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale
+	 * @param outputs output vector that is modified
+	 * @param sat_status saturation status output
+	 * @param min_output minimum desired value in outputs
+	 * @param max_output maximum desired value in outputs
+	 * @param reduce_only if true, only allow to reduce (substract) a fraction of desaturation_vector
+	 */
+	void minimize_saturation(const float *desaturation_vector, float *outputs, saturation_status &sat_status,
+				 float min_output = 0.f, float max_output = 1.f, bool reduce_only = false) const;
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the outputs vector.
+	 *
+	 * Desaturation behavior: airmode for roll/pitch:
+	 * thrust is increased/decreased as much as required to meet the demanded roll/pitch.
+	 * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
+	 */
+	inline void mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs);
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the outputs vector.
+	 *
+	 * Desaturation behavior: full airmode for roll/pitch/yaw:
+	 * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw,
+	 * while giving priority to roll and pitch over yaw.
+	 */
+	inline void mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs);
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the outputs vector.
+	 *
+	 * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
+	 * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
+	 * Thrust can be reduced to unsaturate the upper side.
+	 * @see mix_yaw() for the exact yaw behavior.
+	 */
+	inline void mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs);
+
+	/**
+	 * Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust).
+	 *
+	 * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow
+	 * some yaw control on the upper end. On the lower end thrust will never be increased,
+	 * but yaw is decreased as much as required.
+	 *
+	 * @param yaw demanded yaw
+	 * @param outputs output vector that is updated
+	 */
+	inline void mix_yaw(float yaw, float *outputs);
+
+	void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw);
+
+	float				_roll_scale{1.0f};
+	float				_pitch_scale{1.0f};
+	float				_yaw_scale{1.0f};
+	float				_x_scale{1.0f};
+	float				_y_scale{1.0f};
+	float				_z_scale{1.0f};
+	float				_idle_speed{0.0f};
+	float 				_delta_out_max{0.0f};
+	float 				_thrust_factor{0.0f};
+
+	Airmode				_airmode{Airmode::disabled};
+
+	saturation_status		_saturation_status{};
+
+	unsigned			_rotor_count;
+	const Rotor			*_rotors;
+
+	float 				*_outputs_prev{nullptr};
+	float 				*_tmp_array{nullptr};
+};