inav
inav copied to clipboard
iNavFlight
Improved fixed wing waypoint course tracking
Attempts to improve fixed wing WP course tracking accuracy by adding a couple of options:
-
Tracking accuracy option that forces craft toward the waypoint course line as quickly as possible and actively tracks the line thereafter. A single setting adjusts the strength of the tracking behaviour. Works for WP mode and RTH trackback. Option is disabled by default.
-
Turn smoothing option for waypoint turns. Sets a loiter point within the turn such that the craft follows a loiter turn path that either passes through the waypoint or cuts the turn missing the waypoint (2 settings possible). Only works for WP mode. Option is disabled by default.
PR also refactors waypoint reached/missed code to combine it into a single function for WP and RTH modes + fixes existing issue where loiter WP reached logic was only applied to RTH home point rather than all relevant loiter WPs.
Flight testing shows it works well on a conventional plane with rudder using Yaw control. Also works on a flying wing but maybe not quite as effective due to the lack of direct Yaw control. Still needs more testing on bigger variety of wings though.
Test results (Red = WP mission, Yellow = Both options ON, Blue = Tracking Accuracy OFF):
(Forgot to test with both options OFF, i.e. current INAV. To follow ...)
Nice modifications... But I still think that the entire AutoPilot algorithm for fixed-wing aircraft should be redone from scratch, and a new method should be used... For example the L1 Adaptive Control (perhaps the best)
Comparison showing tracking with both options used (yellow) and both options off, i.e. current INAV (blue). Turn smoothing seems to provide the biggest benefit by reducing or avoiding the overshoot seen with tighter turns.
@RomanLut Google Earth. You can export blackbox logs to earth traces with flightlog2kml. Or you can use fl2xui with graphical interface and easier use.
@breadoven this is really fantastic. I'd be happy to test on my AR Pro that is well tuned to see how this behaves on there. If you could make a MATEKF722WPX, that would be great. (I really have to look into setting up a new build env.
I just use https://github.com/iNavFlight/blackbox-tools to generate GPS GPX files from the flight log which can be loaded/dropped into Google Earth. That's where the above images came from.
@b14ckyy You can use https://github.com/iNavFlight/inav/actions/runs/2704213376. It's the last working version that I used for a longish mission the other day.
Still think this can be improved on. I'm looking at changing the tracking accuracy method to something similar to that used when doing a loiter. This would set a waypoint slightly ahead of the current position as the target to aim for with the waypoint position being a point on the waypoint course line. Could be a waste of time, we'll see.
@breadoven I am not a navigation expert but I also have an idea how the nav planning might be done. What I could imagine is, that Inav prepares for turns in a way that it will hit the follow up leg exactly straight at the WP location. I imagine it like in that picture:
- Waypoint with plane Loiter Radius in blue (Waypoint Radius is ignored at this point, maybe we can get rid of it and use loiter radius instead?)
- Incoming Leg is A, outgoing leg to the next WP is B
- from WP there will be a Loiter Point LP created with the loiter radius (C) as distance and positioned in a right angle relative to Leg B
- The plane will then make a Preparation turn (1) to go into the loiter ring and start to follow the loiter path (2)
- EDIT: the preparation turn should be at 2x loiter radius distance from LP
- This loiter is followed until the plane's heading is within a certain threshold of the leg B direction (maybe within +/- 15°
- after that the plane follows Leg B
- if the created Loiter reference point is created on the left or right of Leg B depends on the angle between A and B if it is a left or right turn.
This process should give us pretty consistent turns that always meet the WP very close and start the new parth as straight and possible. Especially for short WP legs this can be an advantage as the short leg is started in a stable path and not affected by overshoots. It will also adopt to the turn angle at the waypoint. The shallower the turn, the smaller is the preparation torn to go into the next leg. This even allows smooth 180° turns
Just tried your branch on HIL simulator, Roman maid. In my mind, this is not correct logic to follow the cource. As you can see on image - it tried to go WP instead of going to the course Let me tell how to the real pilots fly:
- We should know the heading between 2 WP. e.g. 30°. Knowing the course and our GPS position we can calculate the distance to course (normal). E.g 100m. It means that we have to change the plane course to WP2 - 15°. When the distance to course is lower then 40m, we have to linear decrease course correction value.
- Regarding turning to the next WP. We can calculate the angle between leg1 and leg2. If we know the turning speed and our current speed we can calculate the distance to WP when we can start turning and when we will finish turning we will be precisely on the second leg
360TurningTimeSeconds = (loiter_radius * 2 * Pi) / current_speed_ms
Did some test flights today. It was a bit more complex mission as I was actually testing AHI drift stuff.
This is with 5.0 release:
This is your build with Turn Smoothing ON and Path tracking @ 50 (don't mind the bad loiter, I had bad AHI drift in this flight)
The Path correction was a bit too agressive and overshot and oscillated a bit. my I have POS_XY_P=70
I thought reducing the path tracking value to 25 would make it better but actually it got WAYYYY worse. This was with a value of 25 and the plane was all over the place so I aborted the mission. The crazy lines at the botton are from the RTH Backtrack test that was also unusable.
Here you go. Mission file and all 3 flight logs.
LOG00105.TXT LOG00106.TXT LOG00107.TXT AHI stability test.zip
@sevrugin This turn method is not a good idea. Normal navigation turns are done with a POS_XY PID loop and is limited by the fw navigation max bank angle. We never know how long a turn will take or how much radius a turn will have unless we actively control the turn radius in a safe way that can always be achieved. Depending on wind conditions the minimum turn radius will always be different depending on flight direction.
therefore, a fixed radius based on loiter radius with a safe margin and repeatability is the better option. But then your way of turning ahead of the WP is bad because depending on the angle of Leg A to Leg B the distance to start the turn will always be different and we will be further away from the WP on tighter angles.
Therefore either @breadoven's method or my method are better as with these we always fly over the WP as close as possible (Because that's the actual reason of a WP mission, to fly over the spots we mark and not past them)
@breadoven I am not a navigation expert but I also have an idea how the nav planning might be done. What I could imagine is, that Inav prepares for turns in a way that it will hit the follow up leg exactly straight at the WP location. I imagine it like in that picture:
- Waypoint with plane Loiter Radius in blue (Waypoint Radius is ignored at this point, maybe we can get rid of it and use loiter radius instead?)
- Incoming Leg is A, outgoing leg to the next WP is B
- from WP there will be a Loiter Point LP created with the loiter radius (C) as distance and positioned in a right angle relative to Leg B
- The plane will then make a Preparation turn (1) to go into the loiter ring and start to follow the loiter path (2)
- EDIT: the preparation turn should be at 2x loiter radius distance from LP
- This loiter is followed until the plane's heading is within a certain threshold of the leg B direction (maybe within +/- 15°
- after that the plane follows Leg B
- if the created Loiter reference point is created on the left or right of Leg B depends on the angle between A and B if it is a left or right turn.
This process should give us pretty consistent turns that always meet the WP very close and start the new parth as straight and possible. Especially for short WP legs this can be an advantage as the short leg is started in a stable path and not affected by overshoots. It will also adopt to the turn angle at the waypoint. The shallower the turn, the smaller is the preparation torn to go into the next leg. This even allows smooth 180° turns
I was thinking of doing it that way originally but decided it wasn't so easy to implement. However that was before I changed to using the existing Loiter method which provides that initial offset away from the turn to get better alignment when exiting. I guess it depends on how far you want the track to deviate away from the WP, i.e cut the inside corner completely, doing as this PR does now (partially cut the inside corner) or do as you suggest and stray outside the turn (much like INAV does now except on arrival rather than departing).
Also I don't think it's worth over complicating this. These are model planes that get knocked about by the wind relatively easily which throws off the perfect theoretical flight path very quickly. I'm just looking for something that largely helps to avoid the overshoot you get at the moment rather than something that makes perfect turns.
@breadoven I know what you mean. But keep in mind the current AHRS topic that could change a lot regarding flight precision. Especially with proper wind estimation. The reason why I would prefer my option is also, to be able to use an INAV plane for mapping. This option is very predictable as you just need to extend the outgoing leg (out of the mapping area) by exactly 2x loiter radius. Then the incoming leg starts right at the Mapping Area border. This will result in the following flight path and would result in perfectly straight lines over the mapping area. This kind of planning profile could be easily added to mapping tools like MWP Area planner. Your option in the opener is not as predictable as the overshoot and amount of S-Turn depends highly on the leg angle.
Just flight your mission on simulator
Cruise speed 80kmh or 22ms
nav_fw_wp_tracking_accuracy=22
nav_fw_wp_turn_smoothing=ON
changed max angle on to
courseVirtualCorrection = 1500 * courseCorrectionFactor; instead of 8000
Just tried your branch on HIL simulator, Roman maid. In my mind, this is not correct logic to follow the cource. As you can see on image - it tried to go WP instead of going to the course Let me tell how to the real pilots fly:
- We should know the heading between 2 WP. e.g. 30°. Knowing the course and our GPS position we can calculate the distance to course (normal). E.g 100m. It means that we have to change the plane course to WP2 - 15°. When the distance to course is lower then 40m, we have to linear decrease course correction value.
- Regarding turning to the next WP. We can calculate the angle between leg1 and leg2. If we know the turning speed and our current speed we can calculate the distance to WP when we can start turning and when we will finish turning we will be precisely on the second leg 360TurningTimeSeconds = (loiter_radius * 2 * Pi) / current_speed_ms
I'm not sure what the HIL simulation is doing but I know from testing that it doesn't behave that way in reality as can be seen in the above flight logs.
The idea behind this change is to get onto the WP course line as quickly as possible after flight disturbances/ turns. That's why it uses an 80 degree max convergence angle. The 80 deg angle will only apply when a good distance from the course line, it reduces rapidly as the distance decreases. A 15 deg convergence angle will take much longer to align with the WP course line which defeats the purpose of this change.
I considered using turn rates but realised they vary erratically during a turn making it a bit hit and miss. The current turn used for this change should pass through the WP but rarely does, generally falls short due to the variation in response during the turn.
@breadoven can you explain what a high or low tracking value has as an effect? I wonder why 50 was okay-ish with some snaking but 25 was uncontrollable aggressive and overshooting constantly.
I ran out of lipos to do more tests.
@breadoven Thank you for your explanation I see two possible goals The first one it to fly as clear as possible by waipoints - this way very helpfull for airophotos The second one (I need) is to fly as much economy as possible. In this way I need 15 degrrees to return to course and "start turning before wp reached" for the same reason Is it possible to make configuration variable to be able to choose the the behaviour? I tried today, but unfortunately my C is not the best =(
Did some test flights today. It was a bit more complex mission as I was actually testing AHI drift stuff.
This is with 5.0 release:
This is your build with Turn Smoothing ON and Path tracking @ 50 (don't mind the bad loiter, I had bad AHI drift in this flight) The Path correction was a bit too agressive and overshot and oscillated a bit. my I have POS_XY_P=70
I thought reducing the path tracking value to 25 would make it better but actually it got WAYYYY worse. This was with a value of 25 and the plane was all over the place so I aborted the mission. The crazy lines at the botton are from the RTH Backtrack test that was also unusable.
Seems to work best with Accuracy setting values around 40. I've used 40 on a 2m motor glider and 43 on a 600mm AR Wing. 25 is definitely too low, too much bias pushing it toward the course line hence the overshoot. There's still overshoot at 50 in your case though so maybe a higher setting is required. Flying wings are definitely not as good as planes with a rudder and yaw control on.
Having said that I'm not totally convinced by the accuracy change, not so easy to tune, not fool proof enough. Maybe some PIDification might help with tuning but I was trying to avoid that to avoid over complication. However, this is a recent flight which although it shows some wandering after turns I don't think it's significant given how accurately it follows the route otherwise even with a number of Pos Hold WPs included. From debugging it held to within 2m of the course line (sometimes < 1m for some distance) except at turns where it was 10m off max briefly.
@breadoven can you explain what a high or low tracking value has as an effect? I wonder why 50 was okay-ish with some snaking but 25 was uncontrollable aggressive and overshooting constantly.
I ran out of lipos to do more tests.
The tracking accuracy setting affects the bias between heading toward the course line (to reduce the distance from it to 0) and aligning the aircraft heading with the course bearing. Lower settings increase the bias toward heading to the course line. However, if too low it will overshoot because the aircraft heading won't be aligned with the course line when it arrives. If you increase the setting too much then aircraft heading will tend to align with the course line bearing before it gets to the line so it undershoots in that case (it does then move toward the line but slowly). So higher settings are probably the better starting point, then reduce until it starts to overshoot.
@breadoven ok great knowing that I will make some more tests soon and maybe also on different planes. Your test run looks really neat. Ardupilot couldn't do that much better :D
@breadoven You can test this in simulator https://github.com/RomanLut/INAV-X-Plane-HITL I will prepare RP next week, for now use branch https://github.com/RomanLut/inav/tree/submit-simulator-xplane
@breadoven You can test this in simulator https://github.com/RomanLut/INAV-X-Plane-HITL I will prepare RP next week, for now use branch https://github.com/RomanLut/inav/tree/submit-simulator-xplane
This looks really cool. Does it work with the Demo as well to test? Don't wanna buy the full version as I already have FS2020.
Although this is really nice to test new features, it can't replace real word tuning. As seen above in your test the real world path does not represent the simulated path :D
This looks really cool. Does it work with the Demo as well to test? Don't wanna buy the full version as I already have FS2020. Yes, you can fly 10 minutes at Seattle airport. Than you have to restart the demo.
@breadoven had no time yet to test fly again but One questions came into my mind. Will this also affect cruise mode? As far as I understand cruise mode, it does create a virtual waypoint in front of the plane's bearing direction that gets updated over and over again with the same course.
Current ly if Compass is enabled on fixed wings, the plane will align the compass heading towards this waypoint and not the course, so in crosswind situations, the plane will drift sideways. With compass off it flies a straight line. So I wonder if this path tracking will also work in cruise mode so we have the correct plane heading independant from the plane's course in crosswind and still fly a straight line.
Going to test again today on a small agile and fast Dart 250G, Also a gentle cruising small Talon 250G as well as on the AR Pro again to find some common calibration value for the Path Tracking Bias. If I have time I will also test with and without turn smoothing as it is right now. Some meidum wind though.
A few flight tests done again. Here the results with the Craft and settings. Mission File: Path Tracking Mission.zip
I was pretty impressed about the Loiter precision! It was a pretty constant 15kph wind, but the circles are nearly perfect! Has this also influence on loiter behavior? Also the tracks are very consistent on a properly tuned wing.
AR Pro, tracking_precision = 60, smoothing = OFF
LOG00108.TXT
AR Pro, tracking_precision = 70, smoothing = OFF
LOG00109.TXT
AR Pro, tracking_precision = 85, smoothing = ON
LOG00110.TXT
Talon 250G, tracking_precision = 60, smoothing OFF
Talon_250G-2022-07-28.csv
Dart 250G, tracking_precision = 60, smoothing OFF (by far not optimized for any auto modes and way too fast)
Dart-2022-07-28.csv
Dart 250G, tracking_precision = 75, smoothing OFF (reduced cruise throttle to a reasonable speed)
Same log file as above
Somehow I see no difference between Smoothing ON and OFF. Although the plane makes a short twitchy turn in the opposite direction before starting the actual turn but barely visible in the logs. Planes with rudder are indeed much more capable if tracking the path.
My suggestion is a Path tracking value of 60 for planes with Rudder/Tail and a value of 85 or 90 for Wings. I can include them in the plane presets of the configurator. Planes with tail but no rudder might be difficult though but I have to try with the drift first how well it works there.
Have to admit that I've been trying another method for tracking accuracy that I'm starting to think works better than the current method because it possibly doesn't need any setting to tune it or it'll be easier to tune at least. It works in a different way by setting just a target position on the waypoint course line rather than playing with headings and distances to the course line. Seems more fool proof.
Not sure about the loiter circles, I haven't changed anything that would affect accuracy. I have though been using GPS course over ground rather than IMU yaw as the Nav yaw input. Possibly improves loiter accuracy, hard to be completely sure because it depends a lot on the wind at the time.
Sounds interesting. I showed my logs to someone who uses both Ardu and INAV and he was impressed from the AR Pro flight. He said that could be Ardu :D
On the other hand I think this option is not too bad already. Maybe instead of just a bias we could make it smoother and more precise with a simple P/D controller? With a Derivative value to slow down the approach to the track. Like the POS_XY and POS_Z controller we could easily find values that work on nearly any plane.
I have though been using GPS course over ground rather than IMU yaw as the Nav yaw input.
From my tests, past and present.. I'm 100% sure for fixed wing use, that flying for effect, based on GPS position and Barometric altitude. Is far more accurate than relying on iNav's IMU data, after it has made turn after turn.
iNav relies too heavily on IMU data for fixed wing use. I have seen loiters go badly on planes that experience ACC drift. Even to the point of loosing altitude loiter circuit after loiter circuit. Eventually hitting the ground.. All because iNav doesn't filter out the bad IMU data well enough, by disregarding it, when the plane has drifted or descended far from the original Poshold 3D activation position.
@breadoven Thanks for accidentally making Poshold the best its been.. :+1:
