Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Angle mode feedforward and earth referencing of yaw inputs #12067

Closed

Conversation

ctzsnooze
Copy link
Member

@ctzsnooze ctzsnooze commented Dec 20, 2022

This PR continues the development of @ChrisRosser's initial PR #12041, which added feedforward to Angle Mode.

Main changes

  • angle mode feedforward - adjustable with angle_feedforward and angle_feedforward_smoothing
  • angle mode earth referencing for yaw inputs - adjustable with angle_earth_ref

Angle mode feedforward speeds up responsiveness to stick inputs by adding a feedforward element the simple angle error controller. It respects the angle expo settings and the general feedforward jitter correction settings.

Earth referencing of yaw inputs is provided because:

  • it minimises the cross-axis wobble that otherwise would occur with a fast yaw input at steep quad angles. This happened previously because yaw inputs in the axis of the quad were too fast for the pitch and roll levelling functionality.
  • it automatically 'co-ordinates' turns, keeping the PFV camera image, and the pitch of the quad, unchanged during a fast yaw input while pitched forward. This is achieved by mixing in the right amount of roll to a yaw input, and attenuating yaw at high quad angles.

The earth referencing code in this PR was inspired by Chris Rosser's PR #12071. There are two main differences between the two variants are that here:

  • only yaw inputs are earth referenced; pitch and roll inputs act in the quad's plane of reference, whereas 12071 earth references inputs on all axes. By only earth referencing yaw, stick 'feel' is very similar to our 'traditional' Angle mode feel
  • the 'strength' of the earth referencing can be varied from zero to 100%
  • that the input to the earth referencing code is from target angles, not from measured attitude. This ensures no weird outcomes on impacts where measured attitude values could enter extreme ranges, but can lead to a bit more wobble with very fast inputs in less responsive builds.

Overall this PR brings a significant improvement in Angle Mode responsiveness, reducing the need to push angle P gains higher than normal, and much improved yaw behaviour.

Horizon mode is not changed (hopefully).

CLI snippet to remove all angle feedforward and earth referencing - to see what it used to be like:

set angle_feedforward = 0
set angle_earth_ref = 0

CLI snippet to restore new angle feedforward and full earth referencing of yaw

set angle_feedforward = 50
set angle_earth_ref = 100

Background

Previously, Angle Mode replaced the acro mode setpoint with a new setpoint that was calculated only from the error between the target angle and the measured angle of the quad. It was a simple P controller. The angle_error correction gain factor was called level_strength. In this PR it is renamed to angle_p_gain.

In the older versions, a stick input in Angle Mode (a change from one target Angle value to something different) would dynamically adjust the PID loop setpoint values in proportion to the angle error (it was a simple P controller). The setpoint value / resulting gyro rate was proportional to the angle error. Because the gyro rate was proportional to error, it would decrease as the error decreased, and the quad followed an exponential decay from the initial towards the new target value - literally taking 'forever'. Additionally, the initial response was slow, since error would be small at the start of any change in stick position. The result was a floaty, drifting, slow stick response, in angle mode, typically taking hundreds of milliseconds to acquire the intended target angle. If the user wanted a quicker stick response, the only option was to increase the level gain factor, which often caused oscillation.

@ChrisRosser recognised that we could add a direct stick-movement-related feedforward element to the angle controller, which would generate an immediate change in angle setpoint, especially with quick stick inputs, in turn leading to an immediate gyro response. The concept was, in principle, similar to the feedforward used in the acro PID loop.

There were significant technical challenges in providing feedforward in two places at the same time, most notably exaggerated feedforward noise unless the angle mode feedforward was very heavily smoothed. Additionally, feedforward in angle mode will exaggerate any wobbles in the stick inputs much more than feedforward in the gyro loop. The end result was very good.

Note that Angle Mode feedforward respects both the angle_expo settings and the general jitter reduction and duplicate interpolation settings. Also note that the PID loop feedforward works on setpoint as usual.

Summary of changes

  • all CLI parameters used by Angle Mode re-named to start with angle_

  • angle_feedforward CLI value: Adjusts the angle feedforward amount or strength; default is 50, zero means none, returning the previous behaviour.

  • angle_feedforward_smoothing CLI value: Adjusts the response time of the angle feedforward effect; this is implemented using a PT3 filter over the raw feedforward data, with its cutoff set to 100 / angle_feedforward_smoothing in Hz. The default of 50 returns the time constant of a 2hz PT3 lowpass filter. This filter removes noise and jitter from the angle feedforward element.

  • Angle Mode expo factor: Defaults to 33 on both angle_roll_expo and angle_pitch_expo, to soften centre stick responsiveness

  • angle_limit changes - the default value is increased to 60 degrees (from 55 degrees), and the max reduced to 85 degrees (from 90). You may experience control issues when angle_limit is set to more than 63 degrees and both pitch and roll sticks are at full deflection, since the FC will point to the ground (be up-side down)with those inputs. Don't go above 63 unless you avoid full stick deflections on pitch and roll at the same time.

  • angle_earth_ref sets the amount of earth referencing. When at default of 100, we get automatic 'co-ordinated' turns, i.e. the pitch angle of the quad remains stable even during quick yaws at high stick angle. A value of 0 disables earth referencing, and the behaviour is same as the old Angle Mode code, where the pilot needs to manually provide significant roll input, and use less yaw, when turning at high pitch angles. A value of 50 applies roughly half the effect.

  • iTerm_Relax is made more sensitive to avoid iTerm wobble with slow wobbly stick inputs

  • an ANGLE_MODE debug is provided to visualise target angle, acquired angle, and the contributions from error and feedforward control elements. All values are in degrees.

Understanding Feedforward in Angle Mode

Absolute stick position in Angle Mode sets the requested angle of the quad. A change in angle is acquired by dynamically adjusting the gyro rate setpoint until the required angle is achieved.

Feedforward in Angle Mode translates stick velocity into the rate of change of angle target. This is the same as a direct change in gyro rate setpoint (the target turn rate for the acro PID controller). Angle Feedforward is therefore entirely independent of the contribution from the error-based method, which sets the target turn rate in proportion to angle error only.

After a given amount of stick movement is complete, the gyro rate that has integrated over the stick movement period from angle feedforward will have achieved, from the feedforward alone, a certain change in the angle, or attitude, of the quad. The amount of angle change from angle_feedforward depends only on the speed of stick movement, the duration of the movement, and the angle_feedforward gain setting.

Therefore we can think of the angle feedforward element as being a way to directly add angle from stick input alone. In fact, with enough feedforward, the requested angle can be acquired, with zero angle_P_ gain, from feedforward alone.

If the angle_feedforward value is set too high, it will cause the quad to overshoot the target angle change, leading to a delayed 'rubber-banding' or pull-back of that overshoot. This is a sure way to know that the angle_feedforward setting is too high, assuming the quad is otherwise properly tuned in acro mode. An LOS pilot, or a quad with a gimballed camera, may intentionally seek some pull-back to pull up more quickly in Angle Mode. For general FPV flying the default value is close to the maximum that should be used.

Lowering the angle_feedforward value leaves the simple error controller more work to do, and the quad's responsiveness to stick inputs will be slower, smoother, and less complete, leading to greater lag and drift.

The angle_feedforward_smoothing parameter changes the response time, but not the overall amount, of the angle feedforward factor.

If there was no smoothing, an abrupt increase in stick velocity would lead to a step increase in the angle feedforward value, which would result in a step, or instant, increase in requested gyro rate. This would result in an incredibly jerky response from the motors, and lead to a lot of oscillation. Hence the angle feedforward value must be smoothed quite heavily.

Increasing angle_feedforward_smoothing smooths out the angle mode feedforward effect, using a PT3 filter. Higher values result in slower rates of change of gyro response, with more gentle feedforward effects. Too much smoothing can lead to overshoot or rubber-banding after fast stick inputs, because of slow decay of the feedforward element. When seeking a smoother or less aggressive feedforward effect, the the amount of angle_feedforward gain should be reduced at the same time.

Decreasing angle_feedforward_smoothing makes angle mode stick responses faster. This can be great for LOS level flying, since the quad will start and stop much more quickly and precisely. For FPV racing in level mode, eg level_race mode, the pilot will get a much more prompt and complete angle response. When set really fast, every little adjustment the pilot makes results in a quick gyro change, so that when set too fast, the quad appears to wobble or jump with even a tiny input.

Tuning

The defaults are really very nice, for general use.

Tuning is only needed if optimal performance is sought for level mode racing, or if less aggressive responses are needed for smooth HD video.

For racing / fastest possible angle mode responsiveness:

  • disable angle mode feedforward completely in the CLI with set angle_feedforward = 0.
  • Adjust angle_p_gain upwards. Aim for about 10% less than the value that introduces wobble. Angle wobble is easily seen in FPV goggles. Optimising this value helps the quad quickly fix both commanded and un-commanded angle errors.
  • restore angle_feedforward to the default value
  • test fly with fast stick inputs, spirals etc
  • if faster stick responsiveness is required, increase angle_feedforward until you get rubber-banding, then back off until it isn't there any more.
  • if even faster stick responsiveness is required, reduce angle_feedforward_smoothing until it's just too crazy. You may be able to slightly increase angle_feedforward at this point above the previous value.
  • for easy 'yaw only' inputs on turns, leave angle_earth_ref at default of 100. For a more acro-style / traditional 'roll into fast turn' behaviour, reduce the angle_earth_ref setting. At lower values you must provide a lot of roll input, and use less yaw input, to make clean turns at high pitch angles.

For smoothness:

  • disable angle mode feedforward completely in the CLI with set angle_feedforward = 0.
  • make sure there is no angle_p_gain wobble in the video footage. This would be seen as additional wobble that is not present in acro mode, perhaps more obvious around stick inputs. If there is, reduce angle_p_gain until the wobble is no worse than acro.
  • do a test flight and see if the responsiveness is 'about right'
  • if a faster stick response is needed, add angle_feedforward in steps, until the stick response feels about right
  • if the overall stick response is about right, but the quad still is a bit 'reactive', try increasing the angle_feedforward_smoothing parameter until the video is smooth enough.

For learning

  • use the defaults
  • consider reducing angle_limit, but usually not less than 40 degrees

To Do's

For this PR:

  • check that the unit tests are sensible. Currently they do not consider the effect of yaw inputs.

For other PR's:

  • move the jitterFactor calculation to rc.c so we only do that maths once
  • likewise move the duplicate detection and correction code so that it only applies to rc protocols that create duplicates.
  • integrate yaw inputs into angle set points. Currently a pure yaw input while pitched forward rotates the quad on the local yaw axis, losing pitch angle and gaining roll angle, and the error-based Angle Mode controller corrects these errors. But this correction is slow and has no feedforward. This could, perhaps, generate a setpoint change on roll, proportional to the pitch angle and the yaw rate, to provide a 'co-ordinated turn', and otherwise handle yaw inputs better, in level mode.
  • investigate a more earth-referenced attitude control method

@github-actions

This comment has been minimized.

@blckmn
Copy link
Member

blckmn commented Dec 21, 2022

AUTOMERGE: (FAIL)

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

@@ -84,6 +88,10 @@ extern "C" {
void systemBeep(bool) { }
bool gyroOverflowDetected(void) { return false; }
float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; }
float getRcCommandDelta(int axis) { return simulatedRcCommandDelta[axis]; }
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please also add

float getRcDeflectionRaw(int axis) { return simulatedRcDeflection[axis]; }

But still test is failing but more graceful.

@@ -125,7 +134,7 @@ void setDefaultTestSettings(void)
pidProfile->pid[PID_ROLL] = { 40, 40, 30, 65 };
pidProfile->pid[PID_PITCH] = { 58, 50, 35, 60 };
pidProfile->pid[PID_YAW] = { 70, 45, 20, 60 };
pidProfile->pid[PID_LEVEL] = { 50, 50, 75, 0 };
pidProfile->pid[PID_LEVEL] = { 50, 50, 75, 100 };
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please leave as is. See line 120 (129)

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If we set this to zero then we aren't testing the feedforward calculations. 100 is a sensible value (and not the default)

@github-actions

This comment has been minimized.

@github-actions

This comment has been minimized.

@ctzsnooze
Copy link
Member Author

Included a subset of Chris Rosser's #12071 to fix the wobbles visible when yawing fast at a time when there is a significant pitch or roll offset. The change also results in perfect co-ordinated turns when yawing in pitched forward flight.

I didn't include all of the changes in #12071 because when I flew that code, I found problems performing tight spirals. #12071 attenuates roll at high pitch angles, as part of the earth referencing. That dynamic attenuation of roll seemed to make tight spirals a lot more difficult to fly. It most likely would make roll dependent on pitch, which would be undesirable when changing pitch while holding roll angle to counter a cross-wind.

The simplification used in this PR prevents the cross-axis wobble that occurs with fast yaw inputs in the same way it does in 12071. For example, while pitched forward, an 'instant' 90 degree left yaw in the frame of reference of the quad will rotate the nose of the quad around to the left, and up to the horizontal plane; the quad will find that it has zero pitch angle and has accumulated a right roll angle equal to the original pitch angle. The angle controller will then pitch forward and roll left until the nose of the quad is pointing correctly. Although yaw is never instantaneous, it is much faster than roll or pitch, so the pilot sees a complex wobble with fast yaw inputs of this type; the FPV picture yaws left then pitches down and rolls left.

The code change here adds a setpoint or rate correction to roll (and, if necessary, pitch) that prevents these wobbles from occurring. A pure fast left yaw input while pitched forward results in a clean movement of the FPV image to the left, with a stable pitch angle. There is no wobble on pitch or roll.

While this is happening, the underlying angle mode controller remains active, allowing the pilot to add as much pitch or roll as they like. This permits the addition of extra roll to the turn if needed. Adding extra roll is needed in sharp 90 degree yaw turns, even fully 'coordinated' turns, to offset ongoing drift.

With these changes the quad is very clean and responsive on yaw, but also can do very tight nose-down spirals with high pitch angles where control is mostly via the roll stick, like in acro.

@github-actions

This comment has been minimized.

@github-actions

This comment has been minimized.

@ctzsnooze ctzsnooze force-pushed the Angle-Mode-feedforward branch 3 times, most recently from d51ea49 to 174eb6f Compare December 27, 2022 22:44
@github-actions

This comment has been minimized.

@ctzsnooze ctzsnooze changed the title Angle mode feedforward Angle mode feedforward and earth referencing Dec 27, 2022
@ctzsnooze ctzsnooze changed the title Angle mode feedforward and earth referencing Angle mode feedforward and earth referencing of yaw Dec 27, 2022
@ctzsnooze ctzsnooze changed the title Angle mode feedforward and earth referencing of yaw Angle mode feedforward and earth referencing of yaw inputs Dec 27, 2022
@ctzsnooze ctzsnooze force-pushed the Angle-Mode-feedforward branch 2 times, most recently from e0953e3 to fec5e6b Compare December 28, 2022 00:00
@github-actions

This comment has been minimized.

@ctzsnooze
Copy link
Member Author

Squashed commits, updated the notes. Test flown; works well. Feedback appreciated!

@szmidtpiotr
Copy link

test it for last 2 weeks. Work fantastic on:

Speedybee F405 and F7
Crazybee F4SX1280 ELRS
GEPRC F722 AIO
BETAFPV F405

@trigubov
Copy link

Tested on Crazybee F4SX1280 ELRS - work perfect!

btw - it dont work after applying diff. No erls bind ((( But after applying full damp - it works.

@ksmnt12
Copy link

ksmnt12 commented Feb 14, 2023

Could you do one for BETAFPVF411

@Shura1oplot
Copy link

Could you do one for BETAFPVF411

Here: betaflight_4.4.0_STM32F411_BETAFPVF411.zip

@ksmnt12
Copy link

ksmnt12 commented Feb 16, 2023

Could you do one for BETAFPVF411

Here: betaflight_4.4.0_STM32F411_BETAFPVF411.zip

Thanks.

@ccycv
Copy link

ccycv commented Feb 19, 2023

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!

This firmware does not include the GPS rescue?

@haslinghuis
Copy link
Member

@ccycv to test with extra features (like GPS), enable expert mode in firmware flasher tab and input #12067 in commit field.

@trigubov
Copy link

trigubov commented Feb 24, 2023

After flashing it to my Mobula6 thats running CrazybeeF4FR Pro v2, my pc doesn't recognize my drone anymore after rebooting. After flashing the firmware, everything works fine and it connects. Suddenly after changing back to my default receiver settings and rebooting, my computer doesn't recognize the drone.

Same issue here with Mobula6 STM32F411 - bricking after any save&reboot

Upd: tryed build, that offer in comments ("Try this one: betaflight_4.4.0_STM32F411_CRAZYBEEF4FR.zip") - problem is gone

@szmidtpiotr
Copy link

trying to build with Betaflight Cloud for iFlight_F222_Twing with #12067 in Commit but got error:

https://build.betaflight.com/api/builds/5f9713da4e9c64e770f968deba1b3d8e/log

@haslinghuis
Copy link
Member

@szmidtpiotr try a second time. A timeout can occur intermittent.

@szmidtpiotr
Copy link

No rule to make target 'IFLIGHT_F722_TWING'. Stop.

every time

@haslinghuis
Copy link
Member

@ctzsnooze we need to rebase now the make file has changed.

@haslinghuis
Copy link
Member

@szmidtpiotr
Copy link

@szmidtpiotr

IFLIGHT_F722_TWING

thanks, will test tommorow

@szmidtpiotr
Copy link

@szmidtpiotr

IFLIGHT_F722_TWING

uNfortunetly, after firmware upgrde i got Unknown USB Device (Device Descritpto Request Failed), insting intu usb while holding bootloader button doesnt recognize device too now

@Jackedup2Go
Copy link

Hi all, I'm new here and was hoping I could get some help....I have the CRAZYBEEF4SX1280 board and I'm having the same issue that @trigubov was having but I don't understand how he solved it. I cant get ExpressLRS to bind, I used the F411 "Asset" hex file.....I have both TX and RX on version ExpressLRS on v3.2.0. Could @szmidtpiotr (as I see you have had success) or @trigubov assist?

@trigubov
Copy link

trigubov commented Mar 3, 2023

Hi all, I'm new here and was hoping I could get some help....I have the CRAZYBEEF4SX1280 board and I'm having the same issue that @trigubov was having but I don't understand how he solved it. I cant get ExpressLRS to bind, I used the F411 "Asset" hex file.....I have both TX and RX on version ExpressLRS on v3.2.0. Could @szmidtpiotr (as I see you have had success) or @trigubov assist?

My steps:

  1. On working setup: CLI -> command 'diff' -> save to file -> clear -> command 'dump' > save to another file
  2. Flash F411 Asset
  3. Load custom deafaults, when asked
  4. CLI -> load diff file -> command 'save' -> load dump file -> command 'save' (don forget to type 'save' command after loading file

@Shura1oplot
Copy link

Hi all, I'm new here and was hoping I could get some help....I have the CRAZYBEEF4SX1280 board and I'm having the same issue that @trigubov was having but I don't understand how he solved it. I cant get ExpressLRS to bind, I used the F411 "Asset" hex file.....I have both TX and RX on version ExpressLRS on v3.2.0. Could @szmidtpiotr (as I see you have had success) or @trigubov assist?

Try this one: betaflight_4.4.0_STM32F411_CRAZYBEEF4SX1280.zip
It works with my CRAZYBEEF4SX1280.

@Shura1oplot
Copy link

@szmidtpiotr
IFLIGHT_F722_TWING

uNfortunetly, after firmware upgrde i got Unknown USB Device (Device Descritpto Request Failed), insting intu usb while holding bootloader button doesnt recognize device too now

Try this one: betaflight_4.4.0_STM32F7X2_IFLIGHT_F722_TWING.zip
It is based on a codebase of 4.4.0 with changes merged from this PR.

@Jackedup2Go
Copy link

Hi all, I'm new here and was hoping I could get some help....I have the CRAZYBEEF4SX1280 board and I'm having the same issue that @trigubov was having but I don't understand how he solved it. I cant get ExpressLRS to bind, I used the F411 "Asset" hex file.....I have both TX and RX on version ExpressLRS on v3.2.0. Could @szmidtpiotr (as I see you have had success) or @trigubov assist?

My steps:

  1. On working setup: CLI -> command 'diff' -> save to file -> clear -> command 'dump' > save to another file
  2. Flash F411 Asset
  3. Load custom deafaults, when asked
  4. CLI -> load diff file -> command 'save' -> load dump file -> command 'save' (don forget to type 'save' command after loading file

Hi all, I'm new here and was hoping I could get some help....I have the CRAZYBEEF4SX1280 board and I'm having the same issue that @trigubov was having but I don't understand how he solved it. I cant get ExpressLRS to bind, I used the F411 "Asset" hex file.....I have both TX and RX on version ExpressLRS on v3.2.0. Could @szmidtpiotr (as I see you have had success) or @trigubov assist?

Try this one: betaflight_4.4.0_STM32F411_CRAZYBEEF4SX1280.zip It works with my CRAZYBEEF4SX1280.

That hex file worked like a charm ! Thanks all, great community here !

@FrankPetrilli
Copy link
Contributor

Try this one: betaflight_4.4.0_STM32F411_CRAZYBEEF4SX1280.zip It works with my CRAZYBEEF4SX1280.

This firmware works in every way except VTX control on my v1 Mobula 6. Setting the UART to SmartAudio and inputting a valid VTX table, the "Video Transmitter" tab shows Device ready: Yes, band channel and power changes all appear to happen and save, but the VTX never changes channels. Flashing back to stock 4.4.0 works fine, 4.5.0 runs into the ExpressLRS binding issue others have mentioned.

Interestingly, the v2 Mobula 6 F4SX1280 board seems to work just fine including VTX channel changes with this firmware.

@TrevorCrouch
Copy link

Hi there, I tried installing : betaflight_4.4.0_STM32F411_CRAZYBEEF4FR.zip

This one doesn't seem to work out well with the crazybeeF4FR lite board (no outputs, protocol, etc). Any suggestions on what to use please? TIA

@ctzsnooze
Copy link
Member Author

Closed since #12231 has been merged

@ctzsnooze ctzsnooze closed this Mar 22, 2023
@scrnobaric
Copy link

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!

This firmware does not include the GPS rescue?

No link anymore to "Assets"?

@haslinghuis
Copy link
Member

@scrnobaric changes are merged in the other PR as you can check latest development version for testing now.

@KarateBrot
Copy link
Member

KarateBrot commented Apr 2, 2023

@scrnobaric You can also flash PRs directly from the configurator by typing the PR number into there (for example #12345 - including the pound sign in front).

grafik

@ctzsnooze ctzsnooze deleted the Angle-Mode-feedforward branch April 23, 2023 23:46
@Pierdol
Copy link

Pierdol commented Sep 3, 2023

Nice, asset is just link to the top of site. Dead topic.

@lesharodin
Copy link

Nice, asset is just link to the top of site. Dead topic.

Its allready merged.. just flash 4.5 dev build

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
Status: COMPLETED
Development

Successfully merging this pull request may close these issues.

None yet