Multi-rotor mixer for pre-defined vehicle geometries. More...

#include <MultirotorMixer.hpp>

Inheritance diagram for MultirotorMixer:

Collaboration diagram for MultirotorMixer:

Classes
struct	Rotor
	Precalculated rotor mix. More...

union	saturation_status

Public Member Functions
	MultirotorMixer (ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry, float roll_scale, float pitch_scale, float yaw_scale, float idle_speed)
	Constructor. More...

	MultirotorMixer (ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count)
	Constructor (for testing). More...

virtual	~MultirotorMixer ()

	MultirotorMixer (const MultirotorMixer &)=delete

MultirotorMixer &	operator= (const MultirotorMixer &)=delete

	MultirotorMixer (MultirotorMixer &&)=delete

MultirotorMixer &	operator= (MultirotorMixer &&)=delete

unsigned	mix (float *outputs, unsigned space) override
	Perform the mixing function. More...

uint16_t	get_saturation_status () override
	Get the saturation status. More...

void	groups_required (uint32_t &groups) override
	Analyses the mix configuration and updates a bitmask of groups that are required. More...

void	set_max_delta_out_once (float delta_out_max) override
	Update slew rate parameter. More...

unsigned	set_trim (float trim) override
	Set trim offset for this mixer. More...

unsigned	get_trim (float *trim) override
	Get trim offset for this mixer. More...

void	set_thrust_factor (float val) override
	Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output. More...

void	set_airmode (Airmode airmode) override
	Set airmode. More...

unsigned	get_multirotor_count () override

Public Member Functions inherited from Mixer
	Mixer (ControlCallback control_cb, uintptr_t cb_handle)
	Constructor. More...

virtual	~Mixer ()=default

	Mixer (const Mixer &)=delete

Mixer &	operator= (const Mixer &)=delete

	Mixer (Mixer &&)=delete

Mixer &	operator= (Mixer &&)=delete

Public Member Functions inherited from ListNode< Mixer *>
void	setSibling (Mixer * sibling)

const Mixer *	getSibling () const

Static Public Member Functions
static MultirotorMixer *	from_text (Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
	Factory method. More...

Private Member Functions
float	compute_desaturation_gain (const float desaturation_vector, const float outputs, saturation_status &sat_status, float min_output, float max_output) const
	Computes the gain k by which desaturation_vector has to be multiplied in order to unsaturate the output that has the greatest saturation. More...

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
	Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector. More...

void	mix_airmode_rp (float roll, float pitch, float yaw, float thrust, float *outputs)
	Mix roll, pitch, yaw, thrust and set the outputs vector. More...

void	mix_airmode_rpy (float roll, float pitch, float yaw, float thrust, float *outputs)
	Mix roll, pitch, yaw, thrust and set the outputs vector. More...

void	mix_airmode_disabled (float roll, float pitch, float yaw, float thrust, float *outputs)
	Mix roll, pitch, yaw, thrust and set the outputs vector. More...

void	mix_yaw (float yaw, float *outputs)
	Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust). More...

void	update_saturation_status (unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw)

Private Attributes
float	_roll_scale {1.0f}

float	_pitch_scale {1.0f}

float	_yaw_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}

Additional Inherited Members
Public Types inherited from Mixer
enum	Airmode : int32_t { Airmode::disabled = 0, Airmode::roll_pitch = 1, Airmode::roll_pitch_yaw = 2 }

typedef int(*	ControlCallback) (uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
	Fetch a control value. More...

Protected Member Functions inherited from Mixer
float	get_control (uint8_t group, uint8_t index)
	Invoke the client callback to fetch a control value. More...

Static Protected Member Functions inherited from Mixer
static const char *	findtag (const char *buf, unsigned &buflen, char tag)
	Find a tag. More...

static char	findnexttag (const char *buf, unsigned buflen)
	Find next tag and return it (0 is returned if no tag is found) More...

static const char *	skipline (const char *buf, unsigned &buflen)
	Skip a line. More...

static bool	string_well_formed (const char *buf, unsigned &buflen)
	Check wether the string is well formed and suitable for parsing. More...

Protected Attributes inherited from Mixer
ControlCallback	_control_cb
	client-supplied callback used when fetching control values More...

uintptr_t	_cb_handle

Protected Attributes inherited from ListNode< Mixer *>
Mixer *	_list_node_sibling

Detailed Description

Multi-rotor mixer for pre-defined vehicle geometries.

Collects four inputs (roll, pitch, yaw, thrust) and mixes them to a set of outputs based on the configured geometry.

Definition at line 52 of file MultirotorMixer.hpp.

Constructor & Destructor Documentation

◆ MultirotorMixer() [1/4]

MultirotorMixer::MultirotorMixer	(	ControlCallback	control_cb,
		uintptr_t	cb_handle,
		MultirotorGeometry	geometry,
		float	roll_scale,
		float	pitch_scale,
		float	yaw_scale,
		float	idle_speed
	)

Constructor.

Parameters

control_cb	Callback invoked to read inputs.
cb_handle	Passed to control_cb.
geometry	The selected geometry.
roll_scale	Scaling factor applied to roll inputs compared to thrust.
pitch_scale	Scaling factor applied to pitch inputs compared to thrust.
yaw_wcale	Scaling factor applied to yaw inputs compared to thrust.
idle_speed	Minimum rotor control output value; usually tuned to ensure that rotors never stall at the low end of their control range.

Definition at line 82 of file MultirotorMixer.cpp.

References _idle_speed, _pitch_scale, _roll_scale, and _yaw_scale.

Referenced by from_text().

Here is the caller graph for this function:

◆ MultirotorMixer() [2/4]

MultirotorMixer::MultirotorMixer	(	ControlCallback	control_cb,
		uintptr_t	cb_handle,
		const Rotor *	rotors,
		unsigned	rotor_count
	)

Constructor (for testing).

Parameters

control_cb	Callback invoked to read inputs.
cb_handle	Passed to control_cb.
rotors	control allocation matrix
rotor_count	length of rotors array (= number of motors)

Definition at line 92 of file MultirotorMixer.cpp.

References _idle_speed, _outputs_prev, and _rotor_count.

◆ ~MultirotorMixer()

MultirotorMixer::~MultirotorMixer ( )

virtual

Definition at line 105 of file MultirotorMixer.cpp.

References _outputs_prev, and _tmp_array.

◆ MultirotorMixer() [3/4]

MultirotorMixer::MultirotorMixer ( const MultirotorMixer & )

delete

◆ MultirotorMixer() [4/4]

MultirotorMixer::MultirotorMixer ( MultirotorMixer && )

delete

Member Function Documentation

◆ compute_desaturation_gain()

float MultirotorMixer::compute_desaturation_gain	(	const float *	desaturation_vector,
		const float *	outputs,
		saturation_status &	sat_status,
		float	min_output,
		float	max_output
	)		const

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: also minimize_saturation().

Returns: desaturation gain

Definition at line 169 of file MultirotorMixer.cpp.

References _rotor_count, MultirotorMixer::saturation_status::flags, FLT_EPSILON, MultirotorMixer::saturation_status::motor_neg, and MultirotorMixer::saturation_status::motor_pos.

Referenced by minimize_saturation().

Here is the caller graph for this function:

◆ from_text()

MultirotorMixer * MultirotorMixer::from_text	(	Mixer::ControlCallback	control_cb,
		uintptr_t	cb_handle,
		const char *	buf,
		unsigned &	buflen
	)

static

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.

Parameters

control_cb	The callback to invoke when fetching a control value.
cb_handle	Handle passed to the control callback.
buf	Buffer containing a text description of the mixer.
buflen	Length of the buffer in bytes, adjusted to reflect the bytes consumed.

Returns: A new MultirotorMixer instance, or nullptr if the text format is bad.

Definition at line 112 of file MultirotorMixer.cpp.

References debug, f(), MultirotorMixer(), Mixer::skipline(), and Mixer::string_well_formed().

Referenced by uart_esc::initialize_mixer(), snapdragon_pwm::initialize_mixer(), and MixerGroup::load_from_buf().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_multirotor_count()

unsigned MultirotorMixer::get_multirotor_count ( )

inlineoverridevirtual

Reimplemented from Mixer.

Definition at line 151 of file MultirotorMixer.hpp.

Referenced by MixerGroup::get_multirotor_count().

Here is the caller graph for this function:

◆ get_saturation_status()

uint16_t MultirotorMixer::get_saturation_status ( )

inlineoverridevirtual

Get the saturation status.

Returns: Integer bitmask containing saturation_status from multirotor_motor_limits.msg.

Reimplemented from Mixer.

Definition at line 123 of file MultirotorMixer.hpp.

Referenced by MixerGroup::get_saturation_status().

Here is the caller graph for this function:

◆ get_trim()

unsigned MultirotorMixer::get_trim ( float * trim )

inlineoverridevirtual

Get trim offset for this mixer.

Returns: the number of outputs this mixer feeds to

Reimplemented from Mixer.

Definition at line 140 of file MultirotorMixer.hpp.

Referenced by MixerGroup::get_trims().

Here is the caller graph for this function:

◆ groups_required()

void MultirotorMixer::groups_required ( uint32_t & groups )

inlineoverridevirtual

Analyses the mix configuration and updates a bitmask of groups that are required.

Parameters

groups A bitmask of groups (0-31) that the mixer requires.

Reimplemented from Mixer.

Definition at line 125 of file MultirotorMixer.hpp.

Referenced by MixerGroup::groups_required(), and snapdragon_pwm::task_main().

Here is the caller graph for this function:

◆ minimize_saturation()

void MultirotorMixer::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

private

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

Parameters

desaturation_vector	vector that is added to the outputs, e.g. thrust_scale
outputs	output vector that is modified
sat_status	saturation status output
min_output	minimum desired value in outputs
max_output	maximum desired value in outputs
reduce_only	if true, only allow to reduce (substract) a fraction of desaturation_vector

Definition at line 207 of file MultirotorMixer.cpp.

References _rotor_count, compute_desaturation_gain(), and f().

Referenced by mix_airmode_disabled(), mix_airmode_rp(), mix_airmode_rpy(), and mix_yaw().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mix()

unsigned MultirotorMixer::mix	(	float *	outputs,
		unsigned	space
	)

overridevirtual

Perform the mixing function.

Parameters

outputs	Array into which mixed output(s) should be placed.
space	The number of available entries in the output array;

Returns: The number of entries in the output array that were populated.

Implements Mixer.

Definition at line 336 of file MultirotorMixer.cpp.

References _airmode, _delta_out_max, _idle_speed, _outputs_prev, _pitch_scale, _roll_scale, _rotor_count, _saturation_status, _thrust_factor, _yaw_scale, math::constrain(), Mixer::disabled, f(), Mixer::get_control(), mix_airmode_disabled(), mix_airmode_rp(), mix_airmode_rpy(), Mixer::roll_pitch, Mixer::roll_pitch_yaw, update_saturation_status(), and MultirotorMixer::saturation_status::value.

Referenced by main(), MixerGroup::mix(), uart_esc::task_main(), and snapdragon_pwm::task_main().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mix_airmode_disabled()

void MultirotorMixer::mix_airmode_disabled	(	float	roll,
		float	pitch,
		float	yaw,
		float	thrust,
		float *	outputs
	)

inlineprivate

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 also: mix_yaw() for the exact yaw behavior.

Definition at line 279 of file MultirotorMixer.cpp.

References _rotor_count, _rotors, _saturation_status, _tmp_array, f(), minimize_saturation(), mix_yaw(), MultirotorMixer::Rotor::pitch_scale, MultirotorMixer::Rotor::roll_scale, and MultirotorMixer::Rotor::thrust_scale.

Referenced by mix().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mix_airmode_rp()

void MultirotorMixer::mix_airmode_rp	(	float	roll,
		float	pitch,
		float	yaw,
		float	thrust,
		float *	outputs
	)

inlineprivate

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 also: mix_yaw() for the exact behavior.

Definition at line 231 of file MultirotorMixer.cpp.

References _rotor_count, _rotors, _saturation_status, _tmp_array, minimize_saturation(), mix_yaw(), MultirotorMixer::Rotor::pitch_scale, MultirotorMixer::Rotor::roll_scale, and MultirotorMixer::Rotor::thrust_scale.

Referenced by mix().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mix_airmode_rpy()

void MultirotorMixer::mix_airmode_rpy	(	float	roll,
		float	pitch,
		float	yaw,
		float	thrust,
		float *	outputs
	)

inlineprivate

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.

Definition at line 252 of file MultirotorMixer.cpp.

References _rotor_count, _rotors, _saturation_status, _tmp_array, minimize_saturation(), MultirotorMixer::Rotor::pitch_scale, MultirotorMixer::Rotor::roll_scale, MultirotorMixer::Rotor::thrust_scale, and MultirotorMixer::Rotor::yaw_scale.

Referenced by mix().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ mix_yaw()

void MultirotorMixer::mix_yaw	(	float	yaw,
		float *	outputs
	)

inlineprivate

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.

Parameters

yaw	demanded yaw
outputs	output vector that is updated

Definition at line 313 of file MultirotorMixer.cpp.

References _rotor_count, _rotors, _saturation_status, _tmp_array, f(), minimize_saturation(), MultirotorMixer::Rotor::thrust_scale, and MultirotorMixer::Rotor::yaw_scale.

Referenced by mix_airmode_disabled(), and mix_airmode_rp().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ operator=() [1/2]

MultirotorMixer& MultirotorMixer::operator= ( const MultirotorMixer & )

delete

◆ operator=() [2/2]

MultirotorMixer& MultirotorMixer::operator= ( MultirotorMixer && )

delete

◆ set_airmode()

void MultirotorMixer::set_airmode ( Airmode airmode )

inlineoverridevirtual

Set airmode.

Airmode allows the mixer to increase the total thrust in order to unsaturate the motors.

Parameters

[in] airmode Select airmode type (0 = disabled, 1 = roll/pitch, 2 = roll/pitch/yaw)

Reimplemented from Mixer.

Definition at line 149 of file MultirotorMixer.hpp.

Referenced by main(), MixerGroup::set_airmode(), and snapdragon_pwm::task_main().

Here is the caller graph for this function:

◆ set_max_delta_out_once()

void MultirotorMixer::set_max_delta_out_once ( float delta_out_max )

inlineoverridevirtual

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().

Parameters

[in] delta_out_max Maximum delta output.

Reimplemented from Mixer.

Definition at line 137 of file MultirotorMixer.hpp.

Referenced by MixerGroup::set_max_delta_out_once().

Here is the caller graph for this function:

◆ set_thrust_factor()

void MultirotorMixer::set_thrust_factor ( float val )

inlineoverridevirtual

Sets the thrust factor used to calculate mapping from desired thrust to motor control signal output.

Parameters

[in] val The value

Reimplemented from Mixer.

Definition at line 147 of file MultirotorMixer.hpp.

References math::constrain(), and f().

Referenced by MixerGroup::set_thrust_factor().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_trim()

unsigned MultirotorMixer::set_trim ( float trim )

inlineoverridevirtual

Set trim offset for this mixer.

Returns: the number of outputs this mixer feeds to

Reimplemented from Mixer.

Definition at line 139 of file MultirotorMixer.hpp.

Referenced by MixerGroup::set_trims().

Here is the caller graph for this function:

◆ update_saturation_status()

void MultirotorMixer::update_saturation_status	(	unsigned	index,
		bool	clipping_high,
		bool	clipping_low_roll_pitch,
		bool	clipping_low_yaw
	)

private

Definition at line 438 of file MultirotorMixer.cpp.

Referenced by mix().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ _airmode

Airmode MultirotorMixer::_airmode {Airmode::disabled}

private

Definition at line 251 of file MultirotorMixer.hpp.

Referenced by mix().

◆ _delta_out_max

float MultirotorMixer::_delta_out_max {0.0f}

private

Definition at line 248 of file MultirotorMixer.hpp.

Referenced by mix().

◆ _idle_speed

float MultirotorMixer::_idle_speed {0.0f}

private

Definition at line 247 of file MultirotorMixer.hpp.

Referenced by mix(), and MultirotorMixer().

◆ _outputs_prev

float* MultirotorMixer::_outputs_prev {nullptr}

private

Definition at line 258 of file MultirotorMixer.hpp.

Referenced by mix(), MultirotorMixer(), and ~MultirotorMixer().

◆ _pitch_scale

float MultirotorMixer::_pitch_scale {1.0f}

private

Definition at line 245 of file MultirotorMixer.hpp.

Referenced by mix(), and MultirotorMixer().

◆ _roll_scale

float MultirotorMixer::_roll_scale {1.0f}

private

Definition at line 244 of file MultirotorMixer.hpp.

Referenced by mix(), and MultirotorMixer().

◆ _rotor_count

unsigned MultirotorMixer::_rotor_count

private

Definition at line 255 of file MultirotorMixer.hpp.

Referenced by compute_desaturation_gain(), minimize_saturation(), mix(), mix_airmode_disabled(), mix_airmode_rp(), mix_airmode_rpy(), mix_yaw(), and MultirotorMixer().

◆ _rotors

const Rotor* MultirotorMixer::_rotors

private

Definition at line 256 of file MultirotorMixer.hpp.

Referenced by mix_airmode_disabled(), mix_airmode_rp(), mix_airmode_rpy(), mix_yaw(), and update_saturation_status().

◆ _saturation_status

saturation_status MultirotorMixer::_saturation_status {}

private

Definition at line 253 of file MultirotorMixer.hpp.

Referenced by mix(), mix_airmode_disabled(), mix_airmode_rp(), mix_airmode_rpy(), mix_yaw(), and update_saturation_status().

◆ _thrust_factor

float MultirotorMixer::_thrust_factor {0.0f}

private

Definition at line 249 of file MultirotorMixer.hpp.

Referenced by mix().

◆ _tmp_array

float* MultirotorMixer::_tmp_array {nullptr}

private

Definition at line 259 of file MultirotorMixer.hpp.

Referenced by mix_airmode_disabled(), mix_airmode_rp(), mix_airmode_rpy(), mix_yaw(), and ~MultirotorMixer().

◆ _yaw_scale

float MultirotorMixer::_yaw_scale {1.0f}

private

Definition at line 246 of file MultirotorMixer.hpp.

Referenced by mix(), and MultirotorMixer().

The documentation for this class was generated from the following files:

src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp
src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp

Classes

Public Member Functions

Static Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ MultirotorMixer() [1/4]

◆ MultirotorMixer() [2/4]

◆ ~MultirotorMixer()

◆ MultirotorMixer() [3/4]

◆ MultirotorMixer() [4/4]

Member Function Documentation

◆ compute_desaturation_gain()

◆ from_text()

◆ get_multirotor_count()

◆ get_saturation_status()

◆ get_trim()

◆ groups_required()

◆ minimize_saturation()

◆ mix()

◆ mix_airmode_disabled()

◆ mix_airmode_rp()

◆ mix_airmode_rpy()

◆ mix_yaw()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ set_airmode()

◆ set_max_delta_out_once()

◆ set_thrust_factor()

◆ set_trim()

◆ update_saturation_status()

Member Data Documentation

◆ _airmode

◆ _delta_out_max

◆ _idle_speed

◆ _outputs_prev

◆ _pitch_scale

◆ _roll_scale

◆ _rotor_count

◆ _rotors

◆ _saturation_status

◆ _thrust_factor

◆ _tmp_array

◆ _yaw_scale