ACRO for ArduPilot 3.1

Bob Doiron July 17, 2013 at 2:17pm

Just tested out the above code - huge improvement for stiff leveling. Now I'm playing with adding a DTerm to the acro correction:

set_roll_rate_target(g.pi_stabilize_roll.get_p(roll_axis) + acro_roll_Dterm.get_d(roll_axis, G_Dt) + acro_rate, BODY_FRAME);

(acro_roll_Dterm is just the regular PID Dterm used by the rate controllers, I'm using 6.5 for P and 0.47 for D)

My thinking is that this will give a boost when it's falling behind and back off it's catching up. If this works well, I think it might be worth restructuring the stabilize mode to work in a similar fashion. That is, maintain an axis target that plots out the path to the desired angle. The current structure couldn't really use a Dterm since it only knows the end point. With a plotted path and a Dterm, stabilize mode could use more P gain and follow rapid stick movements better.

Any thoughts?

Bob Doiron July 17, 2013 at 5:11am

Hey Leonard - I found my leveling oscillation source. I was calculating the level rate based on the ahrs sensor value, and not where the copter was supposed to be once it catches up to the target.

As an example, say the sensor said the copter was at 10deg but the target integrator was already wound up to -10deg (which would be relative to the current position of 10deg). My original code would try and subtract more off the target and overshoot, when instead it should just let the copter catch up to the absolute target of 10deg + (-10deg) = 0deg. Here it is in code:

int32_t level_rate = g.pi_stabilize_roll.get_p(constrain(-(ahrs.roll_sensor+roll_axis), -4500, 4500)) * lf;

"roll_axis" is my relative target integrator, which always represents degrees, and that lf factor on the end is just the blending/balance. I'm not sure if this applies to your code, just thought I'd share and let you think through your implementation.

Bob Doiron July 14, 2013 at 6:57am

Take your time Leonard. I think I know why I was getting oscillations - the normal stabilization requests rates, whereas the levelling in acro requests angular positions through the angle target. Since there's no 'slip', the acro levelling behaves as if it has more gain.

12-HAK July 14, 2013 at 4:45am

No idea - Mine is at 0.19 and doing pretty well :)

Developer

leonardthall July 14, 2013 at 3:16am

Bob, I still need to look more at your code. Sorry for the delay.

Developer

leonardthall July 14, 2013 at 3:11am

Thanks mate,

Yeh, Low I terms will mess you up when slowing down because there is a big toque on the copter when traveling fast (called motor flapping). As the I term is made smaller it takes longer for the I term to reduce so it is fighting the controller.

Glad you are enjoying the ACRO!

Bob Doiron July 13, 2013 at 6:23pm

Hey Leonard - the orange lines in the plots are where the rate is near zero, so you can see in the second plot that you get a constant line where the leveling is cancelling out the acro rate - meaning you can hold any arbitrary angle with that stick deflection. The particular deflection where that occurs will depend on your StabKp / BAL / AcroKp factors. Basically inside that deflection the copter auto levels to a particular angle (see the orange diagonal line up there) and outside it rolls at a constant rate. I flew it tonight and I was very impressed - it lets you do big roll arcs at the rate you decide, even with stiff leveling. Before this mod, I could only do half an arc and then it would essentially snap back to level on the underside at full rate.

One thing I noticed is that I couldn't run my full 6.5 StabKp without oscillations, so I had to back it off (did that using 75% balance factor). I don't know if bringing the stability integrator in would clear that up (might just make it worse?) - what might help though is a D term, so I might try that next.

As for the bleeding, I don't know it it's necessary. I added it early on because I had a little takeoff mishap. The 300/301 factor I'm using takes 2 seconds to bleed half the error, so it takes quite a bit of time to fully settle out (10 seconds to get down to 3%). You are right that it will contribute to an small error in angle holding - I just don't notice it because I tend to rely on the leveling to hold an angle.

I've also seen big Yaw jumps when switching out of acro until I added this line of code to set_mode() to reset the nav_yaw target on every mode switch. Further down I reset the angle integrators for body frame mode (but I think you already do that):

nav_yaw = ahrs.yaw_sensor; // always set yaw target to current heading on mode change

switch(control_mode)
{
case ACRO:
ap.manual_throttle = true;
ap.manual_attitude = true;
set_yaw_mode(YAW_ACRO);
set_roll_pitch_mode(ROLL_PITCH_ACRO);
set_throttle_mode(THROTTLE_MANUAL);

if(!g.axis_enabled)
{
// initialize for body frame stabilization (these are used as angle error integrators)
roll_axis = 0;
pitch_axis = 0;
yaw_axis = 0;
}
else
{
// reset acro axis targets to current attitude (yaw set above)
roll_axis = ahrs.roll_sensor;
pitch_axis = ahrs.pitch_sensor;
yaw_axis = ahrs.yaw_sensor;
}
break;

Here's the code I flew tonight (still 2.9.1b):

#define _BLEED 300
#define BLEED_CONST (_BLEED/(_BLEED + 1.0))

// use max stick deflection of roll, pitch or yaw to determine level mix
float CalcLevelMix()
{
float r = ((float)labs(g.rc_1.control_in)) / MAX_INPUT_ROLL_ANGLE;
float p = ((float)labs(g.rc_2.control_in)) / MAX_INPUT_PITCH_ANGLE;
float y = ((float)labs(g.rc_4.control_in)) / MAX_INPUT_YAW_ANGLE;

p = max(y,max(r,p));

return 1.0 - p;
}

int32_t CalcLimit(int32_t acro_rate, int32_t max_level_rate)
{
return abs(abs(acro_rate) - max_level_rate);
}

// Roll with rate input and stabilized to body frame
static void
get_roll_rate_stabilized_bf(int32_t roll_stick_angle)
{
// Convert the input to the desired roll rate
int32_t acro_rate = roll_stick_angle * g.acro_p;

// Calculate rate limiter to avoid discontinuity when crossing 180
float lf = CalcLevelMix() * (g.acro_balance_roll/100.0) * cos_pitch_x;
int32_t limit = CalcLimit(acro_rate, lf*g.pi_stabilize_roll.get_p(4500));

// Calculate level rate for blending (similar to MultiWiii horizon mode), ignore roll level as we approach pitch +-90
int32_t level_rate = g.pi_stabilize_roll.get_p(constrain(-ahrs.roll_sensor, -4500, 4500)) * lf;

// combine/limit rate
acro_rate = constrain((acro_rate + level_rate), -limit, limit);

// Update axis movement
roll_axis -= ((omega.x * DEGX100) * ins.get_delta_time());

if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
roll_axis = 0;
acro_rate = 0;
}

// Set body frame target for rate controller (correction rate + acro rate)
set_roll_rate_target(g.pi_stabilize_roll.get_p(roll_axis) + acro_rate, BODY_FRAME);

// Calculate target for next iteration
roll_axis += (acro_rate * G_Dt);
// Bleed axis error
roll_axis = roll_axis * BLEED_CONST;
// Error with maximum of +- max_angle_overshoot
roll_axis = constrain(roll_axis, -MAX_BF_ROLL_OVERSHOOT, MAX_BF_ROLL_OVERSHOOT);
}

// Pitch with rate input and stabilized to body frame
static void
get_pitch_rate_stabilized_bf(int32_t pitch_stick_angle)
{
// Convert the input to the desired pitch rate
int32_t acro_rate = pitch_stick_angle * g.acro_p;

// Calculate rate limiter to avoid discontinuity when crossing 180
float lf = CalcLevelMix() * (g.acro_balance_pitch/100.0) * cos_roll_x;
int32_t limit = CalcLimit(acro_rate, lf*g.pi_stabilize_pitch.get_p(4500));

// Calculate level rate for blending (similar to MultiWiii horizon mode)
int32_t level_rate = g.pi_stabilize_pitch.get_p(constrain(-ahrs.pitch_sensor, -4500, 4500)) * lf;

// combine/limit rate
acro_rate = constrain((acro_rate + level_rate), -limit, limit);

// Update axis movement
pitch_axis -= ((omega.y * DEGX100) * ins.get_delta_time());

if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
pitch_axis = 0;
acro_rate = 0;
}

// Set body frame target for rate controller (correction rate + acro rate)
set_pitch_rate_target(g.pi_stabilize_pitch.get_p(pitch_axis) + acro_rate, BODY_FRAME);

// Calculate target for next iteration
pitch_axis += (acro_rate * G_Dt);
// Bleed axis error
pitch_axis = pitch_axis * BLEED_CONST;
// Error with maximum of +- max_angle_overshoot
pitch_axis= constrain(pitch_axis, -MAX_BF_PITCH_OVERSHOOT, MAX_BF_PITCH_OVERSHOOT);
}

// Yaw with rate input and stabilized to body frame
static void
get_yaw_rate_stabilized_bf(int32_t yaw_stick_angle, int32_t pitch_stick_angle)
{
// Convert the input to the desired yaw rate
int32_t acro_rate = yaw_stick_angle * g.acro_p;

// Calculate rate limiter to avoid discontinuity when crossing 180
float lf = CalcLevelMix() * (g.acro_balance_pitch/100.0) * (-sin_roll);
int32_t limit = CalcLimit(acro_rate, lf*g.pi_stabilize_pitch.get_p(4500));

// Calculate level rate for blending (similar to MultiWiii horizon mode)
int32_t level_rate = g.pi_stabilize_yaw.get_p(constrain(-ahrs.pitch_sensor, -4500, 4500)) * lf;

// combine/limit rate
acro_rate = constrain((acro_rate + level_rate), -limit, limit);

// Update axis movement
yaw_axis -= ((omega.z * DEGX100) * ins.get_delta_time());

if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
yaw_axis = 0;
acro_rate = 0;
}

// Set body frame target for rate controller (correction rate + acro rate)
set_yaw_rate_target(g.pi_stabilize_yaw.get_p(yaw_axis) + acro_rate, BODY_FRAME);

// Calculate target for next iteration
yaw_axis += (acro_rate * G_Dt);
// Bleed axis error
yaw_axis = yaw_axis * BLEED_CONST;
// Error with maximum of +- max_angle_overshoot
yaw_axis = constrain(yaw_axis, -MAX_BF_YAW_OVERSHOOT, MAX_BF_YAW_OVERSHOOT);
}
//////////////////////////////////////////////////////////////////////////////

Developer

leonardthall July 13, 2013 at 5:45pm

Hi Bob,

Not a bad solution. I didn't quiet get what you meant by "An interesting side effect is that at stick = 45%, the rate is zero." though. I need to think about this a little more. I also haven't put your leaky I term in with a time constant of around 2 seconds so that it settles down when landing on an uneven surface. I am very hesitant to do this because it will also let the copter's angle drift and we already reset the error each time we move the throttle to zero. Is this really necessary do you think? (and why)

Hi Joly,

I don't think this does what you think it does. There is no recording of angle, just angle error. So the worst that can happen is that you are performing a very dynamic maneuver that has resulted in your angle error increasing to 10 degrees. If you change to stabilize at this time you could notice a twitch when you switch back to acro. I may have missed something though.

Bob Doiron July 13, 2013 at 3:46pm

Hey Leonard, I've got an idea to improve the acro flying when leveling is enabled. One thing that has been bugging me is that when I try and do a slower than full rate roll, the first half of the roll is fine, but then the rate jumps up when doing the inverted side of the roll.

What's happening is that during the first half of the roll, the leveling holding back the acro rate but for the flipside it adds to it. For example, with StabKp=6.5 (292.5deg/s @ > 45 degrees from level), AcroKp = 8 (360deg/s at 100%), Balance at 100% and the stick at 50%, the first half rolls at (360-292.5)*0.5 = 33.75 deg/s and the inverted half increases to (360+292.5)*0.5 = 326.5 deg/s.

Here's a surface plot of that effect:

horizontal is roll degrees (showing 2 complete revolutions), vertical is stick deflection, color is roll rate

What you see here is that the only place the rate stays constant throughout the loop is at stick = 100%.

After much pondering, I decided to place a limit on the rate generated from adding acro and level together, so that when you pass the 180 degree mark, the rate doesn't increase. The formula is limit=abs(abs(acro)-maxlevel), where maxlevel = level rate assuming you are 45 degrees from level.

Here's a plot of what that looks like:

So at stick deflections > 50%, the rate is constant all the way through the loop. An interesting side effect is that at stick = 45%, the rate is zero.

What do you think? I'm going out tonight to try this and I'll report back how it feels.

12-HAK July 13, 2013 at 4:16am

I meant a bit jumpy. I asked my daughter to try and do some closeups but I was buzzing around pretty fast.

Decided to build a small testing quad so this is a 330 size quad with 1040kv motors and 8 inch props.

Its very nimble. Need to do some more PID tuning to see if I can get it more locked in.

It seems to respond better to quick power out of the flips. If I bring the power up to slow it I find it washes out the flips but if I burst the power at the end it locks in - same for fast turns.

Blogs

ACRO for ArduPilot 3.1

Comments