Dynamic control allocation for VTOL & others

[bkueng]

This brings the ideas from #18701 and #18558, based on the valuable input from @sfuhrer, @dagar and others.

Details:

• handle reversible motors
• adds separate servo range reversal params (bitset) for simplified configuration
• adds trim + slew rate configuration
• adds support for multiple matrices in control_allocator. This is used for the existing VTOL control modules.
• adds control surfaces + tilt configuration. The tilt captures the physical properties, like min and max tilt angles and direction of tilt. Motors can be assigned to a tilt. For tiltrotors, this is then used to automatically update the motor axis and disable non-tiltable motors during fixed-wing flight (see example UI below).
• generalizes and extends ActuatorEffectiveness:
  • classes can be used hierarchically. For example a VTOL can include motors + control surfaces.
  • adds updateSetpoint callback method to allow it to manipulate the actuator setpoint after allocation (e.g. to add flaps or any kind of non-linear control)
  • handle motor stopping (disable existing vtol logic to manipulate pwm's directly)
• handle standard vtol + tiltrotor
• adds support for rovers (with a fixed effectiveness - this can later still be a bit generalized)
• adds 6DOF vectored thrust UI, used and tested with the UUV gazebo model
• performance on F4 running a vtol (400hz): 4KB more RAM, interestingly ~1% less CPU, and doing matrix inversion @ 10Hz uses about 1% CPU.

Tiltrotor example:

There's still a few more to be added, like tailsitter or tiltable MC's. I also intend to make the motor thrust axis a bit more configurable in the UI, but likely this goes along with some UI changes.

Test flown on a tiltrotor (convergence) - there's still a couple of details to iron out, but it generally works nicely.

We still need to define the control surface torque convention (which we can use to then fill out the constraints table), but I leave that up to others to decide.

See commit messages for further details.

[bkueng]

@jlecoeur what was the original intent for _control_trim? My use here is to add an offset to an actuator, but the _control_trim logic leads to undesired effects, and I did not just want to remove it w/o knowing the background.

[jlecoeur]

Linearization around a specific actuator state. In a linear system the equation above reduces and trim could be removed. However in a non-linear case --for example a tiltrotor-- it is required.
What are the undesired effects?

[bkueng]

Makes sense. For example I added a trim of 0.2 to the right aileron for a FW setup, that resulted in a roll=pitch=0.2 control trim, which then overall resulted in an aileron setpoint of -0.2410.
Sounds like we need both then, as it's 2 different use-cases.

[jlecoeur]

@bkueng and @sfuhrer should I review?
I just read that you introduced two matrices for VTOL, I fear that this defeats the purpose of effectiveness matrix inversion -- almost back to the former mixer system right?

[sfuhrer]

@bkueng I pushed some additional VTOL fixes. de380c4 shows one way to handle the throttle allocation for tiltrotor without needing to change too much on the legacy mixing system. Happy to discuss other ideas.

[sfuhrer]

@bkueng and @sfuhrer should I review? I just read that you introduced two matrices for VTOL, I fear that this defeats the purpose of effectiveness matrix inversion -- almost back to the former mixer system right?

Yes it's two matrices. We chose that approach to be able to bring in dynamic control allocation without the need to change the whole VTOL control pipeline. Benefits compared to the legacy mixing system: much easier setup without the need to recompile, enables actuator failure handling and improves tiltrotor control by taking into account the motor tilts.
The step to an unified effectiveness matrix is a much bigger step that will help us to get rid of a lot of vtol specialties all over px4, but a user would not notice a lot, as the interface doesn't have to change.

[sfuhrer]

@bkueng and @sfuhrer should I review?

Hi @jlecoeur , yes would be cool if you could review. We're adding smaller fixer here and there while we go through all the vehicle types and test them in SITL and real hardware, but the core algorithm/concept will likely not change drastically anymore unless we find something.

[bkueng]

@bkueng and @sfuhrer should I review?

You're welcome to review, yes.

I also added a 'Custom' effectiveness that serves as a catch-all. I think everything should be possible now, except for helicopters, and some mixers that mix in RC aux channels.

[sfuhrer]

Testflown on this bunch of illustrious VTOLs today:

FunCub Standard VTOL: https://review.px4.io/plot_app?log=118268e4-5bfa-462a-88d5-63543afe6d82
Convergence Tiltrotor VTOL: https://review.px4.io/plot_app?log=0c3141ee-407d-4147-9d3f-5b89c06fb116
Xvert Tailsitter VTOL: https://review.px4.io/plot_app?log=f1bab1e5-4e73-49c4-ae73-56be56c8ebdc

All flew fine in hover and fixed-wing modes without too much rate controller tuning, though we could in general tweak the FW rate defaults a bit (less FF, more P and I). This is though not particularly related to this PR.
The tiltrotor (dynamically updated matrix depending on combined tilt) and the tailsitter flew better than I ever saw them, on the standard VTOL no difference was noticeable.

@dagar @jlecoeur, or anybody else: a review would still be very much appreciated!

[dagar]

I'm still working through this, but in the meantime we have a small flash overflow on fmu-v2. https://github.com/PX4/PX4-Autopilot/runs/4606740563?check_suite_focus=true

[dagar]

Flash overflow resolved and rebased on master.

[dagar]

Isn't having 2 mechanisms to reverse an output going to be potentially confusing?

• output reversed if MIN > MAX
• per output group REV bits (eg PWM_MAIN_REV)

Then in the old mixing world we already have a REV parameter per ouput (eg PWM_MAIN_REVx).
Can we keep PWM_MAIN_REVx instead?

[dagar]

One of the bigger takeaways I had from the discussion in #18558 was the handling of control surfaces by type and constraining torque appropriately (eg left vs right aileron).

Is there something else planned to do this with the GUI?

EDIT: I think I found it.

[dagar]

It took me a while to go through this, but overall it looks good and I'd like to get it in before it gets any bigger or harder to rebase or we run out of flash again, etc.

notes/questions/comments

• I'd still like to revisit the reverse configuration (Dynamic control allocation for VTOL & others #18776 (comment))
• control surface constraints handled firmware side, so that it's still possible to safely configure things directly without the GUI
• per output trim (eg PWM_MAIN_TRIMx)
• I'm hoping we'll ultimately be able to consolidate quite a lot of this into something more generally configurable rather than carrying every possible airframe specialization
• can we try to do these things more incrementally going forward?

[mengchaoheng]

some idea in the book "Aircraft Control Allocation" maybe can help you, the author provides some simulation files which can also be downloaded at https://github.com/mengchaoheng/aircraft-control-allocation.git

[Jawad-RoboLearn]

I am trying to check out this PR but it gives error,
fatal: couldn't find remote ref refs/heads/control_allocation_vtol
exit status 128

[Jaeyoung-Lim]

@Jawad-RoboLearn The branch no longer exists from the PR since the branch was deleted after the merge. The commit is merged to the master branch, so you can check out there.

[Jawad-RoboLearn]

Thank You.

[mengchaoheng]

some idea in the book "Aircraft Control Allocation" maybe can help you, the author provides some simulation files which can also be downloaded at https://github.com/mengchaoheng/aircraft-control-allocation.git

I have fixed bugs in the source code of this reference book, which can be found in my repository. The qcat from Ola Härkegård's is very helpful!!
@bkueng

[VTOLDavid]

Wich motor gain matrix will result for a quadcopter in deadcat configuration (Front and rear motors same distance to CoG in x axis but different in y axis)?