jdl

  • ***
  • 113
AutoCruise Mode for a FixedWing
« on: June 05, 2019, 10:11:47 am »
Although mentioned somewhere, the AutoCruise GPS mode is not discussed too much. In fact, I was unaware of its implementation in LibrePilot. Some time ago I decided to do some code reading and make an attempt to add a new flight mode to Librepilot to perform a level flight in specified direction while keeping altitude. Finding the already existing code for AutoCruise was a really pleasant surprise :)

I believe the AutoCruise mode is intended mostly for FixedWing vehicle type and not for VTOLs / multirotors.

I've made some tests that confirmed the existing code functions quite well and reliable. It just needs a minimal setup to be used.

First of all, the other GPS flight modes like ReturnToBase and PositionHold, should be already properly setup and working fine.

AutoCruise cannot be set in the Configuration / Input / FlightMode Switch Positions. Not available in the dropdown list there. Use System tab instead: Settings / FlightModeSettings / FlightModePosition and set it there for the desired switch position.
    
There is one more parameter to be changed:

Settings / FlightModeSettings / PositionHoldOffset / Horizontal. Default value of 30 is way too low. I can suggest a value of 300 which may not be the optimal but gives quite satisfactory results. I'll try to explaing in short:

The way the AutoCruise works is flying in GoToEndPoint (not FollowVector!) mode to a point that is projected some distance away from current airplane position at the same height as of airplane position. The initial direction (heading) is the one captured at the moment of AutoCruise mode engagement.

The target position is being continuously updated with airplane position changes, keeping height and heading, also taking in mind pilot's manual control commands (if any) on pitch and yaw. This allows some precise minimal changes in the heading and height the plane follows autonomously.
The forementioned PositionHoldOffset / Horizontal (meters) is used to setup how far away from current position is the target endpoing projected. Default value of 30 (that is fine for VTOL configuration and the way used in VTOL flight code) is not fine for FixedWing mode. For a fast flying vehicle (airplane) this is too nearby and the airplane likely overshoots its target. This causes ugly wanderings as the plane attempts to start turning back to target. See first video:

Video 1 (AutoCruise with default PositionHoldOffset / Horizontal = 30) - unstable


The suggested value of 300 seems to work fine for MiniTalon (60km/h cruise speed) and Wing Z-84 (45-50km/h cruise speed).

You may also wish to increase the default value for Settings / FlightModeSettings / PositionHoldOffset / Vertical. It is used to control AutoCruise sensitivity for manual pilot commands (pitch stick) for altitude changes.

When in FixedWing mode, changing PositionHoldOffset / Horizontal & Vertical has no side effects on other flight modes. At least I did not found references when browsing flight code. If I'm wrong, devs or some more experienced will step in to correct me, I believe.

Video 2 (AutoCruise with PositionHoldOffset / Horizontal = 300) - OK
« Last Edit: June 05, 2019, 10:27:34 am by jdl »

Re: AutoCruise Mode for a FixedWing
« Reply #1 on: June 06, 2019, 03:30:59 am »
Hey, a good scholarly description of a fixed wing mode that I'm interested in, but haven't tried yet.

And then I saw who posted it <jdl> and of course it sounds like it was written by someone who groks the code.  :)

Code musings follow.  :)

A different method for fixed wing PH was coded in some initial OpenPilot code and that is to just fly in a circle.  I liked that as compared to randomly flying back and forth over the PH location.  Set speed to HorizontalVelMin and it's easy to calculate circle radius from HorizontalVelMin and RollLimit.  Circling at HVelMin and RollLimit is provably closer to the desired position than anything that uses these same values but ever allows the wings to be level.

While I am at it, I have always felt that for fixed wing, altitude should be controlled by pitch and then airspeed should be controlled by throttle.  This fits the physics.  You pull back to avoid the tree and get "instant" angle change to avoid the tree and in that instant you start to slow down, you don't instantly slow down.  You simply add throttle to maintain that same airspeed.  The way it is coded now, it adds throttle, and only as the speed increases does the pitch angle increase.  The crossfeeds were put there to get around this issue.  I keep telling myself I will write a new FWPF, if just to prove myself wrong.  Simpler, easier to understand and configure.

Yes, it may still need methods so that it knows to quickly add throttle when it pitches up to avoid a tree (rather than wait on an airspeed PID to figure it out).  This gets into the code knowing the entire flight envelope for programmed stunts and knowing the "best" way to return to level flight from any attitude.  High alpha and hovering flight should be possible too.  When a beginner is inverted and heading down hill, is simply using down elevator an option for crash avoidance?

Back to the current code base...

In the past, having only done a good once over into that part of the code, PositionHoldOffset has seemed to me a bit of a setting in search of a raison d'ĂȘtre.  Fixed wing should select something like HorizontalVelMin and always just fly in the direction of the desired location (while obeying CourseFeedForward when the course is one point, to avoid constant thrashing around at full control).  I wonder if setting CFF to a higher value would smooth out PH into a circle.  It could be coded that way if it isn't already.

Maybe instead of having settings for PositionHoldOffset / *, these should just be calculated as factors of HorizontalVelMax and VerticalVelMax (and maybe CourseFeedForward)?  Then which set of *VelMax and CFF to use since we have 3 sets for the 3 pathfollowers.  Probably just look at the vehicle type to select which to use.  The multiple sets of *VelMax and CFF could be unified, but that would mean they can't have different defaults for different vehicle types, and also that we can't have a multi-mode aircraft that flies a long fast mission as a fixed wing and then has to hover a slow course as a VTOL.  Switching aircraft types in flight would of course require other code changes.  Having more than one mixer table is one that has been previously bantered about.
« Last Edit: June 06, 2019, 06:41:16 pm by TheOtherCliff »

jdl

  • ***
  • 113
Re: AutoCruise Mode for a FixedWing
« Reply #2 on: June 10, 2019, 04:04:36 pm »
Thanks, Cliff, this gave me something to think about :)

Sadly, I do neither have the time, nor (at present) the necessary knowledge to make and further test any significant changes in the flight code. All I do is examining the code to get better understanding how to properly setup and use different flight modes and sometimes fixing minor imperfections I stumble upon...

Gaining altitude by initially applying throttle also seemed somehow unnatural to me. For instance, my MiniTalon doesnot gain altitude, even at full throttle, unless I put some up elevator. But Librepilot drives it well in PathPlanner when it has to climb, just seems to be a little more aggressve on throttle initially ;)

Anyway, this variant of altitude control for fixedwing seems to be quite safe for avoiding a stall condition if very steep climb is commanded in the flight plan. Or I'm wrong?

I agree that while FWPF seems to be quite reliably working in its current implementation, it could be probably made better.

One thing I'm tinkering with: while flying autonomously, if a lowspeed / stall error is raised (rapid airspeed drop due to sudden wind gust, for instance), no immediate action is taken by the autopilot. The throttle is been gradually raised as a result of the airspeed error but it just takes too long time.

I plan to try to forcefully set AirspeedToPowerCrossFeed.Kp to its max value while error conditions are present hoping this will result in much more rapid throttle response when fixedWingPathFollowerStatus.Errors.Stallspeed = 1 . Do you think this idea makes sense?

Re: AutoCruise Mode for a FixedWing
« Reply #3 on: June 10, 2019, 07:39:34 pm »
Anyway, this variant of altitude control for fixedwing seems to be quite safe for avoiding a stall condition if very steep climb is commanded in the flight plan. Or I'm wrong?
The first things that comes to mind is that PitchLimit ThrustLimit Safetymargins SafetyCutoffLimits must allow the climb to be as steep as you are wanting.
I would guess that it can work well for small angles.  From the looks of it, I wouldn't try to go much more than 45 degrees (even if you have the power) because some things appear to be coded with simple arithmetic instead of trig.  They look like they use the approximation that that sin(x)==x for small x.  For instance VerticalToPitchCrossFeed is described as degrees per m/s when obviously that is wrong once degrees gets past 90 (or even anywhere close to 90).  Importantly (given a fixed airspeed), there is a lot of difference in climb rate between 0 and 1 degree, but almost none between 89 and 90 degrees.  But be aware that I haven't read the code recently enough or deeply enough to know whether it is actually using trig there.

One thing I'm tinkering with: while flying autonomously, if a lowspeed / stall error is raised (rapid airspeed drop due to sudden wind gust, for instance), no immediate action is taken by the autopilot. The throttle is been gradually raised as a result of the airspeed error but it just takes too long time.

I plan to try to forcefully set AirspeedToPowerCrossFeed.Kp to its max value while error conditions are present hoping this will result in much more rapid throttle response when fixedWingPathFollowerStatus.Errors.Stallspeed = 1 . Do you think this idea makes sense?
My first thought about that is that increasing the PID may cause oscillation.  And what about the other case, where you get a high speed error?  For most airframes, high speed error is probably safer to ignore than low speed error, but I still would not want to be diving so fast that it flutters.

This slow throttle response is one reason I would like to try a different flight algorithm.  Tuning the current algorithm for better throttle response seems to just cause throttle oscillation.

Short of a whole new flight algorithm...  This doesn't address gusts, but for me, gusts are not the biggest issue.  Something like this should allow you to climb very steeply, but I have no idea how many places in the code assume a small pitch (and bank) angle and have problems with a large angle.  Assuming a very powerful aircraft that can easily climb at 45 degrees, my first thought is that for pitch angles from -45 to +45 maybe just a throttle factor to be tacked on after the current PID (and leave the current PID tuned fairly low, and you will probably need to zero out the appropriate crossfeeds).  At 0 degrees pitch you get no change.  At +45 degrees you add say 50% power.  At +22.5 degrees you add 25% power.  At -45 degrees you subtract 50% power.  Of course trig handles this correctly, and for all angles.  If your airplane has enough power to climb vertically at say 75% throttle and cruises at say 25% power, and you want more than 45 degrees of climb, then you should use some trig, but the code is similar.  Add zero power when level and add 75-25=50% power when pointed straight up.  For -90<angle<90, add sin(DEG2RAD(angle))x50%?  I am guessing that 50% should be a new setting.

On a similar note, I wanted a steep aileron bank angle so that my turns and position hold would be tighter.  The problem with the stock algorithm is that the nose drops with the steep bank angle, and it takes a while for this algorithm to see the drop and correct it, causing problems for tight turns close to the ground.  I just added elevator based on current sensor bank angle, right there where it commands the bank angle.  The formula I used was empirically tweaked and does not take airspeed into account.  It doesn't use trig because it assumes X degrees of roll needs X (times a constant factor) degrees of pitch.  I think the pitch boost should actually be calculated as the amount of additional elevator needed at that airspeed and bank angle.  In a perfect world, rudder could be boosted too, proportional to the amount of aileron used.  To coordinate the roll (not the turn).

It's a hack because it needs to use a better estimation that takes airspeed into account, it shouldn't mix up the use of roll and pitch limits, and that factor of 0.5 should be a UAV setting because it will be different for different aircraft.  Some thought should be given to how changes in configuration should be adjusted for, automatically.  But I was very pleased with the fact that I can now configure sharp turns in waypoint flight, and not loose altitude.

fixedwingflycontroller.cpp
Code: [Select]
    stabDesired.Roll = boundf(fixedWingSettings->RollLimit.Neutral +
                              courseCommand,
                              fixedWingSettings->RollLimit.Min,
                              fixedWingSettings->RollLimit.Max);

#if 1
    // of course this needs to take airspeed into account
    // but this proof of concept does not bother to
    {
        float fractionalRoll;
        // just use the desired roll angle to calculate the pitch angle
        // the problem with this is that the elevator servo gets to it's angle
        // much more quickly than ailern servo + physical roll
        // and so the nose pops up at the beginning of the turn
        //fractionalRoll = stabDesired.Roll;

        // get the actual bank angle and limit it to the min/max roll (in case aircraft is in an uncommanded attitude)
        AttitudeStateRollGet(&fractionalRoll);
        // simplify by making it positive
        // this does assume symmetric roll "min neutral max" and zero neutral
        fractionalRoll = fabsf(fractionalRoll);
        // if way over banked, don't pull it downward
        // if (fractionalRoll > 90.0f) {
        //    fractionalRoll = 0.0f;
        // } else
        // limit it to the min/max roll (in case aircraft is in an uncommanded attitude)
        // note that stabDesired.Roll has already been limited to fixedWingSettings->RollLimit.Max
        if (fractionalRoll > stabDesired.Roll) {
            fractionalRoll = stabDesired.Roll;
        }
        // since we assume symmetric roll "min neutral max"
        // we must assume neutral is zero
        fractionalRoll = fractionalRoll / fixedWingSettings->RollLimit.Max;
        fractionalRoll *= 0.5f;

        // add fractionalRoll portion of normal ROLL travel, and DO NOT limit it (to max pitch travel)
        // this follows from me seeing that Attitude mode turns, max=(55,55) seems to work well with equal parts roll and pitch
        // but it looks like I reduced the elevator boost to half the aileron bank angle
        // and used the roll limits for pitch to avoid changes in flight from just changing pitch limit?
        // note that this should really take airspeed into account, high airspeed needs low elevator boost
        stabDesired.Pitch += fractionalRoll * (fixedWingSettings->RollLimit.Max - fixedWingSettings->RollLimit.Neutral);
    }
#endif

    // Error condition: roll way out of wack
    fixedWingPathFollowerStatus.Errors.Rollcontrol = 0;
    if (fixedWingSettings->Safetymargins.Rollcontrol > 0.5f &&
        (attitudeState.Roll < fixedWingSettings->RollLimit.Min - fixedWingSettings->SafetyCutoffLimits.RollDeg ||
         attitudeState.Roll > fixedWingSettings->RollLimit.Max + fixedWingSettings->SafetyCutoffLimits.RollDeg)) {
        fixedWingPathFollowerStatus.Errors.Rollcontrol = 1;
        result = 0;
        cutThrust = true;
    }

Or ... to handle gusts, write an airspeed to throttle PID to use with Manual flight mode and get it working and tuned there, then use this new code in autonomous flight modes.
« Last Edit: June 10, 2019, 11:25:30 pm by TheOtherCliff »

jdl

  • ***
  • 113
Re: AutoCruise Mode for a FixedWing
« Reply #4 on: June 11, 2019, 03:38:46 pm »
One thing I'm tinkering with: while flying autonomously, if a lowspeed / stall error is raised (rapid airspeed drop due to sudden wind gust, for instance), no immediate action is taken by the autopilot. The throttle is been gradually raised as a result of the airspeed error but it just takes too long time.

I plan to try to forcefully set AirspeedToPowerCrossFeed.Kp to its max value while error conditions are present hoping this will result in much more rapid throttle response when fixedWingPathFollowerStatus.Errors.Stallspeed = 1 . Do you think this idea makes sense?
My first thought about that is that increasing the PID may cause oscillation. 

The only place in code where AirspeedToPowerCrossFeed.Kp is used is in fixedwingflycontroller.cpp:

Code: [Select]
    /**
     * Compute desired thrust command
     */

    // Compute the cross feed from vertical speed to pitch, with saturation
    float speedErrorToPowerCommandComponent = 0.0f;
    if (hasAirspeed) {
        speedErrorToPowerCommandComponent = boundf(
            (airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
            -fixedWingSettings->AirspeedToPowerCrossFeed.Max,
            fixedWingSettings->AirspeedToPowerCrossFeed.Max
            );
    }

I think that rapidly increasing that AirspeedToPowerCrossFeed.Kp during stall error condition will not make harm as speedErrorToPowerCommandComponent is anyway bound in preset limits. After error condition is cleared, Kp will be set back to its default.
Or, maybe just set speedErrorToPowerCommandComponent = AirspeedToPowerCrossFeed.Max during stall error condition. But this seems too ugly...

Maybe I'm wrong, as I said, I lack a lot of knowledge and I'm just guessing :)

Quote
And what about the other case, where you get a high speed error?  For most airframes, high speed error is probably safer to ignore than low speed error, but I still would not want to be diving so fast that it flutters.

When enabled StabilizationSettings / ScaleToAirspeed I get no speed dive oscillations at all.

High speed error is already handled in the code, simply cuts thrust:

Code: [Select]
        if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
            fixedWingPathFollowerStatus.Errors.Highspeed = 1;
            result = 0;
            cutThrust = true;
        }

I've unwillingly tested it some time ago and yes, the thrust is cut if high speed condition is met.

Funny part: it was a hardware issue with airspeed sensor power supply indeed, electrical noise from the motor/esc was entering the sensor and causing invalid high speed readings  ;D. Simple LC filter resolved that issue...