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
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);
// 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
