Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Multicopter mixer saturation handling strategy #7920

Closed
wants to merge 17 commits into from

Conversation

bresch
Copy link
Member

@bresch bresch commented Sep 5, 2017

After having some issues just after back-transition with a standard VTOL, I found that the problem came from the multicopter mixer.
Since after back-transition the wings can still produce quite a lot of lift with high AOA and high-lift wings, the multicopter thrust is close to zero and the motors are directly saturated when trying to compensate attitude errors. In this special case, the initial 'boost' technique - a gain applied to the thrust - is ineffective:

  • If thrust is close to zero, thrust * thrust_increase_factor is also close to zero...

The 2nd issue highlighted by @sanderux in #7871 is that when the altitude control is active, the thrust saturates the outputs and attitude control becomes sloppy.

I made several passes to first simplify the code and finally rewrite the saturation handling process in order to get rid of the arbitrary boost gains and make the attitude control the highest priority.

Before implementing in PX4, I first made a python script to simulate the different cases, you can try here:
https://gist.github.com/bresch/8ea4232452bc64de1ed780bcfbee3564

Be aware that I never tested this PR in reality and that the mixer code is critical. Be careful when testing!

@bresch
Copy link
Member Author

bresch commented Sep 5, 2017

@eyeam3 Please also try this PR on the Falcon Vertigo if you have time.

@bresch bresch requested review from eyeam3 and RomanBapst September 5, 2017 12:59
@dagar
Copy link
Member

dagar commented Sep 5, 2017

That's certainly much simpler. Does anyone know how the current mixer evolved? What do you think @Stifael @MaEtUgR?

I'm not necessarily suggesting it here, but one thing to be aware of is we could have special multicopter mixer classes for quadplanes if needed.

@Stifael
Copy link
Contributor

Stifael commented Sep 5, 2017

not sure if an additional mixer is needed for quadplanes, unless people are very happy with the current multi-copter mixer. My personal opinion agrees with what @bresch is suggesting to have attitude highest priority to allow full attitude control even at zero thrust (which is currently not the case). However I know that opinions tend to differ sharply and depends a lot on the use case (someone who flies a racer would definitely prefer full attitude control at anytime).

@dagar
Copy link
Member

dagar commented Sep 5, 2017

Right, I mainly want people to understand the possible level of flexibility we have here. It would be easy to have a couple different implementations that extend MultirotorMixer and only make minor changes.

Would unit tests help verify intended mixer functionality and any edge cases?
@diegoeck I believe you offered to help write mixer testing a long time ago. What do you think?

@diegoeck
Copy link

diegoeck commented Sep 5, 2017

Hi @dagar! Sure I can help. I have already study the mixer some time ago (and corrected some bugs), and I was planing to simplify the code in the next weeks with one of my students. I think that the code is very complicated, and it would be nice to better understand how it evoluted. Sometimes I think that a really simple mixer (with no priorities) would perform better, but it is difficult to say. Sometimes altitude should have larger priority, sometimes attitude, so maybe no priority would work.. Many will say that's not the way. My idea was to test in simulation many scenarios and compare performance, but I have only experience with quads and I would need help with VTOL.

@sanderux
Copy link
Member

sanderux commented Sep 5, 2017

It seems to me that (at least at a certain point) prioritizing altitude over attitude would be pointless. When the vehicle is at a 25 percent bank angle there seems to be little point in increasing thrust to gain altitude. I think it would be much more efficient to prioritize the attitude and as soon as we are level try altitude.

@AndreasAntener thoughts?

@mrpollo
Copy link
Contributor

mrpollo commented Sep 6, 2017

@dagar should we have @PX4TestFlights test this PR?

@bkueng
Copy link
Member

bkueng commented Sep 7, 2017

I think I hit that problem as well recently: https://logs.px4.io/plot_app?log=6c4e8474-63dc-4a54-b2ca-9f9ee05566e0
This is on a normal quad. You can see several very large overshoots in roll/pitch angle (going over 90 degrees), for example the roll angle at 5:10. It can be quite easily reproduced by tilting in one direction to the maximum, then quickly tilt towards the other direction and at the same time reduce throttle to zero. The attitude will then overshoot.
To me, it looks like it is due to the same problem: thrust is too low so that the attitude is not corrected anymore.

@mrpollo I think we should first do more SITL/HITL testing, before putting this on a vehicle. If you are brave enough, then sure, flash it! :) But keep the kill switch ready.

@LorenzMeier
Copy link
Member

@PX4TestFlights Could you please do a careful test with kill switch ready?

@santiago3dr
Copy link

@dagar
Copy link
Member

dagar commented Sep 13, 2017

@diegoeck would a mixer unit test be helpful or should we do it within a larger simulation? How would you compare performance or identify failures?

@sanderux
Copy link
Member

I can perform the same test i did before, it is repeatable and quite obvious when it happens. Just need to wait untill the weather becomes a bit calmer. Expected for this weekend.

@bkueng
Copy link
Member

bkueng commented Sep 15, 2017

I tested this as well, first in HIL and then on a quad with pixracer. I mapped aux1 to the previous behavior to switch in-flight (see branch manual_input_rework_mixer_rework, which is on top of #7940).

Tested:

  • full/zero throttle (free fall)
  • fast roll & pitch changes, including yawing
  • I set the minimum throttle to 0% and the max to 100% (MPC_MANTHR_MIN and MPC_MANTHR_MAX), in all the logs.

Logs:

Observations:

  • the large angle overshoot when reducing throttle to 0 (see above post), does not happen anymore. In fact when I tested with the old behavior, it overshot by so much, that it did a full 360 degree roll rotation (first log, 4:37). Thus answering the question whether PX4 can handle flips: yes it can 😃
    roll_flip
  • roll & pitch stay controllable in freefall even with 0 throttle (I tested with the previous as well: the vehicle just tips over). However yaw is not controllable.
  • you can safely go to 100% throttle.

Conclusion: I'm confident flying this, it behaves as it should, but I can't say that I covered all the corner cases.

@sanderux
Copy link
Member

We will be flying this this weekend and will duplicate the behavior in #7871

@r0gelion
Copy link
Contributor

Unassigned @PX4TestFlights, reassign if needed.

sanderux
sanderux previously approved these changes Sep 18, 2017
Copy link
Member

@sanderux sanderux left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We tested the PR and it seems to improve the behavior.
Log: https://logs.px4.io/plot_app?log=fd726ed3-219d-4e5d-8d4f-79557d4b4730
Although there was less wind it was clear that it handles much better at extreme roll angles (we normally set a 25 degree limit)

@MaEtUgR
Copy link
Member

MaEtUgR commented Sep 19, 2017

You were faster! 😄
Very similar changes I had on my list to propose. Here I posted the code before:
#4221 (comment)

I want to share with you the only reason why I did not make a pr for my version yet:

While the logic behind this calculation is pretty simple and increases performance drastically you also have to consider flying safety. As long as the vehicle is in a normal state and the actuators effectuate in the desired way it only helps. But as soon as there is something wrong this logic can make a bad situation like unstable flight a lot worse like uncontrolled full throttle shootout.
For most of the flights this might be no concern and even when the vehicle crashes anyways you might not bother. The situation I have in mind where you would like to have a "safe mode" is if you build for example a >20" prop size multicopter, you build everything, do your first flight test and something in the configuration is wrong (too high gains, wrong prop orientation whatever...), you realize it and go to low throttle but the controller goes unstable and the mixer puts out a high total thrust, until you disarm a lot can go wrong...

Maybe I'm overcautious but I wanted to find a solution to this problem before proposing a change like this. Now you were faster and I'm at least not responsible for the crashes 😉

@dagar I think a mixer unit test would make a lot of sense to try and cover all the rare extreme situations which are hard to reproduce in real flight but can still happen.

@bresch
Copy link
Member Author

bresch commented Sep 19, 2017

@MaEtUgR I'm fine contributing to natural selection :)
I'm joking, I'm fully aware that the boosting can be dangerous is something is wrong in the setup but I don't think that the mixer is the correct place to make any foolproof intelligence. In my mind, the mixer should just do its best to make the response of your quad as close as possible to the desired controller output. If your controller makes your system unstable, it's not the mixer job to do anything against that.
Furthermore, for the mixer point of view, a flip recovery looks the same as an unstable controller and it should not limit it.

@MaEtUgR
Copy link
Member

MaEtUgR commented Sep 19, 2017

@bresch I like your joke xD I totally agree and just didn't want to strike the ideology which I read out from the old implementation. At least not by default. As I say probably my thinking was too cautious.

On topic: We did some tests today with the new mixer on a 450 and a 210 racer and for LOS (line of sight) attitude controlled flight it reacts as expected a lot better when having low throttle. You can also hear the motor speeds of the racer reacting very fast to change attitude when not giving any throttle similar to KISS FC and co. Yaw tracking still has room to improve, I think you concentrated on roll pitch here right? I would have some logic to test also for yaw boosting.

The thing I found strange is I did a test flying the racer FPV (first person view) with live video and there you can do a lot more acro stuff because you can easily see the orientation also when being high up and further away. In these tests when I went off the throttle in acro mode the racer got nearly uncontrollable and sometimes even flipped. Don't get me wrong it feels better than before but not completely as I was expecting coming from other race controllers... I have to investigate the logs tomorrow to see if I can find out why exactly that happened. In principle it could be an idle motor speed problem or alike...

- fmu: in case of _mot_t_max==0 _airmode was not set in the mixer
- px4io: param_val is a float
@bresch
Copy link
Member Author

bresch commented Feb 16, 2018

A new parameter MC_AIRMODE defines whether the mixer is allowed to increase the total thrust command or not.
It works on:

  • FMU
  • IO
  • UAVCAN
  • Linux
  • Sim
  • Snapdragon

Thanks @bkueng !

Sorry, something went wrong.

@dagar
Copy link
Member

dagar commented Feb 25, 2018

Let me think about a way we might be able to achieve this without introducing vehicle type specific parameters in every single output module.

@bkueng
Copy link
Member

bkueng commented Feb 26, 2018

I added the remaining output modules already here: https://github.com/bkueng/Firmware/commits/pr-multi-mix-sat. @bresch can you pull the branch?

@dagar Yes, a nice solution would indeed be good here. I was thinking that the Linux, Snapdragon & Sim drivers can just be merged. The linux driver already supports multiple backends.

@bresch
Copy link
Member Author

bresch commented Feb 26, 2018

@bkueng Done

@nanthony21
Copy link
Contributor

I gave this PR a test today with a quadcopter and after two successful flights I had one flight where things went wrong resulting in a gentle crash. I'm not certain that the issue was caused by this PR but I wanted to post the log here in case it is helpful.

After taking off I went full throttle and full pitch forward in manual mode. After building up some speed I then let of the controls and switched to position mode. At first it looked like it was doing a good job of recovering, then it started to quickly drop. The motors were still running and the attitude was controlled, but despite putting the throttle stick all the way up the thrust wouldn't go high enough to maintain altitude.

Issues I see in the log:
1: Pitch oscillation after switching to position mode. I am using the default "Generic 250 Racer" pid settings and I have never seen oscillation before. I can see that the actuator outputs were saturated, could this be caused by the way that this PR handles saturation?

2: Shortly after switching to position mode the commander reported a loss of local position and switched to altitude mode. I have no idea why this happened. Although the altitude estimate seems fairly accurate the "Local position Z" plot seems to think that the vehicle was ascending all the way up until it crashed.

Log here:
https://review.px4.io/plot_app?log=53b9938b-ad16-453a-b6f5-e0d5637eba66

@nanthony21
Copy link
Contributor

After going through the new mixing strategy for roll and pitch the mixer then uses the old reduction-only approach for yaw. This means that yaw will still be totally uncontrolled for low throttle.

Would it make sense to use the new mixing strategy for yaw as well, possibly with a limit so that yaw can only make a small adjustment to thrust?

@bresch
Copy link
Member Author

bresch commented Mar 12, 2018

Hi @nanthony21 and thank you for testing this PR. By investigating the log, I can see that your crash is most likely due to divergence in the EKF solution. The throttle is in fact cut by the altitude controller since the estimator reports that the drone is climbing (you can see that in the first graph of review.px4.io that the estimated height, in blue, increases much more than the baro and GPS measured altitudes).

From the EKF repost, it looks that:

  1. the velocity innovations are growing
  2. then the velocity aiding is rejected because the normalized velocity innovation is larger than 1.0,
  3. position innovations are growing
  4. position aiding (GPS x,y + baro) in rejected because the normalized velocity innovation is larger than 1.0
  5. after rejecting the updates for a few seconds, altitude is reset to baro measurement (at 18:30) and x,y position is reset to GPS position. However, this is too late and the drone crashes at the same time.

vertical_innov

norm_innov_test

Here is the full EKF report:
mixer_throttle_cut.ulg.pdf

What I can't tell you is the cause of the initial velocity innovation growth. IMU vibrations? Bad GPS? Maybe @RomanBapst could give further explanations.

If not in air-mode the mixer is not able to apply positive boosting
and roll_pitch_scale is recomputed to apply symmetric - reduced - thrust.
This has the consequence to cut completely the outputs when the thrust is
set to zero.
@bresch
Copy link
Member Author

bresch commented Mar 12, 2018

@bkueng @MaEtUgR I just reintroduced the "safe tuning mode". When not in air-mode, during attitude tuning for example, if the controller starts to diverge, reducing the throttle will reduce the outputs and stop the oscillations. No possible flips on the ground! :)

@bkueng
Copy link
Member

bkueng commented Mar 16, 2018

This means that yaw will still be totally uncontrolled for low throttle.

This is correct, and yes we can extend this, for example by adding another value for the MC_AIRMODE param. But it will require the use of a separate (dis)arming switch.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet