How to replace mc_att/rate_control and other modules for quadrocopter?

dmitry1945 · July 22, 2020, 6:34pm

Hi All,

I have PX4 2.8.4

I would like to replace the modules that make control for quadrotor (I would like to try LQR).
I understand that it should replace one of the modules mc_att_control or mc_rate_control (which one?). I need ekf2 and rc as inputs, and will have motor values as outputs.

But I don’t understand, in which format actuators are working and what is actuators?
For example in mc_rate_control I see:
actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
…
Then, as I understand, this actuators.control[…] will go to the mixer.
I don’t understand, how the yaw, pitch, rol and throtle converted to the ESC motor values?

Can someone help me this this here?

Thanks,
Dmitry

xdwgood · July 22, 2020, 11:15pm

Are you willing to share the code if you are successful?

You mean you want to use ekf2 and rc as input, and use motor speed as output instead of going through the mixer, right?

If so,The output of the current angular velocity controller is mixed here and finally output to esc

github.com

PX4/Firmware/blob/master/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp#L336



	for (unsigned i = 0; i < _rotor_count; i++) {
		_tmp_array[i] = _rotors[i].thrust_scale;
	}

	// reduce thrust only
	minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true);
}

unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
	if (space < _rotor_count) {
		return 0;
	}

	float roll    = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
	float pitch   = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
	float yaw     = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
	float thrust  = math::constrain(get_control(0, 3), 0.0f, 1.0f);

github.com

PX4/Firmware/blob/master/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp#L378


	// if a roll command exceeds the motor band limit.
	for (unsigned i = 0; i < _rotor_count; i++) {
		// Implement simple model for static relationship between applied motor pwm and motor thrust
		// model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2
		if (_thrust_factor > 0.0f) {
			outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) *
					(1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] /
							_thrust_factor));
		}

		outputs[i] = math::constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
	}

	// Slew rate limiting and saturation checking
	for (unsigned i = 0; i < _rotor_count; i++) {
		bool clipping_high = false;
		bool clipping_low_roll_pitch = false;
		bool clipping_low_yaw = false;

		// Check for saturation against static limits.
		// We only check for low clipping if airmode is disabled (or yaw

As for which module to replace, it depends on which loop you use lqr control.If you plan to use the lqr algorithm in angular velocity control, then just replacing/modifying mc_rate_control is enough

dmitry1945 · July 23, 2020, 9:09pm

Hi Xdwgood,

Yes, I plan to publish.

I have check and this code is not affected by the PWM output. I have check MultirotorMixer and other mixers and did not found the place, where the controls.control[…] converted to the motor output values…
I see, that in
PX4IO::io_set_control_state(unsigned group)
… for (unsigned i = 0; (i < _max_controls) && (i < sizeof(controls.control) / sizeof(controls.control[0])); i++) {
/* ensure FLOAT_TO_REG does not produce an integer overflow */
…

we copy control values to the px4io controller.
The MultirotorMixer or other mixer should be executed there, but it’s not.
I just need to know the law for transformation between controls.control and real ESC outputs.