I built the AC SIm to do gain tuning and now AB testing of various control algorithms. I've made some significant strides in the control loops, so I though I would detail out how it was done.

There is very little code left over from AC 1 in the current code base. When I started AC 2, I used the v1 control loops and assumed they were good. (Everyone was very happy at the time with them.) I also had no real way to measure the performance of them.

A few months ago I started to get very suspicious that the old carryover code was causing a lot of the flight headaches I've been experiencing such as aggressive flight leading to a flip. Or slow, wobbly response in descents. Max was complaining that AC2 lacked "wings". That's not a lot to go on, but it is real feedback that something needed done.

I built the SIM specifically to tackle this type of problem and here is how it was used:

In the SIM a parameter was created called g.test. A checkbox toggles the value of this param and the boolean value is used to turn on or off chunks of code. This gives me a side-by-side comparison of the impact.

Suspect code:

Attitude.pde: get_stabilize_roll() and get_stabilize_pitch();


// limit the error we're feeding to the PID

target_angle            = constrain(target_angle, -2500, 2500);

This line limits the desired rates sent to the rate loop.  A before and after plot shows the performance gain when this line of code is disabled. The Initial state of the copter was roll left at 450° per second. The plot shows the roll angle going to -85°, then recover to 0. Removing that line improve the recovery time from 1.15s to .75s

Suspect code 2:

This is the quad suddenly rotating at 600°/s to the right.

I've increased the roll output limit  in rate_roll and you can see the recovery time to level has decreased  35% to .8 seconds to level from 1.24 seconds. The angle error significantly decreased as well.

 

Patch 3:

There is an iterm in the stabilization code that causes the copter to level out when the CG is off or a motor is out of balance. The problem is the iterm can drift around when flying because drag or a low CG can prevent a copter from holding an steep angle and cause us to be 1-2° off when going back to level. We only want this term to hold level, so I placed a limit on the iterm like this:

if(abs(ahrs.roll_sensor) < 500){

    angle_error  = constrain(angle_error, -500, 500);

    i_stab  = g.pi_stabilize_roll.get_i(angle_error, G_Dt);

}else{

    i_stab  = g.pi_stabilize_roll.get_integrator();

}

Here is the effect plotted on a copter that has a 3° lean to the right. The iterm ramps to 3°, but it moves a lot when flying around.

Here is the patched code:

Notice the iterm ramps up and doesn't really budge. That should really help in keeping the copter still in the air.

 

Patch 4:

Keeping the copter holding altitude while pitching hard was work OK, but not great. The copter tends to drift down. I got that piece of code from a description in Berkeley's STARMAC copter project. 

static int16_t get_angle_boost(int16_t value)

{

    float temp = cos_pitch_x * cos_roll_x;

    temp = 1.0 - constrain(temp, .5, 1.0);

    int16_t output = temp * value;

    return constrain(output, 0, 200);

}

I changed it to this:

static int16_t get_angle_boost()

{

    float temp = cos_pitch_x * cos_roll_x;

    temp = constrain(temp, .5, 1.0);

    return ((float)g.throttle_cruise / temp) - g.throttle_cruise;

}

This has the effect of maintaining perfect downward acceleration at all times. The reason this works is the throttle-cruise (say 500 out of 1000) is = to 9.81m/s/s of downward acceleration. We can use a linear mapping of throttle to acceleration to achieve the additional throttle necessary for the combined roll and pitch tilt.

Bonus 

The last example not is not a patch but a tuning example:

For a basic setup we've been using Rate_P of .14 and Rate_D of 0. Also a Rate dampener similar to the Wii project of .15 Many folks were OK with these gains, but the performance was meh in Max and Marco's experience.

Here is a copter at 45° returning to 0° with the current gains and no patches:

With Patches:

Notice the overshoot and the bell ringing. The frequency with the above with patches is lower and that's good.

Here is the comparison of the above, but with the optimum gains:

These gains are now the default gains:

rate_P of .18

rate_D of .004

stab_D of 0

Please try these gains in the simulator and raise and lower the rate_D term. I was pretty surprised to see how tied the Rate_P and Rate_D are to each other. They cannot be tuned independently. A rate_P of .18 and Rate_D of 0 will not fly well at all. It will fast wobble in the air quite a bit.

I hope this was interesting. The SIM is currently residing at www.jasonshort.com

Jason