[SOLVED] PID voltage scaling and MOT_YAW_HEADROOM questions

For battery voltage compensation, MOT_BAT_VOLT_MAX and MOT_BAT_VOLT_MIN, is there a way to see in the log what the scaling value is at any given time? Does it apply to the RCOUT value, or adjust PID Gains?

Additionally, what exactly does MOT_YAW_HEADROOM control, and how does it change the craft’s behavior?

Partial answer, MOTB message in DataFlash logs seems to have scaling in it, I assume this is related to the voltage compensation and will investigate

Does it apply to the RCOUT value, or adjust PID Gains?

It gets applied after the PID gains, but before creating servo outputs, in the frame-dependent code (e.g. quadcopter, tricopter, coax - these are all “frames”). So you will not see the scaling difference in PID logs, but will see them in RCOU entries - but the actual logic is a little bit more complicated.

what exactly does MOT_YAW_HEADROOM control

This again depends on the frame, but the general logic is as follows. If the desired attitude rate change is too big, the three axes (roll, pitch, yaw) and throttle get in conflict (e.g. you cannot satisfy all of them perfectly). The craft then needs to resolve these conflicts in some way that is safe and results in better flying if possible.

For throttle, there is logic controlled by throttle mixer, and generally it is as follows: you only decrease throttle if you absolutely have to, and you can increase throttle to get better control in other axes, but only by so much (ATC_THR_MIX_* parameters control that precisely).

Out of the axes, yaw is the first axis to go, because poor yaw handling would not result in an accelerated flyaway. But it’s too bad if you degrade yaw completely. For this reason, there is a minimum guaranteed degree of freedom that is reserved for yaw, and this is precisely what is controlled by MOT_YAW_HEADROOM. The general logic for frames with propeller-driven yaw is to avoid shrinking the quantity, that gets added to (or subtracted from) the propeller outputs because of yaw, below MOT_YAW_HEADROOM in the PWM space. For other frames, the logic is similar, but it might be more convoluted.

Too much of MOT_YAW_HEADROOM should not influence normal flying, but may sacrifice performance in pitch and roll in even a mild emergency. This is closely connected to tuning issues with yaw: if the tuning is too tight, it quickly saturates the yaw headroom, so the machine may start underperforming in pitch or roll for no apparent reason (until you look in the RCOU logs and see how noisy the outputs are). Check this reply (ATC_RAT_YAW_FLTE for small powerful quads: Zero or non-zero? - #2 by Leonardthall) for more details on the underlying physics.

Too little of MOT_YAW_HEADROOM will sacrifice yaw in not-so-emergency conditions. The default value is nice enough for larger copters, but probably a bit too large for smaller ones.

3 Likes

Thank you!!!

@MaxBuzz fantastic answer Maxim! I have nothing to add :slight_smile:

@MaxBuzz @Leonardthall

Can either one of you describe a little more about what behavior to expect as ATC_THR_MIX_ is changed? Does thrust mean “all force opposing gravity” or “all force directed down in the body frame?”

For instance, if this is set low, and we are in loiter and station-keeping, and there is a gust that would normally require an attitude correction, would we drift off target but maintain altitude?

I have seen instances where, when there are too many constraints the system will keep attitude and allow yaw and lose altitude until the wind load is reduced and/or yaw is no longer demanded, and I was wondering how the ATC_THR_MIX would change the behavior here

Maybe Leonard will correct me this time. This is what I see in the code, and what I use in my development version of the coax frame. I’m talking about multicopters only, as I have no experience with planes as of now.


There is throttle commanded (input), which is either something the autopilot decides in throttle-managed modes, or your input, divided by the cosine of the angle between the frame-down direction and the gravity for attitude compensation, in Stabilize, or your input plainly in Acro.

There is also hover throttle (hover) which is initially read from MOT_THST_HOVER and then possibly adjusted to the actual flight conditions. From these two, and yet another parameter which I would call mix here, we derive the maximum average throttle that we must never exceed when figuring out the servo outputs. The exact expression is:

max(input, input * max(0, 1 - mix) + hover * mix)

and the function where this happens is AC_AttitudeControl_Multi::get_throttle_avg_max. This controls thrust in the body frame, not in the earth frame, that is, force directed down from the aircraft not accounting for cosine losses. Any cosine loss accounting comes before that.

The easier way to think about this is when mix is within the interval [0;1], which matches the default parameters. In this case, when input is at least as large as hover, this will be the final result, and your throttle will be the exactly commanded throttle (unless it must go down for sanity). If input is less than hover, however, the expression above becomes bigger than input, but never bigger than hover. This is the slack in throttle that the autopilot may use to keep angles better. The bigger the mix, the higher that maximum allowed throttle. Obviously if you have some horizontal speed, you want better angle handling possibly at the cost of descent speed, so mix should be bigger, and if you want to land vertically, mix should be smaller: this is where ATC_THR_MIX_MIN and ATC_THR_MIX_MAX parameters come into play. The actual mix will be between them based on what the autopilot thinks. In modes like Acro or Stabilize, you typically want roughly same angle handling all the time, so they use a third value for mix, from ATC_THR_MIX_MAN for “manual” modes. By default, this also switches to the minimum value at really low throttle to prevent bouncing on the ground, unless something called “air mode” is turned on.

(If you wish, you may think of this as follows: if ATC_THR_MIX_MIN is zero, your machine will descend as quickly as commanded, but has the chances to lose attitude if throttle is too low to keep angles. If it is higher, the machine may decide to descend slower, but will lose attitude control with lower probability / under higher pressure, and the higher the value, the bigger the effect. In the extreme case of this being say 0.9, vertical descent in windy conditions may get slowed down several times)

Fun things start to happen when mix is greater than 1. In this case - which should generally only be used in well-tuned agile copters, where “well-tuned” is important - maximum output throttle can go well beyond the hover throttle, if angle handling requires so. Which is an absolutely valid case if your machine typically flies well above the hover throttle, that is, very fast, think of racing quads. High ATC_THR_MIX_MAN setting (it’s up to 4 currently despite the recommended range in parameter documentation is 0.1 to 0.9) helps immensely in things like flips and rolls, where you may drop throttle to zero and will still able to use most of the available power if required.

That said, this does allow a vertical flyaway. Sorry for the shameless plug of my own channel, but this (https://www.youtube.com/watch?v=9LjDJAumSJM) towards the end contains good examples of what happens when a high mix value meets an imperfectly tuned machine.


Regarding your question: I think that it is somewhat unlikely that this parameter will influence the behavior in your cases, because your cases are probably closer to an overloaded craft, and this parameter has the most influence on lightweight machines. As happens very often, logs of the events are needed to decide.

2 Likes

I can’t thank you enough

Hi Josh, Max did a good job with his answer here too but I may be able to offer a simple explanation.

The most force is availible to controll roll, pitch and yaw when throttle is at 50%. That measn half the motors can be at maximum and half at minimum to control one of the rotational axis.

Because we proritise yaw over thrust an over loaded quad that is hovering at 60% will loose some altitude when we use 100% yaw output. Without ATC_THR_MIX parameters a quad that is occilating or using 100% yaw for example, a quad that hovers at 25% throttle would climb. Both of these are caused by the fact that if roll, pitch or yaw goes to 100% output then by definition the thrust is 50%.

We use the ATC_THR_MIX parameters to define how and when we prioritise thrust over roll, pitch and yaw. And how much.

ATC_THR_MIX_MIN is for landing
ATC_THR_MIX_MAX is for normal flight
ATC_THR_MIX_MAN is for manual flight

These are the main cases to consider:

  1. Landing - we want throttle to be able to be reduced during landing to a point the aircraft can settle flat on a surface that isn’t level.
  2. Occilation- An aircraft that is over tuned and is occilating we want to have enough control athortiy to reduce the average thrust below hover throttle. Before we did this an occilation on a power quad could cause an uncontrolled climb (and it did).
  3. Breaking - During breaking the thrust is reduced, sometimes to zero. We want to have a compromise between attitude control and height control.

We found that we needed a seperate landing setting to dynamic flight setting because some aircraft would flip during a breaking manuver using ATC_THR_MIX_MIN. So we only go to ATC_THR_MIX_MIN when the aircraft may be landing. We basicly check that the aircraft is decending and not making any aggressive manuvers.

3 Likes