AP_TECS: Initialize flare sink demand with current demand

[Georacer]

Summary

This PR modifies the flare landing stage and the climb rate control, to initialize the climb rate demand not from the current climb (sink) rate, but from the current climb rate demand.

Details

I noticed in a log posted in the forum that when the flare starts, we snap the demanded climb rate to the current climb rate.

The original idea is to progressively fade in the flare climb rate, which is usually 0.
But what actually can happen is that if the current sink rate is different to the sink rate demand, it can result in abrupt pitch up or down demand. It can also lead to unintended loss of altitude at a very critical state of the landing.

A couple of examples from that log:

I think it's a better idea to initialize with the similar quantity of the climb rate demand instead.

Testing

Tested in SITL, using the MAV_CMD_DO_LAND_START autotest.

Before:

After:

Potential improvements

It would be better perhaps if we were holding on to _hgt_rate_dem_prev, but there's no such quantity yet.

[Georacer]

@rubenp02 perhaps you're interested in reviewing?

[rubenp02]

@rubenp02 perhaps you're interested in reviewing?

Sure! I'll try to check it out today

[rubenp02]

Haven't been able to test fly it but it seems like a really good change with zero drawbacks. In fact the old code looks downright wrong to me. Is it known why the current sink rate was used before?

Now that I see this, it's very clear that this is an issue we've encountered with our FW drone. It doesn't matter that much for us since we belly land on a tough airframe so we have it tuned for very firm touchdowns anyway. But I must have pretty much hundreds of real flight logs where this shows up. Looking at a few of them, it typically means that the approach sink rate is held for around 0.3 s longer than intended. Again, haven't had the chance to flight test this change, but it seems like it would do away with that.

[tridge]

makes sense, thanks! I'd like to see a real flight test if we can

[tridge]

@rubenp02 when do you think you could test this?

[rubenp02]

@rubenp02 when do you think you could test this?

We have flight tests planned for next week, so if all goes according to plan, I'll be able to test it then. It's worth noting, however, that we're running slightly custom firmware based on latest stable, not master, and I don't personally own a FW drone to test it myself either. But it should be enough to see if this works.

[matteotarsi]

Can I help? I want to do real tests posing attention only of the flare

[trauntnberg]

My setup is prone to have altitude estimate a few meters higher than reality (baro in fuselage with openings). When the rangefinder is engaged at around 10m, height estimate is corrected and there is commanded descend to real height (which is correct) - but this is linked to almost instant flare due to combination of large actual sinkrate and LAND_FLARE_SEC.

As the actual sink rate during this correction is different each landing (depends on height estimate error), also the initial value for fading from actual sink rate to demanded landing sink rate is different each time, but for high sink rate, the fading takes so long that there is no chance for plane to achieve TECS_LAND_SINK.

This looks like it could probably help, I'd be happy to test it.