Add coordination to level mode setpoints: DO NOT MERGE PROTOTYPE ONLY #12071

ChrisRosser · 2022-12-21T12:58:53Z

Video by @ctzsnooze showing the problem and the fix in this PR: https://www.youtube.com/watch?v=euXXkhVT-9A

@ctzsnooze I hope that similar to my last PR you can work on this, test it, and open a mergeable PR at an approprate time.

Angle mode gets a bit confused when you give sharp yaw inputs to the quad when the quad is already pitched quite far over. This code aims to fix that.

Why do we have an issue today?

The angle mode controller is fundamentally horizon referenced. We request pitch and angle relative to the horizon with stick inputs. We also implicitly request yaw relative to the horizon as well (otherwise our control axes are not orthogonal!)

The angle controller takes those requests and turns them into a gyro setpoint to move the quad to the requested angle. These gyro setpoints are quad referenced.

All is well when quad reference and horizon reference are similar:
A change in roll angle creates a change in roll gyro setpoint. A change in roll angle requires the quad to rotate about it's roll axis only.
A change in pitch angle creates a change in pitch gyro setpoint. A change in pitch angle requires the quad to rotate about it's pitch axis only.

However, when the quad is pitched forward at an angle the quad reference is at an angle to the horizon reference. This means that a change in roll angle (which is horizon referenced) requires the quad to rotate about both it's roll and yaw axes.

When the quad is rolled over at an angle the quad reference is again at an angle to the horizon reference. This means that a change in pitch angle (which is horizon referenced) requires the quad to rotate about both it's pitch and yaw axes.

Yaw is even more complicated. A change in yaw angle (horizon referenced) may require roatation about all 3 quad axes to achieve it.

This causes issues for the current angle controller as when the quad is at an angle an input on one axis creates error on the others this error then needs to be corrected by the controller causing wobbles.

This PR updates the roll, pitch and yaw gyro setpoints based on the current attitude of the quad so that the gyro setpoints always move the quad straight towards the target angles. Inputs on one axis will no longer create angle error on the other axes.

add coordination to level mode setpoints

blckmn · 2022-12-21T15:17:14Z

AUTOMERGE: (FAIL)

github identifies PR as mergeable -> FAIL
assigned to a milestone -> FAIL
cooling off period lapsed -> PASS
commit count less or equal to three -> FAIL
Don't merge label NOT found -> FAIL
at least one RN: label found -> FAIL
Tested label found -> FAIL
assigned to an approver -> FAIL
approver count at least three -> FAIL

ChrisRosser · 2022-12-21T18:39:52Z

Some examples to assist the review:

Example 0: Quad is level.

Both before and after the PR roll, pitch, and yaw moves translate to gyro setpoint as both coordinate systems are aligned.

Example 1: Quad pitched forwards 45 degrees. Yaw right.

Before PR: The yaw move (in quad coords) will create angle error on the roll and pitch axes (horizon coords). The angle controller will apply roll and pitch setpoint (quad coords) to "fix" this with significant delay. The delay can cause wobbles and makes the quad very slow to react to the stick inputs.

After PR: The yaw move is correctly translated from horizon coords to quad coords. The angle controller requests appropriate roll and yaw rotation. The result is a coordinated pure yaw move in horizon coords as requested. No error is generated by the move for the angle controller to fix.

Example 2: Quad pitched forwards 90 degrees. Yaw right.

Before PR: The yaw move (quad coords) will not achieve any yaw effect. Instead it will only create error on the roll and pitch axes (horizon coords). The angle controller will try to fix this by applying roll and pitch setpoint (quad coords). The yaw move only happens due to these delayed corrections. The delay can cause wobbles and makes the quad very slow to react to the stick inputs.

After PR: The yaw move is correctly translated from horizon coords to quad coords. The angle controller requests right roll (quad coords) and the quad yaws (horizon coords) exactly as expected! No error is generated by the move for the angle controller to fix.

Quick-Flash · 2022-12-21T18:51:22Z

Cosine matrix math going on here. Thats one of the possible methods to solve this. Curious to see this flying. It should help remove the non-linearities as well :)

src/main/flight/pid.c

Quick-Flash · 2022-12-21T20:53:13Z

src/main/flight/pid.c

+{  
+    if(axis == FD_YAW){
+        if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(GPS_RESCUE_MODE)){
+            currentPidSetpoint = pidRuntime.angleSetpoint[FD_YAW] * cos_approx(DECIDEGREES_TO_RADIANS(attitude.values.pitch)) * cos_approx(DECIDEGREES_TO_RADIANS(attitude.values.roll)) - pidRuntime.angleSetpoint[FD_PITCH] * sin_approx(DECIDEGREES_TO_RADIANS(attitude.values.roll)) + pidRuntime.angleSetpoint[FD_ROLL] * sin_approx(DECIDEGREES_TO_RADIANS(attitude.values.pitch));


Attitude filter should be added here as the angleSetpoint should not be prefiltered due to non-linearities.

src/main/flight/pid.c

Quick-Flash · 2022-12-21T20:59:22Z

I'd be curious to see what 90 degree roll/pitch would look like. I'm thinking that you'll want to also limit pitch/roll based on how high pitch/roll are.

Think of angle mode like the top of a sphere, where the pitch stick is the y axis on the top of the sphere and roll the x axis of the ball. Since its spherical you would need to limit your max pitch/roll so that you stay on the ball. That way a full pitch and roll, with a 90 degree limit, would be completely vertical with the front right motor towards the ground.

ctzsnooze · 2022-12-21T21:44:59Z

Should these corrections be done in imu.c? Or here in pid.c?

Quick-Flash · 2022-12-21T22:05:43Z

Should these corrections be done in imu.c? Or here in pid.c?

Here, as this correction should be applied based on the pitch and roll setpoint and not the angle that you are currently at.

ChrisRosser · 2022-12-21T22:18:51Z

I'd be curious to see what 90 degree roll/pitch would look like. I'm thinking that you'll want to also limit pitch/roll based on how high pitch/roll are.

Think of angle mode like the top of a sphere, where the pitch stick is the y axis on the top of the sphere and roll the x axis of the ball. Since its spherical you would need to limit your max pitch/roll so that you stay on the ball. That way a full pitch and roll, with a 90 degree limit, would be completely vertical with the front right motor towards the ground.

At 90 degree pitch your yaw stick controls quad roll rate.

At 90 degree roll your yaw stick controls quad pitch rate.

The maths should work even over 90 degrees. Not sure how useful that is. You'll stay "on the ball" but be on the underside of it!

ChrisRosser · 2022-12-21T22:25:23Z

Should these corrections be done in imu.c? Or here in pid.c?

Here. The job of the angle controller is to instruct the rate controller what to do to achieve the angle setpoint. At the moment the instructions are like mediocre directions, you'll get there but it takes longer than it needs to.

This PR improves the instructions from the angle controller by taking into account the real effect of gyro roll, pitch and yaw given the current angle of the quad so the target angle is achieved faster and with less wobbles.

ctzsnooze · 2022-12-22T06:20:34Z

I did some testing and made this video:
https://www.youtube.com/watch?v=go1h4DI2uDw
it’s not perfect but will do.

Seems that yaw is too fast for both master and 12071, even if done relatively slowly. In both cases, the quad yaws ‘first’, losing pitch and gaining roll in the opposite direction to the yaw (eg right roll on yawing left); then the level P controller re-gains pitch and removes the roll.

I am not good enough a coder to know what is going on in 12071 😞 , but I think that the yaw input is over “too soon”. It's too quick for the angle controller. While yawing, the roll setpoint may indeed be changed, but once the yaw stick is still, the roll and pitch setpoints revert to normal, well before the quad has actually achieved appropriate post-yaw angles.

One possible solution could be to converted yaw inputs direct to gyro rate changes in pitch and roll, not angle setpoint changes.

By directly changing the gyro setpoint when we yaw, the similar lags between yaw and roll should keep the quad in normal attitude.

For example, if a quad with 45 degrees of pitch was commanded 360 deg/s left for 0.25s, so that it would acquire 90 degrees of yaw in a quarter of a second, then during that ¼ second, roll gyro rate should be changed with a left roll rate target that would lose 45 degrees of angle in ¼ second, ie at a rate target of 180 deg/s left. Likewise, pitch gyro should be driven actively to gain 45 degrees forward over that quarter second, also at 180 deg/s, since that is how much the yaw would bring the pitch axis upwards.

If this additional rate request went direct to the pitch and roll gyro set points, the angle controller wouldn’t know about it, nor would it need to; it would find only relatively small errors to fix.

Update pid.c Update pid.c fix signs Update pid.c

Update imu.h Update imu.h updates

github-actions · 2022-12-23T12:23:59Z

Do you want to test this code? Here you have an automated build:
Assets
WARNING: It may be unstable. Use only for testing! See: https://www.youtube.com/watch?v=I1uN9CN30gw for instructions for unified targets!

github-actions · 2023-02-14T04:34:14Z

This pull request has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs within a week.

github-actions · 2023-02-26T04:33:47Z

Pull request closed automatically as inactive.

ctzsnooze · 2023-03-22T23:14:50Z

Closed since #12231 has been merged

This comment has been minimized.

Sign in to view

haslinghuis added In Development Don't Merge labels Dec 21, 2022

Update pid.c

8fa7d27

add coordination to level mode setpoints

ChrisRosser force-pushed the angle-coordination branch from 98b06b1 to 8fa7d27 Compare December 21, 2022 14:09

This comment has been minimized.

Sign in to view

Quick-Flash reviewed Dec 21, 2022

View reviewed changes

src/main/flight/pid.c Show resolved Hide resolved

Quick-Flash reviewed Dec 21, 2022

View reviewed changes

src/main/flight/pid.c Outdated Show resolved Hide resolved

Update pid.c

3dc7d09

This comment has been minimized.

Sign in to view

updates for level_race_mode

2cd4507

Update pid.c Update pid.c fix signs Update pid.c

ChrisRosser force-pushed the angle-coordination branch from 0888995 to 2cd4507 Compare December 22, 2022 16:52

This comment has been minimized.

Sign in to view

optimisation

9e71a43

Update imu.h Update imu.h updates

ChrisRosser force-pushed the angle-coordination branch from 0b22174 to 9e71a43 Compare December 23, 2022 12:21

ChrisRosser mentioned this pull request Dec 23, 2022

Improved angle mode with coordination and FF #12081

Closed

ctzsnooze mentioned this pull request Dec 26, 2022

Angle mode feedforward and earth referencing of yaw inputs #12067

Closed

blckmn added the Rebase Required label Jan 15, 2023

ctzsnooze mentioned this pull request Jan 31, 2023

Angle and Horizon Mode improvements #12231

Merged

github-actions bot added the Inactive Automatically detected and labeled, will be closed after another week of inactivity. label Feb 14, 2023

github-actions bot closed this Feb 26, 2023

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add coordination to level mode setpoints: DO NOT MERGE PROTOTYPE ONLY #12071

Add coordination to level mode setpoints: DO NOT MERGE PROTOTYPE ONLY #12071

ChrisRosser commented Dec 21, 2022 •

edited

This comment has been minimized.

This comment has been minimized.

blckmn commented Dec 21, 2022 •

edited

ChrisRosser commented Dec 21, 2022 •

edited

Quick-Flash commented Dec 21, 2022 •

edited

Quick-Flash Dec 21, 2022

Quick-Flash commented Dec 21, 2022

ctzsnooze commented Dec 21, 2022

Quick-Flash commented Dec 21, 2022

ChrisRosser commented Dec 21, 2022

ChrisRosser commented Dec 21, 2022 •

edited

ctzsnooze commented Dec 22, 2022

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

github-actions bot commented Dec 23, 2022

github-actions bot commented Feb 14, 2023

github-actions bot commented Feb 26, 2023

ctzsnooze commented Mar 22, 2023

Add coordination to level mode setpoints: DO NOT MERGE PROTOTYPE ONLY #12071

Add coordination to level mode setpoints: DO NOT MERGE PROTOTYPE ONLY #12071

Conversation

ChrisRosser commented Dec 21, 2022 • edited

This comment has been minimized.

This comment has been minimized.

blckmn commented Dec 21, 2022 • edited

ChrisRosser commented Dec 21, 2022 • edited

Quick-Flash commented Dec 21, 2022 • edited

Quick-Flash Dec 21, 2022

Choose a reason for hiding this comment

Quick-Flash commented Dec 21, 2022

ctzsnooze commented Dec 21, 2022

Quick-Flash commented Dec 21, 2022

ChrisRosser commented Dec 21, 2022

ChrisRosser commented Dec 21, 2022 • edited

ctzsnooze commented Dec 22, 2022

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

This comment has been minimized.

github-actions bot commented Dec 23, 2022

github-actions bot commented Feb 14, 2023

github-actions bot commented Feb 26, 2023

ctzsnooze commented Mar 22, 2023

ChrisRosser commented Dec 21, 2022 •

edited

blckmn commented Dec 21, 2022 •

edited

ChrisRosser commented Dec 21, 2022 •

edited

Quick-Flash commented Dec 21, 2022 •

edited

ChrisRosser commented Dec 21, 2022 •

edited