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

Revised multicopter navigation acceleration attenuation settings #9421

Open
wants to merge 8 commits into
base: master
Choose a base branch
from

Conversation

breadoven
Copy link
Collaborator

@breadoven breadoven commented Oct 28, 2023

Changes the way settings nav_mc_vel_xy_dterm_attenuation_start and nav_mc_vel_xy_dterm_attenuation_end work to better control how speed attenuates multicopter acceleration during navigation. Intended to overcome a problem of speed hunting around the target speed which seems to affect some pilots whereby the multirotor significantly overspeeds then decelerates abruptly for a number of oscillations before eventually settling at a more stable flight speed. Changing the more obvious PID settings doesn't seem to help overcome this problem.

The above 2 settings now work based on absolute fixed start and end speed limits rather than start and end speed percentage limits set relative to the maximum speed used for a navigation leg. This results in consistent attenuation independent of the actual max speed set for a specific navigation leg.

Limited flight testing appears to show the speed hunting is prevented with this change with the target speed approached smoothly with little overshoot.

@Jetrell
Copy link

Jetrell commented Oct 29, 2023

I ran some tests with nav_mc_vel_xy_accel_attenuatefirst set to 50, then 25 and finally down to 10.
Reducing the setting and attenuating the velocity PIDs, seemed to hit a wall between 50 and 25.

But there was an improvement with it activated.... It didn't bob up and down as harshly as before.. Although the issue is still they. But the slowdown pitching action is now smoother.

The times the effect was worse, was obviously with a tailwind. Because its easier to over-speed the set point.

As a side note.. I have noticed that the wind estimator is rather accurate in multicopter use.. The speed value does not show on the OSD.. But the wind direction arrow is very precise.
I wonder if this could be used in some way to filter the slowdown pitching action. And just let it over-speed a little.

I'lll be interested to see how much difference #9387 makes in assisting navigation.

@breadoven
Copy link
Collaborator Author

I thought it seems to be better mainly because it doesn't over brake after over speeding. It still over speeds but not as badly as before but then drops back to the target speed with little undershoot. The lurching pitching back behaviour around the target speed is the worst part of the current behaviour. One issue I did notice is the 10 setting seems to slightly undershoot the target speed, I'm guessing because the gain of the I term is reduced although you'd think this would eventually catch up with itself. I also thought it made PosHold less nervous ?

Definitely better but I still can't help but think it should be perfectly possible to accelerate at max acceleration until close to the target speed at which point it smoothly backs off and creeps up on the target speed without over speeding at all. Multirotors seem to be able to respond quickly enough to be able to do this so it should just be a matter of finding the best way of achieving the required control. Maybe it would be better to target a rolling target speed that increments up the final target speed, might provide smoother more controllable acceleration.

@shota3527
Copy link
Contributor

I think D gain and air resistance should have already attenuated the acceleration.
If attenuate the pid. the tracking of the continuously changing velocity target will deteriorate.

In my theory, this PR could cause lag on the estimated velocity #4681 lag in the position estimator causes things like overshoot.
acc data is set to 0 in position estimation when clipping is detected in the accelerometer. The accelerometer value set to 0 equals velocity/position by the accelerometer is always 0.
Clipping detection also has problems, it is set to 7.9G on the raw accelerometer value(with vibration), compared to16G scale range on most of the gyros. A typical 5-inch quad can achieve 8G acceleration in a throttle punch.

We should have a look at the GPS velocity of m10/m8 GPS vs position estimator velocity first.

@breadoven
Copy link
Collaborator Author

breadoven commented Oct 31, 2023

Looking at the dynamic dTerm scaling used by the acceleration controller, the default settings reduce D gain down to only 10% for any speed > 60% of the target speed ... so the D term is doing next to nothing as you approach target speed. This is supposed to smooth things at higher speeds. Guess it's worthwhile playing with these D term attenuation settings, nav_mc_vel_xy_dterm_attenuation etc, to see if changes from defaults help with the speed oscillation problems. Did you try this already @Jetrell ?

@Jetrell
Copy link

Jetrell commented Oct 31, 2023

@breadoven I remember adjusting the vel_xy_dterm_attenuation and its start and end settings two years back, when Julio was trying to track down the cause of MC braking runaway... But I couldn't notice any significant difference when changing them, even on the log traces.
There wasn't much overhead on nav_mc_vel_xy_dterm_attenuation anyway.. Meaning that all it seemed to do, was make Poshold sluggish in CRUISE flight control mode.

But as Shota implied. GNSS hardware/software technology has improved within that time. But INAV's sensor fusion filtering methods are practically 5 years old.

@Jetrell
Copy link

Jetrell commented Oct 31, 2023

In my theory, this PR could cause lag on the estimated velocity #4681 lag in the position estimator causes things like overshoot. acc data is set to 0 in position estimation when clipping is detected in the accelerometer.

@shota I think you may have something.. Considering CRUISE control mode is constantly transforming stick command inputs to speed, from the body-frame to earth-frame.. Which requires fast updates from the estimator.

@Jetrell
Copy link

Jetrell commented Nov 2, 2023

@breadoven I attempted to tune the old nav_mc_vel_xy_dterm_attenuation again today..
Even using @shota3527 yaw estimation sensor fusion code.
And the outcome was the same. As with this PR.. MC Cruise mode and Poshold in CRUISE/control mode still experiencing the same speed management jumping issues.

@breadoven
Copy link
Collaborator Author

breadoven commented Nov 2, 2023

@Jetrell I'm not sure we're talking about the same issues somehow. The main problem I found, the the reason for this PR, was during the initial acceleration phase where the MC would over speed significantly then decelerate excessively in a series of oscillations that would repeat for way too long before finally settling down to a more consistent speed. It was this see sawing back and fore in pitch that was the issue for me.

I think you might be referring to another issue I noticed testing recently where once the initial acceleration has settled down you get a speed glitch. The speed drops slightly followed by an abrupt accel/pitch correction. In my case this is happening every 5s. I'd imagine it gets worse as speed increases and when I think about it I reduced speeds during WP because it seemed to reach a point where things became quite unstable. Is this what your problem is ? The 5s is very consistent. Almost looks like something accumulating then resetting. It's not I term as far as I can see so something in the position estimator affecting the speed, but 5s is an odd timing ? You can see it in the attached log (debug 2 is fwd accel, debug 3 is pitch deci degs).

Did you test with nav_mc_vel_xy_dterm_attenuation = 0 ?

Screenshot (173)

@Jetrell
Copy link

Jetrell commented Nov 2, 2023

I've notice both cases.
But yeah, the second one is the most pronounced.. I find it happens on my quads in the lower and mid speed ranges. And occasionally I hit just the right speed. that it constantly pitch up and down, bouncing off the speed setpoint.. And won't stop, until I either pitch forward more to speed up, or pitch back to slow the speed a little.

I always have nav_auto_speed and nav_max_auto_speed set to 72km/h.. And have nav_mc_bank_angle set to around 30degs for good forward velocity in navigation modes.
So when the stick is pushed fully forward, to make that bank angle.. I rarely see this issue. Unless their is a strong tailwind.

No. I didn't take nav_mc_vel_xy_dterm_attenuation = 0.. Only test ever 10 points, in its range.

@breadoven
Copy link
Collaborator Author

I tested the other day with nav_mc_vel_xy_dterm_attenuation = 0 ... and the over speeding issue on initial acceleration disappears, at least up to the 18 km/h speed the WP mission was using. It gets close to the target speed then backs off and slowly creeps up on it which is exactly how I'd want it to behave. Maybe it works better at higher speeds which might be the problem here. nav_mc_vel_xy_dterm_attenuation uses the max speed set for the Nav mode, e.g. WP mission leg or Cruise mode speed, and applies the attenuation based on that speed regardless of whether it's a high or low speed setting. I'd have thought it should only become active above a specific fixed speed where nav_mc_vel_xy_dterm_attenuation is required. Below that speed it should remain inactive.

There is still this odd 5s speed glitch but that's to do with something else I'm sure. Would be interesting to understand what's causing it because it's on a perfect 5s cycle which doesn't tie in with any obvious timings I've managed to find.

@Jetrell
Copy link

Jetrell commented Nov 12, 2023

I tested the other day with nav_mc_vel_xy_dterm_attenuation = 0

I can confirm that.
But in the case of my quads. Speeds up to 20km/h are lucky to be 10deg of forward bank angle.. So it shoots past that speed range so quickly, that I hardly notice it. And moves straight in to the mid range over speed issue..
All my quads cruise comfortably between 35 to 70km/h.
Out of interest, what is your nav_mc_bank_angle set too ?

I'd have thought it should only become active above a specific fixed speed where nav_mc_vel_xy_dterm_attenuation is required.

I suppose the issue is. How does it calculate air resistance and wind direction.. I'd say it doesn't. And without these factors. Its just based on CoG, and the way vel_xy reacts to stick input, and appears to feed in some pitch bank angle. Then waits to see what happens.
But the issue also seems to be based on nav_mc_bank_angle.
From what I can perceive. It just keeps blindly using the bank angle range with in that constraint. Instead of saying.. We have rapidly over shot the speed set point. Limit the bank angle, in which the velocity controller can command, within a speed deadband, until more or less stick is applied.. Otherwise with a tail wind, it ends up being a closed loop condition that the velocity controller can't deal with.

@breadoven
Copy link
Collaborator Author

Out of interest, what is your nav_mc_bank_angle set too ?

Set to 30 degs in my case. I don't think the bank angle is really the issue. It should start to back off the acceleration and therefore bank angle before it reaches the target speed ... which is what it did with nav_mc_vel_xy_dterm_attenuation set to 0. You might bounce off the bank limit when initially accelerating but if you're bouncing off the bank limit and not achieving the max speed that's set then clearly your bank limit isn't high enough or the set speed is too ambitious.

As far as setting some cut off speed for nav_mc_vel_xy_dterm_attenuation is concerned I'd think you can only set some compromise speed that works in most situations that would still be acceptable for normal up/downwind performance ... unless you do decide to factor in wind corrections obviously.

@Jetrell
Copy link

Jetrell commented Nov 13, 2023

It should start to back off the acceleration and therefore bank angle before it reaches the target speed.

True... But we have to look at how it backs off the acceleration.. It can't back off the throttle by more than a few percent, or the quad will loose altitude.
So the only way left is to pitch the quad back closer to level. to reduce forward velocity.
So when the speed is holding the setpoint target.. The vel_xy controller is constantly telling the pitch bank angle to change to hold setpoint. Which can be constantly changing with wind speed.

You might bounce off the bank limit when initially accelerating but if you're bouncing off the bank limit and not achieving the max speed that's set then clearly your bank limit isn't high enough or the set speed is too ambitious.

I'm not referring to initial acceleration.
This clip is an example of the bouncing effect.
When the target speed is set by the stick, I experience induced bouncing anywhere between 20 to 50km/h. Depending on the wind direction and speed. And it also effects the roll axis too, if you watch carefully.

You can see, the only time it stops, is at the 13 second mark, when I push the stick fully forward. To allow nav_mc_bank_angle to be meet (30deg). And nav_max_auto_speed to be the target (72km/h).. And the only reason it then fly smoothly. Is because the quad can't make the ground target speed with the prevailing head wind.

Cruise.bouncing.off.target.speed.mp4

The vel_xy controller is doing what IT should... But its the changing of bank angle to alter velocity, that is purely un-aesthetically pleasing from an FPV perspective...
And the issue is obviously worse on lightweight powerful quads.

I'll go on to say. That I have the nav_mc_vel_xy controller tuned tightly on all my quads. So it pulls up spot on the home location in an RTH. Or precisely stops on a WP PH marker.. This tuning prevents over shoot by 10s of meters if the quad is traveling at good speed.. Which often occurs with the stock vel_xy setting.
This is how the velocity controller should work.. BUT, if I de-tune nav_mc_vel_xy_p, it can make a reasonable improvement to the Cruise mode bouncing issue. Because its effectively making the acceleration response to the speed target change very sloppy.
But why would I do that. When it then makes every other function that uses that controller work badly.

@Jetrell
Copy link

Jetrell commented Dec 3, 2023

@breadoven I hadn't heard back on your view of the previous post. Agree or disagree with my assessment.. Or just not sure of what direction to take for a fix.

I've talked to Shota and to Julio, who is working on an altitude acceleration controller that I've been alpha testing for him.
But one thing I mention to Shota. Which I believe is relevant to this issue. Is the way the velocity controller appears to be used in multiple ways. And for a variation of different purposes.
e.g. It is used to control XYZ target response to stick input, and also for navigation targeting.

But the issue I see with the problem we have here.. Seems to be the same issue that arises for the stabilization PID controller, at different air-speeds.. i.e. We can't tune the controller to work at its best in two different airspeed ranges.
Which is the same issue the velocity controller experiences in the different ways its called to be used.
e,g. We can't have it tuned tightly in cases when small adjustments are needed. And we can't have it tuned loosely, when larger changes are experienced between the Actual and the Target.

The example I gave to Shota. Was surely the ABS braking and Auto Cruise control, PID controllers in a motor vehicle, are based on the same controller, but with a different tune according to the purpose.
Being that both can see a large window of operation. But one will see the need for a higher P/D response, with little I-term (braking).
While the other will require a lower P/D response with a higher I-term (Cruise control).

What do you think ?

@breadoven
Copy link
Collaborator Author

What do you think ?

I think this is probably mixing up a number of different issues which may or may not be related. I agree that sometimes the same PID controller is used to control target conditions that aren't best matched to its use which causes problems. It's part of the reason for using velocity for fixed wing altitude control ... it seems to be better suited to the Nav PID controller than using a rolling Z position target.

I tend to think the bobbing behaviour you show in the above video is related to this glitch you can see in the logs which seems to occur every 5s. The cyclic nature of this says it's nothing to do with wind related effects but something in the code which accumulates some error that resets every 5s. It could be the estimated speed that unexpectedly drops or the pitch reducing due to some PID effect I guess, although I seem to remember it looked like it was the estimated speed that was the issue. This probably only happens when it's achieved the target speed and is trying to maintain it because the speed glitch is within the controllable speed range whereas when the target speed can't be achieved the speed glitch is simply outside the controllable speed range so it doesn't affect the pitch angle which remains at max demand.

For this particular PR I think it would be better to just attenuate the D term based on absolute speed limits rather than percentages of some max speed (that can be completely different depending on the flight mode), i.e. no attenuation up to a set speed then attenuation increases up to the max allowed at another specific speed. So similar to the TPA control. This should improve the acceleration response during speed changes but I doubt it would help with the bobbing when trying to maintain speed because I think that's a different issue that needs a different fix.

@breadoven breadoven changed the title Multicopter navigation acceleration attenuation setting Revised multicopter navigation acceleration attenuation settings Apr 21, 2024
@breadoven
Copy link
Collaborator Author

breadoven commented Apr 22, 2024

I reworked this PR to change how nav_mc_vel_xy_dterm_attenuation_start and nav_mc_vel_xy_dterm_attenuation_end work. Rather than setting a percentage of the set Nav speed they instead set fixed values of 2D speed as mentioned in my last post above.

So nav_mc_vel_xy_dterm_attenuation_start sets the speed when the attenuation begins and nav_mc_vel_xy_dterm_attenuation_end sets the speed when attenuation reaches its maximum (set by nav_mc_vel_xy_dterm_attenuation). These speeds are the same regardless of the Nav speed set for the navigation leg unlike previously where the attenuation could reach max even at low speeds if the max speed set for the navigation leg was set low.
NOTE:
nav_mc_vel_xy_dterm_attenuation_start and nav_mc_vel_xy_dterm_attenuation_end have been changed to nav_mc_vel_xy_dterm_attenuation_start_speed and nav_mc_vel_xy_dterm_attenuation_end_speed to avoid setting crossover issues between versions.

Testing for WP missions and RTH the other day showed the speed oscillations I was getting before don't happen any more. There is pretty well no speed overshoot (there was minimal deceleration showing from debugging confirming this). It was also possible to fly at higher speeds, speeds which you avoided before because it caused such bad speed oscillations around the target speed.

Not sure this has fixed the other 5s cyclic speed glitch mentioned above, it wasn't obvious from the OSD DVR, need to check the log in detail to see if it's still there.

Do you want to test this @Jetrell, see if improves the bobbing issue you were having ?

@Jetrell
Copy link

Jetrell commented Apr 22, 2024

@breadoven I seen your changes yesterday. And ran some tests in Cruise mode today... But I wasn't going to report yet, because I don't have any conclusive information.
Because it seemed to bob up and down worse over the whole speed range in Cruise mode, on both the pitch and roll axis's.. But this quad had just been rebuilt after a crash.. And I haven't re-tuned it after I replaced many components.
So I'm not sure how much was a poor tune issue, or how much was the new vel_xy_d attenuation method.
I'll keep you informed over the next week.

@breadoven
Copy link
Collaborator Author

breadoven commented Apr 22, 2024

Well you should be able to simulate how the quad would have performed with the old settings by adjusting the settings for this PR to give you the same attenuation as you'd have with the old settings. E.g. if you fly a WP leg at 10m/s, with the old default settings, attenuation would start at 1m/s and reach max attenuation at 6 m/s. So you could set those speeds for the settings in this PR and it should behave the same as the old settings, certainly up to the point max speed is reached.

The testing I did used the following settings for a 5 inch quad:
nav_mc_vel_xy_dterm_attenuation_start_speed = 7
nav_mc_vel_xy_dterm_attenuation_end_speed = 15
nav_mc_vel_xy_dterm_attenuation = 90

I tested it up to 13m/s so it wouldn't have reached max attenuation although in fact I noticed an error which would have reduced the attenuation further so the quad I was using seems to prefer Dterm being active at speed rather than the opposite. I'll test again with the error fixed.

@Jetrell
Copy link

Jetrell commented Apr 24, 2024

With the latest commit, I tried it again with different size quads, and altered the start and end attenuation speeds to gauge any difference in effect. But it still encounters the same bouncing issue.

The only times it wasn't a problem, was on the headwind legs of the mission. When the copter was lucky to make it too the WP speed value at nav_max_bank_angle, so it never had to reduce the bank angle to slow down.
But once it turned around and followed a down wind leg of the mission, it then caused the copter to bounce on the pitch and a little on the roll, as it was trying not to overshoot the WP speed value.

I admit it seemed a little more tame sometimes. But other times it seemed just the same.

@Jetrell
Copy link

Jetrell commented Apr 25, 2024

@breadoven I'd like to layout some idea's and observations.

Speed based vel_xy attenuation of the d-term only stops fast bouncing. Because the vel_xy_p is still pushing hard enough to cause this effect as the target velocity is altered by air resistance.

As it is. The nav_mc_vel_xy controller is primarily tuned for absolute target velocity. Which is good for position deceleration. But not much good for maintaining an approximate target velocity, based on the changing variable of the wind pushing the copter.

How do you think it would go if the vel_xy P, D and FF are attenuated under these conditions ?

  1. When the target speed is set, and within a percentage of that speed.
  2. Apply a deadband in the shape of a bell curve, that ramps up the attenuation on one side of the target speed, and ramps it
    down on the other side.
  3. Then holds the attenuation deadband until the target is altered by Stick input, WP navigation speed or
    nav_max_auto_speed ?

This should reduce the bouncing effect on the pitch axis, by reducing the overall velocity PDFF response. And allowing it to drift either side of the speed setpoint.

Ideally, multicopter wind estimation would also make a big difference to this.

@breadoven
Copy link
Collaborator Author

@Jetrell The D Term attenuation was brought in by #5338. Unfortunately it doesn't explain what it was trying to fix exactly and the videos in the PR are no longer available.

As I said above I think there are 2 different issues here:

The issue I was trying to fix is the over acceleration and significant speed overshoot you get as the quad initially accelerates(decelerates ?) to the target speed followed by a speed undershoot as it tries to decelerate back to the target speed. This only affects the initial acceleration and usually the speed does settle down eventually to the target speed but the instability whilst it's doing it is excessive and shouldn't happen. This is pretty well eliminated by this PR although it also doesn't happen if you simply turn off the current D Term attenuation by setting nav_mc_vel_xy_dterm_attenuation to 0. This tells me D Term is required to avoid this initial speed overshoot which is what you'd expect surely because that is what D Term is supposed to do, limit overshoot and oscillation. The latest change to this PR is intended to allow you keep D Term attenuation but just make it work at fixed higher target speeds, where maybe it's useful, rather than have it active even at low target speeds where it's not required. The following log shows a test for this PR with acceleration up to 13m/s and there is no speed overshoot. I think D Term attenuation was around 30% at that speed for the settings used. I'm pretty sure the speed dip you see in the log just before max speed was reached is related to this speed glitch/nodding/bouncing issue you seem get as mentioned below, don't think it's PID related.
Screenshot (233)

The other issue is the pitch nodding/bouncing motion you mention which I think is caused by something else because in my case it occurs on a 5s cycle as you can see in the following log. I don't know what's causing this, I find it hard to believe it's PID related given the cyclic nature of it. The speed and pitch look pretty stable until the glitch at which point the speed suddenly drops causing the PID loop to increase pitch leading to a slight overspeed with the pitch then dropping back lower than before this happened before recovering back to where it was before the glitch. This will appear as a "nod" in the camera view. This happens every 5s. Could have sworn I thought I found it was coming from the GPS speed glitching but can't find where I mentioned it before. So basically INAV thinks the speed is dropping and "nods" the pitch to compensate but it's potentially reacting to a false speed input caused by some glitch in the speed estimation. Can't be wind related or it would occur randomly. And I just checked fixed wing logs, the same speed glitch is there, every 5s. So find the issue with the speed estimation and you'll perhaps fix this nodding bouncing problem.

Screenshot (232)

Debug 3 is demanded pitch.

@breadoven
Copy link
Collaborator Author

breadoven commented Apr 25, 2024

Just checked a log that shows this speed glitch and GPS speed doesn't drop at the same time so this doesn't appear to be GPS related. I also noticed the glitch isn't always there, mostly it is but sometimes it disappears even at the same higher speeds where it was very obvious in other parts of the log. Weird ... and probably going to be difficult to work out what's causing it.

@Jetrell
Copy link

Jetrell commented Apr 25, 2024

The issue you are aiming to fix here. Is only something that I've seen occur on quads that weigh in over 450g or so.
While I never see it occur on the lighter builds 300g and less. While the results are practically the opposite with the second issue.

Its the quads above 450g that seem to bob less often with the second issue, but its still noticeable from time to time when the actual speed reaches the target speed.
But the lighter quads just go mad in their pitch bouncing, when the actual speed gets near the target speed. As seen in the video I posted above last year with a 3" quad. (I've also tested that with my other sub 300g quads). They all pitch up and down a couple of times a second.. So inertia and air resistance likely play a part in the effect.

And from looking over logs. Its noticeable to see how this fast pitch bouncing causes GNSS horizontal position deviation. And upsets the IMU. Which in turn effects attitude and heading reference in Waypoint mission that uses speed set points at each WP marker.
So it may also be causing a feedback loop of sorts as well, because of reduced data accuracy.

@breadoven
Copy link
Collaborator Author

breadoven commented Apr 26, 2024

@Jetrell Maybe it would help to clarify what the issues are here:

D Term attenuation - is this to fix pitch instability at higher speeds or at any speed around the target speed (even at low speed) ? This PR is assuming it's more a high speed issue whereas the current D Term attenuation is based on it occurring at any speed even low speed, hence the reason it's based on a percentage of the target speed.

Pitch bouncing issue - is this the same as the D Term issue or something different ? I assume it's something different given you've played with D Term attenuation settings and it doesn't sound like it helps. Looking at the video above again it does seem like it's bouncing on a >3s cycle so could be related to this speed glitch. You'd need to look at the logs to get a better idea, see if it's on a constant cycle or more random.

Overspeed with D Term attenuated - I'm surprised others haven't mentioned the overspeed instability you get during initial acceleration when the current D Term attenuation defaults are used (also happens when decelerating especially to a stop). The pitch bouncing when at target speed is a minor irritation compared to this. The 5" quad I was testing with flies better with the attenuation turned off. I need to test at higher speeds to see if there are problems the faster you go, something I avoided before.

I did also notice this speed glitch is absent in HITL flight logs but occurs in real flight logs made with the same firmware loaded which suggests it's related to data the FC is receiving from the simulator, GPS data perhaps.

@Jetrell
Copy link

Jetrell commented Apr 26, 2024

D Term attenuation - is this to fix pitch instability at higher speeds or at any speed around the target speed (even at low speed) ?

At any speed around the target speed. But the actual instability it is causing is due to the velocity controller commanding an unrealistic speed set point window. (1km/h or less)
From tests with 3 different size sub 300g quads. I have seen this occur from 4km/h, all the way up to 71km/h with a tailwind, just below nav_max_auto_speed's maximum allowable set point.
Which effects every navigation mode that uses rolling velocity targets. i.e nav_user_control_mode = Cruise, Cruise flight mode and WP speed targets.

Looking at the video above again it does seem like it's bouncing on a >3s cycle so could be related to this speed glitch. You'd need to look at the logs to get a better idea.

I've looked over many of these logs. The pitch base speed regulation (bouncing) can occur anything from a couple of times a second, to once every few seconds. depending on how much speed regulation is required to hold the target speed.
When looking over all the DVR footage of this issue. These things can be noticed when

  • Comparing the actual speed, to the target speed. Along with the pitch and roll axis elements.
  • Pitch and Roll bounces occur at any interval . Based solely on when the actual speed over or under shoots the target
    speed
    .
  • Also the pitch attitude can also change from a couple of degrees, to tens of degrees. Because it doesn't like speed
    over/under shoot of the target, even by fractions of a kilometer.
  • And how the quads flight becomes smooth when the target set point speed can not be reached at nav_mc_bank_angle with
    a prevailing headwind. So its no longer trying to regulate its speed via the pitch angle.

The copters Thrust at a specific bank angle, Mass and Air resistance all play a part in how easily it can loose speed, and obtain it again, when pitch angle is adjusted by way of the velocity XY controller... And the tighter the tune on, vel_xy FF and P-term, the harder and faster it will push to meet that target speed, regardless of the d-term. Which is evident from the logs.

I have reduced both the velocity FF and P-term.. And it helped alleviate much of this effect over the whole speed range. Which is why I recommended something like this #9421 (comment)
But just de-tuning those overall gains provided absolutely awful performance when the copters were approaching a fixed target location (RTH home) at speed.. It would overshoot by 20 or 30 meters, and then back up to the target and over shoot again! While if tuned correctly those gains pull it up within 10m.

I did also notice this speed glitch is absent in HITL flight logs but occurs in real flight logs made with the same firmware loaded which suggests it's related to data the FC is receiving from the simulator, GPS data perhaps.

From the use of MC braking mode and nav_user_control_mode = Cruise used with Poshold. If higher bank angles are commanded by the user, and the copter gathers some speed, then the user releases the stick. The copter will temporarily runaway for 10's of meters, before the GNSS speed finally halts it.
Although its hard to see it in the logs.. I firmly believe its caused by the accelerometer/ gyro axis data conflicting, and two axis's becoming equal to1g for a time as the copter starts to tilt backwards and slows to brake. Similar to what used to happen with the old AHI drift, but inverse.
But in this case, because it happens so quickly, GNSS velocity can not be used for AHRS correction at the instant its required.

@breadoven
Copy link
Collaborator Author

It doesn't sound like this PR is likely to work if this bouncing issue isn't related to flying at higher speeds, but affects flying at any target speed. The testing I've done just shows that the quad I've been using works fine with D Term attenuation OFF, the only issue being this slight speed glitch/pitch bounce you get on a fixed cycle which I don't think is PID related given it also affects fixed wing.

I'll do some more testing at higher speeds, see if more obvious pitch instability problems arise. Other than that it would be useful to work out where this speed glitch is coming from because fixing that would at least probably help in general.

@cptX
Copy link

cptX commented Aug 30, 2024

Hi guys, just discovered this issue after having exactly the same problem in my 5" quad. Specifically I fly 8 km autonomous flights (4km and back). I'm on vacations in my village in Crete and the wind here is constantly 4-5bf with gusts up to 7bf. The going phase has the wind at the rear right of the quad. The first minute of the flight has a lot of oscillations at the pitch axis. After a while it stabilizes. The returning has the wind at the front left of the copter and is absolutely stable.
I have posted this issue and the video in the official INAV facebook page, but the answers were completely off topic.
You can check it here https://www.facebook.com/groups/INAVOfficial/permalink/1937783719997603/

I'm glad that I found this issue here. I have started playing with the nav_mc_vel_xy parameters and I saw great enhancement but I think it affected other parts of the flight, so I'm not sure if this is the way to go. But I still don't get why we should try solving the issue with the nav_mc_vel_xy_dterm_attenuation. I still have the feeling that good nav_mc_vel_xy parameter tuning could solve the issue. I actually have set the capability to tune the mc_vel_xy p, i, d during flight from my controller. This has helped a lot. But the fact that these oscillations happen only at the beginning of the flight makes these tests very difficult...

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

Successfully merging this pull request may close these issues.

4 participants