AP_Soaring: Wiggles when entering loiter

[samuelctabor]

Bug report

Issue details

Plane exhibits wiggle when entering LOITER after thermal detected. Particularly evident in Marco Robustini's log attached.

Version 3.9.8 (6ea22)

Platform [ ] All [ ] AntennaTracker [ ] Copter [ x ] Plane [ ] Rover [ ] Submarine

Airframe type ASW28 4 mt. wingspan

Hardware type Pixhawk 1

Logs 2019-05-15.16-14-05.zip

Target roll angle very noisy

Airspeed control isn't bad

Quite pitchy

Estimated thermal position movement appears reasonable after initial transient

Updated code with a number of fixes available here https://github.com/samuelctabor/ardupilot/tree/SoaringImprovements-398

Compiled for Pixhawk 1 here arduplane.zip

[robustini]

Thanks Sam! Soon as possible I try and i leave you feedback and df log.

[robustini]

@samuelctabor please, could you compile the firmware in APJ format?

[samuelctabor]

Hi Marco,

Here it is arduplane-apj.zip

Should be commit 291ad.

[robustini]

Hi @samuelctabor , i've tried the new code with my Elipsoid glider and there is a big problem, both in FBWB and AUTO many times it activates the motor, while with the official 3.9.8 it does what it should do. Here two logs, one with your fix and the other with the official 3.9.8. I'm sorry for the low bitmask so I don't think that you can be too useful but still check what I wrote to you because the problem is definitely there, I was piloting in FPV and the motor during gliding started very often, so please check the code. Tested with the SOAR channel to 1470 and 2000, same issue.

logs.zip

[samuelctabor]

Hi @robustini ,

It's related to the ARSPD_USE=2 setting, I haven't tested this before. When throttle is >0 the airspeed sensor is not used, and this calls a different function in the TECS, which does not set throttle to zero like the usual with-airspeed function.

I'll fix and recompile today.

Sam

[robustini]

Thanks Sam! Meanwhile I'm trying Frigateboard on the sim, it's not bad at all, besides the SOAR part provides a myriad of parameters, not included in the Ardupilot master. I wait for the fix and now can compile it.

https://youtu.be/ZpCTVLUqy48

[samuelctabor]

Hi Marco,

Updated firmware - should be commit c9322. Fixes throttle issue with ARSPD_USE=2 and aligns methods with PR https://github.com/ArduPilot/ardupilot/pull/9643 arduplane-c9322.zip

Yes there are a lot of parameters in the FrigateBird POMDP method. I started some work to simplify a little - https://github.com/samuelctabor/ardupilot/tree/Soar-POMDP-master - a work in progress and not flight tested.

[robustini]

Thanks Sam, if I can I try it in the air this afternoon. I would have an implementation to ask you, which I think there is in the POMDP, define a radius where if the glider reaches it during Loiter it does not return to the home but does something else. For example if in AUTO it goes into the Loiter to follow a thermal and reaches the max radius could do that it passes to the next waypoint, or it takes the home as wp and when it gets there it goes to the next waypoint. In FBWB you could do the same thing, or create a sort of "default angle" where when it reaches the radius it goes back inwards with a definable radius (for example 40 °), to be clear as if it were a billiard ball hitting the edge of this virtual round table. This is to ensure that the glider always continues to autonomously look for the thermals without ever performing an RTL. If you look at my logs you will see that when in Loiter I often switch to RTL because it goes too far, then I go back to AUTO, and it's boring. The intention is to switch to AUTO or FBWB and continue to follow it in VLOS during the thermal hunting as long as the batteries allow, but keeping it in a radius (for example 500 meters). You could also use the geofence (polygon) but unfortunately goes to RTL, so it's pretty useless in that case. The geofence function should be modified, in case of Loiter (SOAR) it performs another thing, as I described above.

[robustini]

Hi @samuelctabor, I'm trying your changes and it seems to me that it goes much much much better! However there is a problem, in SOAR it never activates the motors in any case, for example when it reaches the SOAR_ALT_MIN and goes to RTL, or during AUTO mission, only enabled in MANUAL mode. However today there are incredible thermals here, look at the log! I await the fix of the throttle problem and the i hope that @tridge adds your improvements to the master when you have solved that issue. logs_new.zip

[Naterater]

This happens to all of my planes, and I don't use soaring, however it is only on Loiter entry. After that it stabilizes. Does your aircraft loiter normally when it doesn't use soaring?

Do you mind if I remove the AP_Soaring prefix?

[samuelctabor]

Hi Marco,

New version with throttle issue fixed arduplane-31ade.zip

[samuelctabor]

Hi Nate,

I'm still concerned that there may be a regression specific to soaring - the target roll angle in the OP looks off to me - compare to Marco's recent flights using my dev branch

The target still moves around and even reverses, as the target location is updated, but not very high frequency as in the stable release.

[robustini]

Hi Marco,

New version with throttle issue fixed arduplane-31ade.zip

Hi Sam, thanks for the fix, I tried it now with X-Plane 11 and SITL using your branch and it seems to work regularly, as soon I can I try it at the airfield.

[samuelctabor]

Sounds good Marco. You could try setting your WP_LOITER_RAD smaller as well. 20-30m should work for your aircraft at 12m/s.

[robustini]

Hi @samuelctabor, I noticed this problem in your 3.9.8 SOAR version, the FBWB speed is not maintained regularly in the simulator, while with the Ardupilot Master it works perfectly. It does exactly the same thing when I am in AUTO and is gliding in search of thermals. I'm running this test with the same parameters, is this behavior normal? I set the SOAR_VSPEED to 50 so that it maintains the FBWB without going into the Loiter, in the first part of the video I do the test with Ardupilot Master, in the second (at 02:00) with your "SoaringImprovements-398" branch. Watch when I perform FBW MIN changes in both tests, Ardupilot Master respects FBW MIN speed perfectly, with your branch absolutely not. Before trying it in reality I would like a check on this possible issue. I know it's the same code that I tried in flight the other time, but I don't understand why on the sim it behaves in this anomalous way. What do you think?

https://youtu.be/WEfFOkThVHE

[robustini]

@samuelctabor i've another request regarding the logic of the SOAR: if the SOAR is active in FBWB or CRUISE and goes to Loiter because it finds a thermal it does not seem correct to me that when it considers it vanished switch to RTL, it should do so only if reached the SOAR_AL_MIN, should instead return to FBWB or CRUISE. If the glider is within the SOAR_ALT_MIN and SOAR_ALT_MAX range it should return to looking for thermals, not at home. I don't understand why this was thought. For example, if the range min-max is 100<->300 meters and the glider is at 250 meters, it finds a thermal, goes into Loiter, maybe keeps it and rises to 270, then loses it, it seems absurd to me that at 270 meters of height then return to the home, should keep looking for thermals! Please consider changing this because it is quite castrating and illogical in my opinion.

[robustini]

In "soaring.cpp", line 66 and 67, please consider this changes, already tested in the sim and work fine:

            gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring FBWB");
            set_mode(mode_fbwb, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);

Unfortunately however the LOITER-> FBWB switch works only once, I checked it right now, the second time he enters Loiter he remains even if the glider is fallingmaybe that's why they entered RTL. This means however that logically something has not been done correctly.

[robustini]

I found the problem, just reinitialize the cruising after returning to FBWB, now the flight mode rotation work fine:

gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring FBWB"); plane.g2.soaring_controller.init_cruising(); set_mode(mode_fbwb, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);

[magicrub]

hey @robustini take a look at this PR

[magicrub]

plane.g2.soaring_controller.init_cruising(); set_mode(mode_fbwb, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);

Hmm, that should have no effect because that happens already in the set_mode for modes fbwb, cruise, and auto.

[robustini]

Absolutely agree with you, yet I have tried and so it works, maybe it's just a coincidence.