Stability Patch for Motor Max and Min and Yaw Curve

Hi all, it appears that my blog post “Yaw Control, ESC Linearization, and Hover” has been lost. I hope that it will be saveable but I am not holding my breath. If it can’t be saved I will attempt to reconstruct it but while I have most people’s replies on my emails but I don’t have my own. If anybody has a copy of my replies I would be very grateful if you could send them to me.

The discussion that followed that blog post focused on the "stability patch" in AP_MotorsMatrix.cpp.

Both myself and R_Lefebvre attempted to explain this in detail in the blog post above so I won’t do it again now in a hope that we will get that work back (fingers crossed). But I will attempt to summarise the issues that we would like to address.

  1. Near maximum and minimum throttle the control is compromised because we can’t decrease propeller lift much further at minimum throttle or increase it much further maximum throttle.
  2. When multiple Roll, Pitch, and Yaw manoeuvres are happening, how do we combine these with the requested Throttle setting to maintain stability.
  3. Yaw control is very limited in most of the multi rotor geometries and therefore results in large RPM variation in motors compared to Roll and Pitch to create an equivalent Yaw response. The lift force generated tends to be non-linear creating an increase in lifting force when Yaw is applied. How can we correct this behaviour?

While addressing these points we need to ensure that:

  1. We maintain stability and preferably good control over Roll, Pitch, and Yaw.
  2. Throttle response and altitude control is not compromised any more than absolutely necessary.
  3. That we don’t do anything that can prevent an overpowered multi rotor from coming down!

 

Yaw Curve

Ok, easy bit first.

Yaw is achieved by increasing the RPM of half the motors and decreasing it in the other half of the motors. However, because the lift of a given propeller tends to increase by the square of the change in RPM, this results in an increased total lifting force.

Reference is page 16 here: http://dc-rc.org/pdf/Model%20Propellers%20Article.pdf

So what we want to do is increase the RPM of half the motors less than we decrease the RPM of the other half and keep it the same as when all motors are at the same RPM.

Ok, some maths.

x = RPM of all motors at stationary hover.

y = RPM reduction of half the motors during Yaw.

z = RPM increase of half the motors during Yaw.

Total lifting force at hover for a quad is:

Total Force = 4*x^2

When inducing a Yaw it is:

Total Force = 2*(x - y)^2 + 2*(x + z)^2

We want these two equations to be equal if we don’t want the quad to start accelerating vertically therefore:

4*x^2 = 2*(x - y)^2 + 2*(x + z)^2

2*x^2 = (x - y)^2 + (x + z)^2

2*x^2 = x^2 + y^2 -2*x*y + x^2 + z^2 + 2*x*z

2*x^2 = 2*x^2 + y^2 -2*x*y + z^2 + 2*x*z

0 = y^2 -2*x*y + z^2 + 2*x*z

0 = z^2 + 2*x*z + y^2 -2*x*y

if we solve this for z we get:

z = -x + sqrt(x^2 – y^2 + 2*x*y)

If we plot this we get:

 The excel file I used is here if you would like to play with it.

YawScaling.xls

The current ratio line is the ratio we are currently considering using, ie 0.7 up and 1.42 down. As you can see, the ratio starts off 1:1 then reduces to sqrt(2)-1 or 0.414. We could get pretty close to the ideal line using a straight line approximation:

z = 1 - (2-SQRT(2)) * x/y

This will give us a small increase in lift half way along but it is pretty close. Looking at the added complexity with this approach I think it is probably better to go with the constant ratio approach that is currently commented out in the code. This makes all calculations much quicker and easier.

Stability Patch

Ok, now the difficult bit.

The approach I suggested in the lost blog was

  1. calculate all motor settings for Roll, Pitch and Throttle.
  2. calculate the remaining headroom between these throttle settings and the max_throttle and min_throttle.
  3. if the headroom is not enough to contain the requested Yaw input the we reduce the requested Yaw to fit the available headroom or the minimum reserved yaw headroom.
  4. We then go through and add the Yaw request to each motor output with the throttle offset to ensure the outputs remain between the max_throttle setting and the min_throttle setting.

We do need to be very careful that Pitch or Yaw oscillations can’t make a high powered quad fly away though. I have some ideas on this but it may not be needed.

 

In the lost post the question was raised by Randy. “For example, I think it's much better to abandon a request from an autopilot for a 45 degree pitch forward in favour of maintaining yaw.” We could do this by:

  1.  reducing the stick input travel at low or high throttle,
  2. reducing the limit on roll and pitch rate outputs at low or high throttle,
  3. or scaling the rate outputs in the MotorMatix function.

I don’t like any of these options because they either directly interfere with the users inputs or change the ability of the Rate_I (or Stab_I) term to do it job and correct for CG and motor output offsets.

The good news is that I don’t think this is a problem if we leave the Roll and Pitch components un touched. At near minimum throttle we are descending and we can get a large Roll or Pitch output from the Rate PID’s from two things:

  1. a large stick input from the user,
  2. a large exterior force.

Point 1. If the user enters a command to change attitude from horizontal to 45 degree pitch for example. With full Pitch control this will happen in less than a second (for my quad anyway). For this short time we may lose almost all our Yaw authority leaving just a little more than is required to maintain a heading. However, as the Pitch angle increases the throttle boost kicks in. This will dramatically increase the throttle setting restoring Yaw authority.

On the other hand, If the airframe is at 45 degrees and I let go of the sticks in stabilise mode. I want to see my copter go level as fast as possible. I don’t care if I lose all Yaw authority for a second provided my copter is level afterwards. Then with the small amount of Yaw authority that we keep in reserve the copter can change the heading as requested.

Point 2. If the copter experiences a large outside force… say clips a branch while descending to land, all I care about is that it gets up the right way again. As before, Throttle boost will help us here for a portion of the rotation, but Yaw is my absolute lowest priority.

Remember I am not advocating removing all Yaw authority, I am suggesting reserving only enough to maintain a heading and apply small Yaw accelerations.

Code Example

To quote R_Lefebvre from the lost blog post, “this isn't finished working code, just kind of a back-of-the-envelope sketch”

 

    // capture desired roll, pitch, yaw and throttle from receiver

    _rc_roll->calc_pwm();

    _rc_pitch->calc_pwm();

    _rc_throttle->calc_pwm();

    _rc_yaw->calc_pwm();

 

    // mix roll, pitch, throttle into output for each motor

    for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {

        if( motor_enabled[i] ) {

            motor_out[i] = _rc_throttle->radio_out +

                           _rc_roll->pwm_out * _roll_factor[i] +

                           _rc_pitch->pwm_out * _pitch_factor[i];

        }

    }

 

    // calulate the minium headroom left for Yaw.

    high_yaw_pos = 1000;

    high_yaw_neg = 1000;

    low_yaw_pos = 1000;

    low_yaw_neg = 1000;

    high_throttle = 0;

    low_throttle = 2000;

 

    // the signs I have used might be wrong

    for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {

        if( motor_enabled[i]) {

             if( _yaw_factor[i] > 0 ) {

            if( high_yaw_pos > (out_max - motor_out[i])/ ) {

                    high_yaw_pos = out_max - motor_out[i];

                }

            if( low_yaw_neg > motor_out[i] - out_min ) {

                    low_yaw_neg = motor_out[i] - out_min;

                }

                    } else if( _yaw_factor[i] < 0 ) {

            if( high_yaw_neg > out_max - motor_out[i] ) {

                    high_yaw_neg = out_max - motor_out[i];

            }

            if( low_yaw_pos > motor_out[i] - out_min ) {

                    low_yaw_pos = motor_out[i] - out_min;

                }

             }

            if( high_throttle < motor_out[i] ) {

            high_throttle = motor_out[i];

            }

            if( low_throttle > motor_out[i] ) {

            low_throttle = motor_out[i];

            }

       }

       }

 

    yaw_ratio = 0.7/1.42;

 

    high_yaw_pos = max(high_yaw_pos, min_yaw_headroom);

    high_yaw_neg = max(high_yaw_neg, min_yaw_headroom);

    low_yaw_pos = max(low_yaw_pos, min_yaw_headroom);

    low_yaw_neg = max(low_yaw_neg, min_yaw_headroom);

 

    yaw_pos = min(high_yaw_pos/yaw_ratio, low_yaw_pos);

    yaw_neg = min(high_yaw_neg/yaw_ratio, low_yaw_neg);

 

       rc_yaw_pwm_out = constrain(_rc_yaw->pwm_out, -yaw_neg, yaw_pos);

      

 

       min_required_yaw_headroom = min(reserved_yaw_headroom, rc_yaw->pwm_out);

      

       if( (out_max-high_throttle)/yaw_ratio < (low_throttle - out_min) ) {

             throttle_change = min((out_max-high_throttle) - min_yaw_headroom*yaw_ratio,0);

       }else{

             throttle_change = max((out_min-low_throttle) + min_yaw_headroom,0);

       }

 

    for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {

        if( motor_enabled[i] ) {

             yaw_contribution = rc_yaw_pwm_out*_yaw_factor[i];

                    if (yaw_contribution > 0 ){

                           yaw_contribution *= yaw_ratio;

                    }

            motor_out[i] = motor_out[i] +

                                 throttle_change +

                                 yaw_contribution;

        }

    }

 

    // ensure motors are not outside alowable region

    for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {

        if( motor_enabled[i] ) {

            motor_out[i] = constrain(motor_out[i], out_min, out_max);

        }

    }

 

Views: 1219

Comment by Marooned on September 17, 2012 at 1:18pm

I've sent your 5 messages I had in my inbox via PM here.

I was also surprised to find that blog post gone. One of the most interesting lately.

Comment by Rob_Lefebvre on September 17, 2012 at 1:36pm

Did you run through the calculation for the yaw equation, and find that my WAG of 0.7/1.42 was pretty close?

I had actually planned to make the 0.7 a parameter that the user could tune.  It only needs one parameter because the upside should be 1/0.7=1.42, or whatever it works out for on it's parameter.

This yaw differential was deprecated because Jason found that on his high powered quad, the slowing motors slowed too much, and hit throttle-min, leaving him with no attitude control.  I hadn't experienced this because my moderately powered quad hovers at 50%, so I have lots of room to go down.  But simply making it a parameter would allow users to tune it to their liking.  If they want to prevent rise on yaw, then use 0.7.  If they need to have no differential, just set it to 1.  

This is getting into "parameter creep" but, I think if it was an advanced param, and set to something like 0.9 as a default, it should work out OK.

If we get to the full attitude control hierarchy going, then having this a parameter won't matter so much.  The problem is, determining which control gets priority is a really complicated problem!  It's also possible that we don't have enough processor overhead left to do a bunch more complicated stuff.  It may have to wait for ARM.

I do think we should try to at least get "negative stability patch" going.  The stability patch makes sense, but but it only works on the top of the PWM range.  Many quads these days are operating closer to the bottom of the range.


Developer
Comment by leonardthall on September 17, 2012 at 3:39pm

Lefebvre,

Yeh, the code I wrote above was written for communication, not for speed but it is still all in the 200 Hz loop.

I did check your numbers and compare them in the graph above. I make the RPM reduction equal to _rc_yaw->pwm_out and the up going RPM equal to _rc_yaw->pwm_out * 0.7/1.42. That way the down moving throttle that is causing the problem doesn't change in peek magnitude.

Marooned,

Thanks for that mate, If they can't get the Blog post back I will attempt to reconstruct it with the comments that everybody did. I will give them a couple of days though because I haven't had any message yet.

Comment by Rob_Lefebvre on September 19, 2012 at 4:53am

Leonard, check your gmail please.

Comment

You need to be a member of DIY Drones to add comments!

Join DIY Drones

Groups

Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service