The way we do stabilize/althold is bad

[Jonathan Challinger]

I've known this one for a long, long time, but I think it's probably time to gripe about it to the list. These problems have been around from the very beginning and I want to raise awareness about them and at least talk about fixing them. They affect the stabilize and alt hold modes.

Section 1: problem description

Given the default 45 degree tilt limit, the stick in the upper-right corner, and the copter facing north (no wind), you would expect the copter to travel north-east (45 degrees), tilted to the limit (45 degrees). It does not do EITHER of those things. Instead, it tilts well past the limit to 60 degrees. At 60 degrees of tilt, it is accelerating ~73% faster than the 1g that a 45 degree tilt would imply. So, it is accelerating almost twice as hard as it is supposed to. Further, it is NOT traveling in the operator's expected direction of north-east (45 degrees)!! Instead, it is traveling east-north-east (54.74 degrees).

Section 2: Math that shows the problems

Take a look at AC_PosControl.pde in the accel_to_lean_angles function. At the bottom of this function are a couple very well-reviewed (correct) lines that convert accel_forward and accel_right to roll and pitch.

If we simplify these lines and remove all the unit conversions etc, they look like this:

    roll = atan(accel_right*cos(pitch)/9.81)

    pitch = atan(-accel_forward/9.81)

Solve for accelerations and you get this:

    accel_right = tan(roll)*sec(pitch)*9.81

    accel_forward = -tan(pitch)*9.81

Substitute in 45 degrees everywhere (negative for pitch forward):

    accel_right = tan(45 deg)*sec(-45 deg)*9.81

    accel_forward = -tan(-45 deg)*9.81

Our direction of travel is atan2(accel_right,accel_forward), so lets substitute into it:

    atan2(tan(45 deg)*sec(-45 deg)*9.81,-tan(-45 deg)*9.81) = 54.74 degrees <-- NOT 45 degrees! Problem.

We should be accelerating at tan(45 deg)/9.81 = 9.81 m/s/s given our 45 degree tilt limit. Lets see if we are: total acceleration is sqrt(accel_right^2+accel_forward^2). Lets sub in our values from before and see what that comes out to:

    sqrt( ( tan(45 deg)*sec(-45 deg)*9.81 )^2 + ( -tan(-45 deg)*9.81 )^2 ) = 16.99 <-- NOT 9.81! Problem.

    atan(16.99/9.81) = 60 deg <-- NOT 45 degrees! Problem.

Section 3: solutions

To fix the angle limit, we should do one of the following (probably both, on a parameter):

- Keep the response to stick movements linear, and have dead zones in the corners of the sticks.

- Scale the sticks such that the sticks are less sensitive when they're being moved towards the corners than they are when they're being moved towards the edges. In other words, stretch the circle so that it becomes a square.

To fix the directionality problem, we should be doing something like the following in stabilize and alt hold (obviously optimize for less trig):

    float max_accel_mss = tan(ToRad(g.angle_max*0.01f))*GRAVITY_MSS;

    float accel_forward_mss = -rc_2.norm_input() * max_accel_mss;

    float accel_right_mss = rc1.norm_input() * max_accel_mss;

    

    //angle limit fix goes ~here

    

    float pitch_target_rad = atan(-accel_forward/GRAVITY_MSS);

    float roll_target_rad = atan(accel_right * cos(pitch_target)/GRAVITY_MSS);

    

    //convert to centidegrees (ewwwwww, another topic) and send to attitude controller

[Jason Short]

I thought Leonard had already done this? He restricts it for sure on loiter.

Jason

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Robert Lefebvre]

I don't think anything has been done for Stabilize.  The over-angle is still there for sure.  I don't fully understand the tilt-direction-error, but can accept there's a problem there too, and then I think that's not being handled either.

Yes, it appears Leonard fixed this is Loiter, as that's where Jonathan got the snippet of code.

[mlloyd]

I've noticed this too and fixed it in my own branch a few weeks back. It's a pretty simple fix: I just limit roll/pitch commands to the unit circle. I've been flying with this change and really like it. I never notice the dead zones in the corner of the sticks.

https://github.com/diydrones/ardupilot/pull/1253

[David Pawlak]

I'm not sure I've even gotten to the corners!! Are there many that do?

My vote would be scale the circle to a square. I think if we're cranking it, we go to the stops, and anything else in between should be less.

But, given that many haven't even been to the corners, one way or another would be about the same.

As I was writing that, I thought that it is possible that maybe I've been to the corner during some desperate save maneuver. In which case I would probably want more than 45 deg.... in which case, the angle is probably more important to fix that the tilt.

????

[Jason Short]

Yeah, this one has been known about for a few years, but has always been on the back burner for fear of running out of resources. With pixhawk it's a no brainer.

Jason

[Randy Mackay]

 

     Mlloyd’s solution is somewhat light weight and it would be used only in althold, autotune, drift, land, poshold and stabilize modes. Poshold is probably the heaviest of those in terms of CPU… it might be ok.

 

     Jonathan, you happy with this solution?

 

-Randy

[Robert Lefebvre]

It's not a complete solution to the problem Jonathan has pointed out.  But it is at least a partial solution.  I like it as a start. 

[Jaime Machuca]

This sounds a lot like a really old problem that's been around even before Ardupilot. When flying RC Helis in the past going to the corners would cause binding on the swash plate. The solution (low tech solution) in the past was to use a cyclic ring on the transmitter (http://www.rcheliwiki.com/Cyclic_ring) then a few transmitters included an electronic cyclic ring function that works like Mlloyd's solution limiting to the circle. 

Jaime

Sent from my iPhone

[mlloyd]

Interesting. Before I implemented this I looked to see whether Taranis had unit circle limiting, and I briefly considered adding support for it in OpenTX. Much easier to do in the ardupilot codebase as it turned out :)

[mlloyd]

Yep it fixes the >45 degree tilt angle problem in a clean way, for loiter and for all flight modes that call get_pilot_desired_lean_angles.

I don't think it fixes the east-north-east direction of travel issue, but that seems somewhat separate...

I definitely prefer unit circle limiting rather than stretching the circle into a square - I think it's pretty important to keep the control response linear as a function of stick distance. It's counterintuitive for the effective speed to be different depending on whether you've moved the stick 10% forward or 10% diagonally.

[john...@gmail.com]

Yes, please don't mess with control linearity. If people want curved control responses, it should be explicitly selected like you do with expo in the radio today.

- JAB

[Robert Lefebvre]

John, if you're talking about not wanting to stretch the circle into a square, I agree with you. But  the angle problem (east north east) that Jon is talking about, is an existing non-linearity.  He's trying to linearize it.

I don't fully understand the trig involved (have't really looked at it) but what he's saying is that, if the copter nose is pointing due north, and you push the stick 45° up to the right, you would expect the compter to tilt in the diection of 45° on the compass and accelerate that direction.  But apparently that's not what happens due to an innaccuracy in the way Euler angles are done, and instead it will tilt to 60° on the compass and head off that way.  

[john...@gmail.com]

Yes, is was talking about the linearity of the R/C stick inputs.

[David Pawlak]

It's a complicated problem. What we are actually doing is projecting a 45 deg pitch vector onto the already 45 deg roll plane.

What is needed is to work on the output vector based on the control stick input, which would ultimately  mean less that 45 deg roll and pitch in the corner, but a total final lean of 45 in the desired direction.

So "output vector" would be linear, but roll and pitch would not. 

[Leonard Hall]

Nice!

I have been waiting for this to come up :)

I personally like the limiting the sticks to a unit circle and that is basicly what we have done in Loiter.

Keep up the chat :)

Leonard

[jolyboy]

Sorry for my ignorance, but is it possible that using quaternions instead of Euler angles would solve this whole problem of 45° angle projections aka east-north-east?

Then add unit-circle limiting on top of that. I use a plastic mechanical limiter on my cyclic stick, but a software solution as well would be more elegant.

[Robert Lefebvre]

Not directly, I don't think.  At least not in rate mode.  Because if you have Acro_RP_P at 10 for example, you get 450 deg/s pitch and roll.  But push the stick in the corner, and you get a total of root(450^2 + 450^2)= 636 deg/sec.  And the angle recording mechanism won't change that.

On 9 September 2014 03:20, jolyboy <futur...@gmail.com> wrote:

Sorry for my ignorance, but is it possible that using quaternions instead of Euler angles would solve this whole problem of 45° angle projections aka east-north-east?

Then add unit-circle limiting on top of that. I use a plastic mechanical limiter on my cyclic stick, but a software solution as well would be more elegant.

[jolyboy]

I'm not referring to the total rate output for 45°, I'm referring to the angle of tilt that is currently different to the user input. To quote Jonathan:

"Further, it is NOT traveling in the operator's expected direction of north-east (45 degrees)!! Instead, it is traveling east-north-east (54.74 degrees)."

Can THIS be fixed with quaternions? Because it seems to be an anomaly associated with the use of Euler angles.

[Paul Riseborough]

It is due to the use of Euler Angles as the means of defining tilt and can be fixed by changing to a vector representation of the tilt command as this will allow a true circular limit to be applied and ensures the operator (and navigation loop) intended direction of tilt is preserved.

Although Quaternions can be used in the intermediate calculations required to calculate the attitude error, vector algebra based on the DCM matrix can also be used, and will gives a more intuitive solution. The processing steps would go something like this:

1) Operator stick inputs are converted to a tilt unit vector which would be body relative. For simple and super simple modes, this would be calculated as earth relative and rotated into the body frame.
2) A circular limit is applied to the tilt vector X and Y components and the vector is re-normalised. The resultant represents the demanded tilt in body frame.
3) The negative of the bottom row of the DCM matrix is used to represent the orientation of the local vertical in body frame. The resultant represents the measured tilt in body frame.
4) A vector cross are used to calculate the rotation vector that represents the difference in angle between the demanded and measured tilt vectors. The X and Y components of this represent the angle error about the X and Y body axis.
5) The roll and pitch errors and the corresponding body rates are then used by the attitude PID's

I don't have the spare bandwidth to write code for this, but am happy to work anyone that wants to have a go through the maths.

Regards,

Paul

[David Pawlak]

The key is in step  1) Just about everything is done there. and this produces a unit vector, step 2) would just have to multiply by a factor which brings pure X or Pure Y to the edge. The circular limit should already be applied, right?

The matrix math takes care of the rest.

[Paul Riseborough]

Scaling X and Y to bring the magnitude back within the circular limit and could be done at the user or control loop input stage prior to 1), or it can be done at 2) after the rotation into body axes. It doesn't matter, as long as the circular limit is respected and the resulting vector has a length of 1).

The rest, as you say, is just linear algebra.

[jolyboy]

The limits are also non-circular at the output stage. I observed it today making the pirocomp video. I'm not sure if it matters for multis, but for helis this can cause issues such as exceeding cyclic pitch limits causing blade stall.

I'll have a go at making a cyclic limits patch for tradheli

[jolyboy]

Rob, if you'd like to take a look, I've made a quick mod to the output limits for TradHeli. This will fix the rectangular cyclic pitch limits (roll/pitch output) that can cause excessive blade pitch leading to blade stall. The cyclic pitch limit will now be the same for the entire rotor-head rotation ensuring that cyclic pitch will not exceed what is measured during setup. 

https://github.com/jolyboy/ardupilot/commit/6c6c4e9d2be4590eec5901540eea87da65ce21e9

[Haygood]

I'm pondering the difference between limiting the inputs to a  circle, vs. scaling them to a circle.  

Here is what I'd like to have happen.  If the stick inputs are LIMIITED to a circle, and outside the circle is a dead zone to each corner, then I can have the drone following a car at 5m/s.  If I want to move from following on the right of the car or following on the left of the car, I can leave the right stick where it is in pitch, and only move it to the left a little.  My forward speed would stay 5m/s the whole time.  If I try the same maneuver at a much higher speed, the copter might hit the limit circle and slow its forward progress so it never exceeds 45 degrees, but within the circle, it does exactly what I personally expect.  That sounds good to me.

If the inputs are scaled, then If I move the stick to the left, my forward speed is reduced, even when I'm not near the limits of the copter's ability to do both at the same time.  I wouldn't like this because I would rather have stick forward position related to forward force/tilt as much as possible, and the same for sideways movement.  That's just what I expect when I put the stick there.

The benefit of the scaled approach might be that I could maintain 5m/s in any direction with the same amount of stick displacement, so if I rotate the copter, I can keep the speed constant by moving the right stick in a circular path.  

So I'm for the limit circle/cyclic ring with dead zones per the mlloyd method and against scaling the whole pattern of stick movement as Mr. Riseborogh mentioned.

Carry on.  :)

[Paul Riseborough]

I was talking about a circular limit applied to tilt demands to the attitude control loop, not about how speed demands are limited (different issue)

[jolyboy]

Are we in agreement that we need a circular limit applied to the cyclic pitch for helis as per the commit above? If so I'd like to submit a PR for this.

[Randy Mackay]

I agree that the deadzones at the corners is the probably the way to go.

I'd be happy to accept a patch into master that does this (it won't make AC3.2). There was one a while ago but it had some problems so it didn't make it in.

I think all the changes can be made in Attitude.pde's get_pilot_desired_lean_angles function.

-Randy

[jolyboy]

What I'm referring to is the hard cyclic limits for the swashplate servos on helis, not attitude control. It is a similar issue, but different to the main attitude issue being discussed. I don't mean to be off topic by posting it here, so I apologise if the discussion is being misdirected. I saw an opportunity for this quick fix and took it :)

The limiting is done by replacing H_ROL_MAX and H_PIT_MAX with a different parameter, H_CYC_MAX. For each iteration, the pitch and roll are summed and then compared to H_CYC_MAX. The vector angle is also calculated. If the total pitch and roll exceed the limit, they are scaled down appropriately using the vector angle. This gives the swashplate a circular limit. It is calculated within AP_MotorsHeli.cpp

I highly recommend this patch because AFAIK there isn't a helicopter out there that requires different cyclic pitch limits for pitch and roll axes, plus this eliminates the issue of exceeding preset cyclic pitch limits everywhere that isn't dead on axis. Please take a look at the commit and you see what I mean :)

[Robert Lefebvre]

Hi Joly, I haven't looked at the commit yet, but I agree with what you're saying here in principle.

It may have been the case in the past with mechanical mixing that helis would have different pitch and roll limits.  But with modern CCPM, I really don't see why this would be the case.  In fact, typically cyclic limits are set to constrain the blade cyclic pitch to a reasonable amount, rather than any kind of mechanical limitation.  So a circular limit makes sense.

My only problem with this is that I think we need to solve both the input and the output problems at the same time.  If the input is able to feed square pitch and roll limits, which are able to sum to exceed the cyclic limit, so we truncate the output before sending it to the servos, then the Pitch and Roll I-terms will wind up falsely.  So at the very least, we'd need to feedback that these are on the limits, and freeze the I-term.

But I think the best case is to limit the input to circular limits, run it through the pitch and roll pids, and then scale the output by H_CYC_MAX.  We could do a check that we aren't exceeding the circular cyclic limits.  I'm trying to think of how/why that might be a problem if we have circular input limits...  yeah, we could probably still have a problem here because despite circular limited inputs, one of the axis PIDS could still have an excessive output due to a large rate error.  So what you've done will probably be good.  We just need to make sure to feed back to the PID loops that outputs are being limited.  There is already a provision for this.  So it should be easy (if you haven't done so already).

[jolyboy]

There is already the flag you added that goes true when a limit is reached. I have just maintained the existing functionality of that. If we currently dont already do anything with that flag, its going to be easy to do so in the future because it is there waiting to be used.

I'll have a go at circular limits on the input side

[jolyboy]

Hey guys, here are some commits for the circular limits. One is for limiting the input in Attitude.pde as suggested by Randy (thanks for the suggestion it saved me hours). The other two commits are for TradHeli swashplate limiting.

The code is much shorter/smaller than the original. And in the case of helis we have one less parameter to worry about too :)

https://github.com/jolyboy/ardupilot/commits/Circular-Limits

[Jonathan Challinger]

It can be done without all that trig in component form.

[jolyboy]

I don't follow, do you mean that you know of a way to convert from polar to cartesian without trig, or that I should be using vector2f to do the trig?

[Jonathan Challinger]

So you do this:

    total_out = pythagorous2((float)pitch_in, (float)roll_in);

    vector_angle = atan2((float)pitch_in, (float)roll_in);

    if (total_out > ROLL_PITCH_INPUT_MAX) {

        roll_out = (int16_t)((float)ROLL_PITCH_INPUT_MAX * cosf(vector_angle));

        pitch_out = (int16_t)((float)ROLL_PITCH_INPUT_MAX * sinf(vector_angle));

and you could do this:

    total_out = pythagorous2((float)pitch_in, (float)roll_in);

    if (total_out > ROLL_PITCH_INPUT_MAX) {

        float ratio = (float)ROLL_PITCH_INPUT_MAX / total_out;

        roll_out = roll_in * ratio

        pitch_out = pitch_in * ratio;

    }

    //for bonus points, do the conversion from tilt-vector to euler angles here

On Thu, Sep 18, 2014 at 10:46 PM, jolyboy <futur...@gmail.com> wrote:

I don't follow, do you mean that you know of a way to convert from polar to cartesian without trig, or that I should be using vector2f to do the trig?

[jolyboy]

:O Wow OK yes nicely done. Much, much better than mine. Do you mind if I commit that?

[Jonathan Challinger]

No problem.

On Thu, Sep 18, 2014 at 11:04 PM, jolyboy <futur...@gmail.com> wrote:

:O Wow OK yes nicely done. Much, much better than mine. Do you mind if I commit that?

[jolyboy]

OK I've made the changes as per Jonathan's suggestion, plus added circular input limiting to ACRO. Bench tests and hovers are good, but cant get out of the city to do proper testing for a while.

https://github.com/jolyboy/ardupilot/commit/084b3ce87dab4c1ecb1d22eeb036fea1e83c5b7f