[jdl]

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

[TheOtherCliff]

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.

[jdl]

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?

[TheOtherCliff]

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.

[jdl]

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

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  . Simple LC filter resolved that issue...

[jdl]

Hmm, I sometimes amuse myself how stupid things I can say/write before I think (or at least read more carefully, in this case). I've read the once again your posts and the LP code and understood the way AirspeedToPowerCrossFeed.Kp works.

Cliff, you are absolutely right, of course!

Anyway, out of curiosity, I've tested how much I can raise it before oscillations occur. At AirspeedToPowerCrossFeed.Kp = 1 or 0.8 there are already perceptable oscillations.

I think I've got the ideas you shared but I cannot write the necessary code now. No time to write and further test, maybe no enough knowledge...

A quick and dirty workaround (with the current fixedwingflycontroller code) for avoiding stall condition due to low airspeed I've added is to raise AirspeedToPowerCrossFeed.Kp to 2 if airspeed drops below systemSettings.AirSpeedMin * Safetymargins.Stallspeed and keep it until airspeed raises over HorizontalVelMin * Safetymargins.Lowspeed. With properly set limits for the particular wing
( for Z-84 I set:
systemSettings.AirSpeedMin * Safetymargins.Stallspeed = 30km/h
HorizontalVelMin * Safetymargins.Lowspeed = 35km/h
HorizontalVelMin = 44km/h
HorizontalVelMax = 48km/h
)
this causes instant throttle burst to (or almost to) ThrustLimit.Max if airspeed drops below 30km/h and keeps throttle high until 35km/h are reached. This happens quite rapidly and I cannot really notice thrust oscillations during the throttle burst.

I've made a test flight with higher speed limits just to prove code is working. I'm aware the speed thresholds set higher gave lower effect on speedErrorToPowerCommandComponent from AirspeedToPowerCrossFeed.Kp due to lower airspeedError. That's why I'd like to further test how the extra code behaves with really low airspeed and large airspeedError.

I'll report back results here, hopefully with DVR and telemetry records.

Code: [Select]

// Private constants
uint8_t StallSpeed_Avoidance = 0;
...
...
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
            fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
StallSpeed_Avoidance = 1;
            result = 0;
        }
if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
StallSpeed_Avoidance = 0;
}
...
...
    float speedErrorToPowerCommandComponent = 0.0f;
    if (hasAirspeed) {
        speedErrorToPowerCommandComponent = boundf(
            (airspeedError / fixedWingSettings->HorizontalVelMin) *
((fixedWingSettings->AirspeedToPowerCrossFeed.Kp * (1 - StallSpeed_Avoidance)) + (2.0f * StallSpeed_Avoidance)),
            -fixedWingSettings->AirspeedToPowerCrossFeed.Max,
            fixedWingSettings->AirspeedToPowerCrossFeed.Max
            );
    }
...
...


[TheOtherCliff]

I hadn't closely looked at the code in question when I posted, but any control code with Kp in a variable name is almost always referring to the P term of a PID, and starting with a tuned PID, increasing P will cause oscillation.   

Without a whole lot of research to back up my statements, it seems to me that you start with an algorithm that takes the current throttle stick (or algorithm's desired "speed") and modify it with simple trig that depends on pitch angle ... like Cruise Control does for quads (but different trig since we don't need infinite power at a 90 degree angle).  This can be used either with a PID to set an actual speed number, or better yet used alone without a PID.  Imagine the simplification of not even needing an airspeed sensor (air pressure or GPS based) because you are not running an airspeed PID.   

It seems I am trying to talk myself into an enhancement for CruiseControl that allows you to use it with airplanes.   

Some airplanes can climb straight up on half power, and some might only manage to climb at 30 degrees on full power.

Given an airplane that needs to go, say from 30% power when level to 80% power to climb straight up (both at "cruise speed"), calculate the boost factor (always 1.0: when level, 80/30: which boosts 30% to 80% when straight up) using trig (30/80 power factor when pointing straight down reduces 30% power to 11.25%).  Pilot or code requests standard cruise speed so it starts with only two numbers to set (cruise power %, and max boost factor).  With this maintaining throttle for you, you get reasonable airspeed whatever pitch angle you or the nav code requests.

Given an airplane that can only climb at 30 degrees max and needs goes from 50% when level to 100% at 30 degrees, use the same trig, but you can see that the climb angle must be limited to 30 degrees where in the more powerful airplane it gets limited to 90 degrees (no actual limit).  When pointed down, the throttle boost factor is 50% when diving at 30 degrees so multiply the 50% normal cruise power by 50%.  Or should it be a power factor of 0% in a 30+ degree dive?  So for this more complicated case we see in general we need 3 settings: cruise power % (50% for this), max climb angle (30 degrees for this) and max climb boost factor (2.0 for this).

Given this to control throttle, the navigation code just needs to run a PID on pitch attitude to keep it on the sloped line from one waypoint to the next.

Probably need to implement a max dive angle too, for airplanes that can't handle flying straight down at zero or reduced power.

Now to think what, if anything, should be done for steep roll bank angles (ignore) or elevator induced loiter drag (another (linear) adjustable throttle boost that is based on elevator deviation).

Also, this algorithm unnecessarily limits airplanes that e.g. can fly straight up slowly, but not at cruise speed.  You can fudge the setup a little (set a higher max climb boost factor than possible for cruise power %) and the code will clip the power at 100%.  That part of the code will think you can fly straight up at cruise speed because it will think it is giving more power than it really is.

[TheOtherCliff]

For those that might not know, roll/pitch/yaw PIDs on a fixed wing are tuned for one specific airspeed but you don't fly at one specific airspeed.  If a Revo class FC has an airspeed sensor enabled (even the GPS based one) it uses measured airspeed to change the PIDs to fit the airspeed and you always have the correct PIDs.

With thrust control coded like is being discussed here (e.g. when pitching up, throttle output is automatically increased by a user configurable number and simple trig to maintain the same airspeed without actually measuring the airspeed ... within known limits), airspeed more reasonably corresponds to throttle input (stick) position (airspeed doesn't change when you pitch up/down) and the code could modify the roll/pitch/yaw PIDs during flight with this airspeed cousin in the same way we already modify the PIDs using the real airspeed sensor (including the GPS based airspeed sensor).

The throttle to speed curve is not linear (can't be used as an airspeed approximation, even with simple scaling).  When flying straight and level, 100% throttle is generally not exactly 2 times as fast as 50% throttle.  Probably the most useful way to handle this is to convert the throttle stick position into an actual approximate airspeed using a non-linear function.  Thus, any code that wants the airspeed can use this approximation of airspeed.

Here is the kicker.  Once this is set up (tuned) by the user, it gives you this airspeed approximation, even when you are not using this new thrust mode, so your PIDs are always correct and you always have an airspeed approximation available for other uses (e.g. telemetry).  No airspeed sensor required.  No GPS required.

Update: I realize that if you don't actually have some airspeed sensor that this can fail if the motor fails.  You may be asking for cruise speed plus a climb and the motor is zero while it is pulling up elevator.  A stall could easily happen without some knowledge of airspeed to code in some failsafes.  Further, an unstable airplane (badly tail heavy) can't even be repeatably trusted to do X airspeed each time it is given the same Y throttle and Z elevator.

I actually have a plan to code a CruiseControl for fixed wing as part of a GPS fixed wing algorithm I have started working on and have documented that the non-GPS version will stall if you pull up on a dead motor (just like manual throttle).  The GPS (or other airspeed sensored) version will see the slowdown and avoid letting it get below a configured min airspeed.

[jdl]

...
A quick and dirty workaround (with the current fixedwingflycontroller code) for avoiding stall condition due to low airspeed I've added is to raise AirspeedToPowerCrossFeed.Kp to 2 if airspeed drops below systemSettings.AirSpeedMin * Safetymargins.Stallspeed and keep it until airspeed raises over HorizontalVelMin * Safetymargins.Lowspeed.
...
I'll report back results here, hopefully with DVR and telemetry records.

Code: [Select]

// Private constants
uint8_t StallSpeed_Avoidance = 0;
...
...
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
            fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
StallSpeed_Avoidance = 1;
            result = 0;
        }
if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
StallSpeed_Avoidance = 0;
}
...
...
    float speedErrorToPowerCommandComponent = 0.0f;
    if (hasAirspeed) {
        speedErrorToPowerCommandComponent = boundf(
            (airspeedError / fixedWingSettings->HorizontalVelMin) *
((fixedWingSettings->AirspeedToPowerCrossFeed.Kp * (1 - StallSpeed_Avoidance)) + (2.0f * StallSpeed_Avoidance)),
            -fixedWingSettings->AirspeedToPowerCrossFeed.Max,
            fixedWingSettings->AirspeedToPowerCrossFeed.Max
            );
    }
...
...


I've recently had a real flight situation while flying in PP mode  (MiniTalon with Revo running modded firmware with this stall-speed-avoidance code) when the airspeed dropped below the preset 40km/h threshold. I've left some margin over the real stall speed (32-34km/h).

Windy weather with gusts often changing direction.



You can see (and hear) in the FPV DVR record (at 00:16) how the code kicks in (AirspeedToPowerCrossFeed.Kp is set to 2.0) for a second or two (airspeed has dropped below 40km/h), a throttle burst and the plane rapidly accelerates. Then AirspeedToPowerCrossFeed.Kp is restored to its default value of 0.2. I see no side effects / significant oscillations.

The settings were:

HorizontalVelMax = 58km/h
HorizontalVelMin = 54km/h
systemSettings.AirSpeedMin * Safetymargins.Stallspeed = 40km/h
HorizontalVelMin * Safetymargins.Lowspeed = 46km/h


A better approach may be to raise and lower AirspeedToPowerCrossFeed.Kp not rapidly but gradually, 10 steps in a second, for example, to avoid wild throttle changes. Here is what I'm currently using:

changes in fixedwingflycontroller.cpp

Code: [Select]


...
// Private constants

uint8_t StallSpeed_Avoidance = 0;
float SSA_Kp = 0.0f;

...

uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
{
...
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
StallSpeed_Avoidance = 1;
result = 0;
}
if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
StallSpeed_Avoidance = 0;
}

...

    float speedErrorToPowerCommandComponent = 0.0f;
    if (hasAirspeed) {

SSA_Kp = boundf(
SSA_Kp,
fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
(fixedWingSettings->AirspeedToPowerCrossFeed.Kp * 10.0f)
);
if (StallSpeed_Avoidance == 1) {
if (SSA_Kp < (fixedWingSettings->AirspeedToPowerCrossFeed.Kp * 10.0f)) {
SSA_Kp += fixedWingSettings->AirspeedToPowerCrossFeed.Kp;
}
} else {
if (SSA_Kp > fixedWingSettings->AirspeedToPowerCrossFeed.Kp) {
SSA_Kp -= fixedWingSettings->AirspeedToPowerCrossFeed.Kp;
}
}
        speedErrorToPowerCommandComponent = boundf(
            (airspeedError / fixedWingSettings->HorizontalVelMin) * SSA_Kp,
            -fixedWingSettings->AirspeedToPowerCrossFeed.Max,
            fixedWingSettings->AirspeedToPowerCrossFeed.Max
            );
    }
...

Reaction is still quick enough (it takes just a second to increase Kp x10 or decrease it back to x1) but noticeably smoother.