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

        }

    }