Hi, I am working on performing automated landings in a moving net. We do this by using Guided mode to implement LOS guidance using a separate onboard computer, following a glideslope with several segments into the net. For this we need accurate altitude control, but do not care as much about the airspeed, and therefore we set the TECS SPDWEIGHT parameter to 0 or close to it.
Despite many attempts at tuning the TECS controller we still have problems with climb rate overshoot. This is especially visible when looping the landing plan, having a steep climb directly after passing through the target. See this plot of the logged TECS h, hdem (actual and desired height) and dh, dhdem (actual and desired climb rate):
The log for this flight is also linked at the bottom of this post. This flight used a SPDWEIGHT of 0.2. The problem is clearly shown at the beginning and end of the climb. This is also a problem for the landing as it causes us to hit about 0.5m above the target with the glide slope segments shown here (caused by the change in path angle at approx 932s). The integral term of the controller makes the height reach the desired if given enough time where the desired climb rate does not change.
I have been digging in the TECS code, and my hypothesis for the cause of this at this point is that the feedforward part of the pitch control does not properly account for the speedweight parameter, and that this does not occur for the default speedweight of 1. Essentially, if SPDWEIGHT is set to 0, the desired pitch is calculated as
where TAS is airspeed, TC is TIME_CONST, k_damp is PTCH_DAMP, k_i is INTEG_GAIN and the subscript d denotes the desired value. The factor 2 in all terms comes from
float SPE_weighting = 2.0f - SKE_weighting;
(here) where SKE_weighting is the speedweight which is 0. For all terms where the time constant is included, the extra factor of 2 is no problem, as we just use the time constant for tuning anyway, but this is not the case for the feedforward . If this is a linearization of the required path angle for a given speed and climb rate, I don’t see how there should be a factor 2 included. I have also looked at the change in the value of the integral term with changes in desired climb rate, and the integral change is not far off from changing by half as much as the feed forward term, perhaps indicating that the extra pitch added by the factor 2 in the feedforward is compensated for by the integral.
I would appreciate some feedback on this, whether it is possible to remove the problem by changing parameters or if this is actually a bug in the TECS controller.