Smooth throttle in navigation modes on FW

[Airwide]

This pull request resolves issue #6073.

Usage
Set new cli variable nav_fw_pitch2thr_threshold to a value between 0 and 100 decidegrees. That equals 0 to 10 degrees. 0 is default and keeps current behaviour.

The variable is also accessible in the OSD menu under features/navigation/fixed wing/.

Testing
You can see in the movies that the red throttle curve is more stable in the video with nav_fw_pitch2thr_threshold = 30 than in the video with nav_fw_pitch2thr_threshold = 0.

nav_fw_pitch2thr_threshold = 0:

nav_fw_pitch2thr_threshold = 30:

Btw, the higher throttle and speed with nav_fw_pitch2thr_threshold = 0 indicates to me that my board orientation is off by a degree or two.

@avS2 - Can you verify this is what you requested?

[avsaase]

Perfect, exactly what I had in mind. Thanks!

[avsaase]

I think you are not supposed to change Settings.md since it is auto generated.

[Airwide]

I think you are not supposed to change Settings.md since it is auto generated.

I'm not so sure. I think the auto generated note is actually a note for the table on the web page Settings.md:

I could be wrong though.

[Airwide]

@digitalentity What do you think of this pull request? Anything else I need to add before it can be merged?

[avsaase]

I have just build and flashed this for testing on my Dart 250. I will try to fly it later today and let you know how it went. Looks promising.

[avsaase]

I tested this with nav_fw_pitch2thr_threshold = 30 (together with #6101) and I think it works fine. Throttle level seems to be more stable. It was way too windy for this little wing so it was hard to see the exact effect. I think it would be nice to add this parameter to the in-flight adjustments.

I have to say that I did get some substantial horizon drift and the OSD altitude fluctuated by +-20 meters when turning the nose or tail into the wind direction. I think it's unlikely that this is related to this PR but I want to mention it anyway for completeness.

[avsaase]

This PR calms down the throttle behavior around zero degrees pitch, but outside of ±nav_fw_pitch2thr_threshold/10 degrees pitch, throttle can fluctuate as before. Also, throttle will stay closer to cruise throttle level if nav_fw_pitch2thr is left unchanged.

Would it be difficult to apply a filter to the pitch angle to smooth out the throttle behavior for all pitch angles? I think a simple moving average would do, but this firmware already includes some way more advanced filters so there may be better options. I think this would be a more elegant solution.

[Airwide]

@avS2 - Interesting idea. Let's say we have 50% cruise throttle. I'm thinking that at 5 degrees filtered (or projected) ascent you would like the deadband to be centered around 55% and at 45% for a 5 degrees descent. Let me see what I can come up with.

[avsaase]

@Airwide Hmm, so you're saying to keep the throttle constant at nav_fw_cruise_throttle - "target_angle" * nav_fw_pitch2thr as long as "actual_angle" is within nav_fw_pitch2thr_threshold/10 degrees of "target_angle"? I wonder if this will have unexpected effects when "target_angle" is changing.

[Airwide]

@avS2 You're right, using the target angle would cause weird throttling in the target transitions. A moving average (or filtered) angle throttle compensation would work much better.

[Airwide]

Ok, I have added const int16_t baseThrottleCorrectionwhich is calculated from the 128 last pitch values (const int16_t movingAverageCycles = 128)

I thought it was unnecessary to create a cli variable for const int16_t movingAverageCycles = 128. The question is what the constant should be set to. How often is fixedWingPitchToThrottleCorrection(int16_t pitch) called? Every loop?

@avS2, @digitalentity WDYT?

[avsaase]

Nice! I have added some comments to the code. Have you tested this already? For testing purposes it would be nice to have a CLI command so that you can quickly turn the smoothing on and off in the field.

[Airwide]

Nice! I have added some comments to the code. Have you tested this already? For testing purposes it would be nice to have a CLI command so that you can quickly turn the smoothing on and off in the field.

I have not had the opportunity to test. My AR Wing crashed a couple of weeks ago, caused by the FrSky failsafe bug disarming the wing mid flight. Hopefully I will be up in the air again in the upcoming weeks.

[Airwide]

This pull request resolves issue #6073.

This commit has two new variables nav_fw_pitch2thr_smoothing and nav_fw_pitch2thr_threshold, which together with nav_fw_pitch2thrshould make it possible to tune in a stable throttle for maximum efficiency during wp missions and still keep the airspeed stable at different targeted pitch angles.

Usage

Latest commit has 2 new variables:

1. nav_fw_pitch2thr_smoothing

This variable holds the number of fixedWingPitchToThrottleCorrection cycles used to calculate a moving average for the pitch angle which is used for the pitch to throttle correction. Set nav_fw_pitch2thr_smoothing to a value between 1 and 4096. To keep the smoothing calculation efficient, choose a value for nav_fw_pitch2thr_smoothing that equals a power of 2 (1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096). 1 is default and keeps current behaviour with no smoothing.

nav_fw_pitch2thr_smoothing is also accessible in the OSD menu under features/navigation/fixed wing/.

2. nav_fw_pitch2thr_threshold

This variable creates a deadband of +/- nav_fw_pitch2thr_threshold around the average pitch.
Set nav_fw_pitch2thr_threshold to a value between 0 and 100 centidegrees. That equals 0 to 10 degrees. 0 is default and keeps current behaviour.

nav_fw_pitch2thr_threshold is also accessible in the OSD menu under features/navigation/fixed wing/.

Testing

I'm currently grounded, and haven't been able to test this latest commit. I should be up in the air in the next week or so. Meanwhile, help with testing and feedback would be much appreciated. Let me know if you have questions.

[kniuk]

@Airwide you are refering to centidegrees (1 degree = 100 centidegrees), however I guess you meant decidegrees (1degree = 10 decidegrees)

[Airwide]

@Airwide you are refering to centidegrees (1 degree = 100 centidegrees), however I guess you meant decidegrees (1degree = 10 decidegrees)

@kniuk You are correct! 👍 I've edited the first post. I'll post a commit eventually too.

Further work needed

[Airwide]

This PR has been changed after feedback from @digitalentity.

• A low pass filter has replaced the moving average. Setting nav_fw_pitch2thr_smoothing between 0 and 9 adjust the cutoff frequency between NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ = 2 Hz and 0.1 Hz according to the following:

  • nav_fw_pitch2thr_smoothing = 0: 2.1 Hz
  • nav_fw_pitch2thr_smoothing = 1: 1.6 Hz
  • nav_fw_pitch2thr_smoothing = 2: 1.1 Hz
  • nav_fw_pitch2thr_smoothing = 3: 0.79 Hz
  • nav_fw_pitch2thr_smoothing = 4: 0.53 Hz
  • nav_fw_pitch2thr_smoothing = 5: 0.35 Hz
  • nav_fw_pitch2thr_smoothing = 6: 0.23 Hz
  • nav_fw_pitch2thr_smoothing = 7: 0.15 Hz
  • nav_fw_pitch2thr_smoothing = 8: 0.12 Hz
  • nav_fw_pitch2thr_smoothing = 9: 0.10 Hz

• Changed to full throttle compensation outside of the deadband defined by fw.pitch_to_throttle_thresh.

• Default values of fw.pitch_to_throttle_smooth = 0 and fw.pitch_to_throttle_thresh = 0 keeps current behaviour.

@digitalentity , @avsaase , @kniuk , @b14ckyy WDYT?

I plan to head out and test tomorrow, if the weather is ok.

[avsaase]

Awesome!

I think this way of filtering the pitch angle is better, and the configuration much easier. I do wonder if we still need the threshold and if we can't just apply the filter to all pitch angles. As long as filteredPitch is not too sluggish and cruise throttle is set at a safe level I don't think that will ever cause problems. The threshold also causes a weird kink in the throttle response when climbing or diving at an angling outside of the threshold, for example clinbing at 10 degrees when nav_fw_pitch_to_throttle_thresh = 30:

This plot still uses averagePitch instead of filteredPitch, but I think the results would be similar. As filteredPitch is catching up to pitch, the difference becomes smaller than the threshold so there is an abrupt changes to using filteredPitch instead of pitch for the throttle correction calculation. Since filteredPitch has not yet fully caught there is a drop in the throttle output. I think any use of an if statement will always cause some sort of kink in the throttle response so I propose to just get rid of it. If @digitalentity doesn't want that an alternative would be to increase the max value for pitch_to_throttle_thresh to 900 so that informed users can disable it themselves.

Weather permitting I will test this weekend. What would be a good starting point for nav_fw_pitch_to_throttle_smooth?

PS.: GitHub indicates some line changes, but all that is different is the line ending?

[Airwide]

@avsaase I'm aware of the weird kink, and that's the reason why I implemented it the way I did in the first place with partial compensation outside the deadband. However, I think @digitalentity has a valid point about full compensation to avoid the risk of stalls. Let's try this version and see if the kink is a problem in reality.

Interesting that you are thinking about a not too sluggish filter. I'm thinking the other way around to get a really stable throttle. In fact, I'm not sure nav_fw_pitch2thr_smoothing = 9 which equals 0.10 Hz is low enough. I'm thinking of altering the cutoff frequency setting to enable an even lower frequency according to the below graph:

I could increase nav_fw_pitch2thr_threshold maximum value at the same time to facilitate testing of a wide range of different setup configurations. Once we find a good default setting, we could narrow down the interval if necessary.

WDYT?

Btw, I went out for testing yesterday. Both my Dart 250 and my Mini Talon sky rocketed at the start of the test mission, so I had to abort the testing. I found out later that I had a 0 too many in the altitude value for all the waypoints. 👎 I will head out again as soon as the weather permits.

[Airwide]

PS.: GitHub indicates some line changes, but all that is different is the line ending?

Sorry about that. I have automatic removal of line end whitespace at save in one of my code editors.

[avsaase]

@Airwide I am not too familiar with low-pass filters so I can't really comment on sensible cutoff frequencies. I think the slower the filteredPitch, the longer the kink in the throttle curve so that is becomes more noticeable.

[digitalentity]

Looks good! Did you have a chance to test it yet?

[Airwide]

@digitalentity I did get one flight in yesterday with the Dart 250 before the rain. I used the following settings:

nav_fw_pitch2thr = 22
nav_fw_pitch2thr_smoothing = 6
nav_fw_pitch2thr_threshold = 50

Throttle correction was smoothed a bit. Immediate full correction outside of deadband worked as expected. No signs of the "kink" during transitions by looking at the dvr footage.

I'm going to push another commit with an increased span of cutoff frequency beyond 0.1 Hz and also open up nav_fw_pitch2thr_threshold max value to give @avsaase a chance to test his ideas.

My next opportunity for testing is Tuesday if I'm lucky. I plan to test with much more smoothing and hopefully both with the Dart250 and the Mini Talon.

@kniuk, @b14ckyy - I could build hex files for you, if you are interested in helping out with testing.

[digitalentity]

I'm going to merge this now. I think we can keep the settings as-is - setting pitch_to_throttle_thresh to a large value (900) will effectively always apply the LPF which should be reasonably safe in most cases. This needs to be tested, but it's not a blocker for this PR. Thanks for looking into this feature!

[Airwide]

@digitalentity I'm sorry, I just realized I did a mistake in the last commit. I accidentally changed getSmoothnessCutoffFreq() function instead of getPitchToThrottleSmoothnessCutoffFreq function. I made another commit to fix it, but the PR is already merged. Do I need to file another PR?

Edit: Bug fix created. See #6292

[avsaase]

Maybe we should have some documentation on this feature? In fact, when we find sensible values I think it should be considered to enable it by default (and/or add it to the configurator). There are already several features that make INAV fly better but are hidden in the CLI where most inexperienced users will never find them. In my opinion this shouldn't be one of them.

[Airwide]

@avsaase Agreed! Let's try and find a good basic setup together and then change the default value in a separate PR.

Btw, I did get a test flight in this morning with my Mini Talon. I must say it is working good.

Settings:
nav_fw_cruise_thr = 1400
nav_fw_control_smoothness = 0
nav_fw_pitch2thr = 10
nav_fw_pitch2thr_smoothing = 9 // Note that this was with the old smoothing calculation where 9 equals 0.1 Hz
nav_fw_pitch2thr_threshold = 50

Here are two screenshots showing the behaviour with pitch changes outside of the deadband.

I want to test with an even lower frequency. It would also be interesting to combine these settings with somenav_fw_control_smoothness. I think that could be an alternative to increasing nav_fw_pitch2thr_threshold.

[avsaase]

I finally found the time and the weather to test this, and works very good! My setting were:

nav_fw_cruise_thr = 1250
nav_fw_pitch2thr = 10
nav_fw_pitch2thr_smoothing = 4
nav_fw_pitch2thr_threshold = 50
nav_fw_control_smoothness = 4 // I didn't know I had this setting enabled

This is with the implementation in RC3. Is that with the new smoothing curve? Not that it makes a big difference with these settings.

I think these settings for nav_fw_pitch2thr_smoothing and nav_fw_pitch2thr_threshold are generally safe to use and won't cause problems, so I propose making them the new defaults.

[Airwide]

This is with the implementation in RC3. Is that with the new smoothing curve?

@avsaase Yes, RC3 has the new smoothing curve.

I think these settings for nav_fw_pitch2thr_smoothing and nav_fw_pitch2thr_threshold are generally safe to use and won't cause problems, so I propose making them the new defaults.

Your suggestion for default values is quite conservative, which can be a good thing. However, I would consider nav_fw_pitch2thr_smoothing = 6 together with nav_fw_pitch2thr_threshold = 50 for defaults.

@digitalentity WDYT?