2013-06-26 05:38:40 -03:00
# include "AP_TECS.h"
2019-06-27 00:05:58 -03:00
2015-08-11 03:28:46 -03:00
# include <AP_HAL/AP_HAL.h>
2019-06-27 00:04:31 -03:00
# include <AP_Baro/AP_Baro.h>
2019-06-27 00:05:58 -03:00
# include <AP_Logger/AP_Logger.h>
2013-06-26 05:38:40 -03:00
extern const AP_HAL : : HAL & hal ;
2015-05-04 03:17:30 -03:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2015-09-15 22:17:28 -03:00
# include <stdio.h>
# define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
2013-06-26 05:38:40 -03:00
# else
2015-09-15 22:17:28 -03:00
# define Debug(fmt, args ...)
2013-06-26 05:38:40 -03:00
# endif
//Debug("%.2f %.2f %.2f %.2f \n", var1, var2, var3, var4);
// table of user settable parameters
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_TECS : : var_info [ ] = {
2013-06-26 05:38:40 -03:00
// @Param: CLMB_MAX
// @DisplayName: Maximum Climb Rate (metres/sec)
2018-12-20 19:42:08 -04:00
// @Description: Maximum demanded climb rate. Do not set higher than the climb speed at THR_MAX at TRIM_ARSPD_CM when the battery is at low voltage. Reduce value if airspeed cannot be maintained on ascent. Increase value if throttle does not increase significantly to ascend.
2015-09-15 22:17:28 -03:00
// @Increment: 0.1
// @Range: 0.1 20.0
2016-07-26 02:35:15 -03:00
// @User: Standard
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " CLMB_MAX " , 0 , AP_TECS , _maxClimbRate , 5.0f ) ,
2013-06-26 05:38:40 -03:00
// @Param: SINK_MIN
// @DisplayName: Minimum Sink Rate (metres/sec)
2018-12-20 19:42:08 -04:00
// @Description: Minimum sink rate when at THR_MIN and TRIM_ARSPD_CM.
2015-09-15 22:17:28 -03:00
// @Increment: 0.1
// @Range: 0.1 10.0
2016-07-26 02:35:15 -03:00
// @User: Standard
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " SINK_MIN " , 1 , AP_TECS , _minSinkRate , 2.0f ) ,
2013-06-26 05:38:40 -03:00
// @Param: TIME_CONST
// @DisplayName: Controller time constant (sec)
2018-12-20 19:42:08 -04:00
// @Description: Time constant of the TECS control algorithm. Small values make faster altitude corrections but can cause overshoot and aggressive behavior.
2015-09-15 22:17:28 -03:00
// @Range: 3.0 10.0
// @Increment: 0.2
// @User: Advanced
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " TIME_CONST " , 2 , AP_TECS , _timeConst , 5.0f ) ,
2013-06-26 05:38:40 -03:00
// @Param: THR_DAMP
// @DisplayName: Controller throttle damping
2021-03-01 19:10:42 -04:00
// @Description: Damping gain for throttle demand loop. Increase to add throttle activity to dampen oscillations in speed and height.
2015-09-15 22:17:28 -03:00
// @Range: 0.1 1.0
// @Increment: 0.1
// @User: Advanced
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " THR_DAMP " , 3 , AP_TECS , _thrDamp , 0.5f ) ,
2013-06-26 05:38:40 -03:00
// @Param: INTEG_GAIN
// @DisplayName: Controller integrator
2018-12-20 19:42:08 -04:00
// @Description: Integrator gain to trim out long-term speed and height errors.
2015-09-15 22:17:28 -03:00
// @Range: 0.0 0.5
// @Increment: 0.02
// @User: Advanced
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " INTEG_GAIN " , 4 , AP_TECS , _integGain , 0.1f ) ,
2013-06-26 05:38:40 -03:00
// @Param: VERT_ACC
// @DisplayName: Vertical Acceleration Limit (metres/sec^2)
2018-12-20 19:42:08 -04:00
// @Description: Maximum vertical acceleration used to correct speed or height errors.
2015-09-15 22:17:28 -03:00
// @Range: 1.0 10.0
// @Increment: 0.5
// @User: Advanced
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " VERT_ACC " , 5 , AP_TECS , _vertAccLim , 7.0f ) ,
2013-06-26 05:38:40 -03:00
// @Param: HGT_OMEGA
// @DisplayName: Height complementary filter frequency (radians/sec)
2013-07-04 04:52:40 -03:00
// @Description: This is the cross-over frequency of the complementary filter used to fuse vertical acceleration and baro alt to obtain an estimate of height rate and height.
2015-09-15 22:17:28 -03:00
// @Range: 1.0 5.0
// @Increment: 0.05
// @User: Advanced
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " HGT_OMEGA " , 6 , AP_TECS , _hgtCompFiltOmega , 3.0f ) ,
2013-06-26 05:38:40 -03:00
// @Param: SPD_OMEGA
// @DisplayName: Speed complementary filter frequency (radians/sec)
2013-07-04 04:52:40 -03:00
// @Description: This is the cross-over frequency of the complementary filter used to fuse longitudinal acceleration and airspeed to obtain a lower noise and lag estimate of airspeed.
2015-09-15 22:17:28 -03:00
// @Range: 0.5 2.0
// @Increment: 0.05
// @User: Advanced
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " SPD_OMEGA " , 7 , AP_TECS , _spdCompFiltOmega , 2.0f ) ,
2013-06-26 05:38:40 -03:00
// @Param: RLL2THR
// @DisplayName: Bank angle compensation gain
2021-05-17 12:02:13 -03:00
// @Description: Gain from bank angle to throttle to compensate for loss of airspeed from drag in turns. Set to approximately 10x the sink rate in m/s caused by a 45-degree turn. High efficiency models may need less while less efficient aircraft may need more. Should be tuned in an automatic mission with waypoints and turns greater than 90 degrees. Tune with PTCH2SRV_RLL and KFF_RDDRMIX to achieve constant airspeed, constant altitude turns.
2015-09-15 22:17:28 -03:00
// @Range: 5.0 30.0
// @Increment: 1.0
// @User: Advanced
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " RLL2THR " , 8 , AP_TECS , _rollComp , 10.0f ) ,
2015-09-15 22:17:28 -03:00
2013-06-26 05:38:40 -03:00
// @Param: SPDWEIGHT
// @DisplayName: Weighting applied to speed control
2018-12-20 19:42:08 -04:00
// @Description: Mixing of pitch and throttle correction for height and airspeed errors. Pitch controls altitude and throttle controls airspeed if set to 0. Pitch controls airspeed and throttle controls altitude if set to 2 (good for gliders). Blended if set to 1.
2015-09-15 22:17:28 -03:00
// @Range: 0.0 2.0
// @Increment: 0.1
// @User: Advanced
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " SPDWEIGHT " , 9 , AP_TECS , _spdWeight , 1.0f ) ,
2015-09-15 22:17:28 -03:00
2013-06-26 05:38:40 -03:00
// @Param: PTCH_DAMP
// @DisplayName: Controller pitch damping
2018-12-20 19:42:08 -04:00
// @Description: Damping gain for pitch control from TECS control. Increasing may correct for oscillations in speed and height, but too much may cause additional oscillation and degraded control.
2015-09-15 22:17:28 -03:00
// @Range: 0.1 1.0
// @Increment: 0.1
// @User: Advanced
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " PTCH_DAMP " , 10 , AP_TECS , _ptchDamp , 0.0f ) ,
2013-06-26 05:38:40 -03:00
// @Param: SINK_MAX
// @DisplayName: Maximum Descent Rate (metres/sec)
2018-12-20 19:42:08 -04:00
// @Description: Maximum demanded descent rate. Do not set higher than the vertical speed the aircraft can maintain at THR_MIN, TECS_PITCH_MIN, and ARSPD_FBW_MAX.
2015-09-15 22:17:28 -03:00
// @Increment: 0.1
// @Range: 0.0 20.0
2020-09-13 19:42:54 -03:00
// @User: Standard
2013-06-26 07:49:08 -03:00
AP_GROUPINFO ( " SINK_MAX " , 11 , AP_TECS , _maxSinkRate , 5.0f ) ,
2013-06-26 05:38:40 -03:00
2014-02-22 13:54:37 -04:00
// @Param: LAND_ARSPD
// @DisplayName: Airspeed during landing approach (m/s)
2014-03-19 17:59:10 -03:00
// @Description: When performing an autonomus landing, this value is used as the goal airspeed during approach. Note that this parameter is not useful if your platform does not have an airspeed sensor (use TECS_LAND_THR instead). If negative then this value is not used during landing.
2014-11-26 04:02:51 -04:00
// @Range: -1 127
2014-02-22 13:54:37 -04:00
// @Increment: 1
2020-09-13 19:42:54 -03:00
// @User: Standard
2014-02-22 13:54:37 -04:00
AP_GROUPINFO ( " LAND_ARSPD " , 12 , AP_TECS , _landAirspeed , - 1 ) ,
2014-08-11 03:45:06 -03:00
// @Param: LAND_THR
2014-02-22 13:54:37 -04:00
// @DisplayName: Cruise throttle during landing approach (percentage)
2016-01-06 16:13:07 -04:00
// @Description: Use this parameter instead of LAND_ARSPD if your platform does not have an airspeed sensor. It is the cruise throttle during landing approach. If this value is negative then it is disabled and TECS_LAND_ARSPD is used instead.
2015-12-27 01:08:32 -04:00
// @Range: -1 100
2014-02-22 13:54:37 -04:00
// @Increment: 0.1
2020-09-13 19:42:54 -03:00
// @User: Standard
2014-02-22 13:54:37 -04:00
AP_GROUPINFO ( " LAND_THR " , 13 , AP_TECS , _landThrottle , - 1 ) ,
2014-04-09 13:50:52 -03:00
// @Param: LAND_SPDWGT
// @DisplayName: Weighting applied to speed control during landing.
2016-01-08 22:49:59 -04:00
// @Description: Same as SPDWEIGHT parameter, with the exception that this parameter is applied during landing flight stages. A value closer to 2 will result in the plane ignoring height error during landing and our experience has been that the plane will therefore keep the nose up -- sometimes good for a glider landing (with the side effect that you will likely glide a ways past the landing point). A value closer to 0 results in the plane ignoring speed error -- use caution when lowering the value below 1 -- ignoring speed could result in a stall. Values between 0 and 2 are valid values for a fixed landing weight. When using -1 the weight will be scaled during the landing. At the start of the landing approach it starts with TECS_SPDWEIGHT and scales down to 0 by the time you reach the land point. Example: Halfway down the landing approach you'll effectively have a weight of TECS_SPDWEIGHT/2.
// @Range: -1.0 2.0
2015-09-15 22:17:28 -03:00
// @Increment: 0.1
// @User: Advanced
2016-01-08 22:48:49 -04:00
AP_GROUPINFO ( " LAND_SPDWGT " , 14 , AP_TECS , _spdWeightLand , - 1.0f ) ,
2014-04-09 13:50:52 -03:00
2014-08-05 22:46:01 -03:00
// @Param: PITCH_MAX
// @DisplayName: Maximum pitch in auto flight
2018-12-25 23:54:15 -04:00
// @Description: Overrides LIM_PITCH_MAX in automatic throttle modes to reduce climb rates. Uses LIM_PITCH_MAX if set to 0. For proper TECS tuning, set to the angle that the aircraft can climb at TRIM_ARSPD_CM and THR_MAX.
2015-09-15 22:17:28 -03:00
// @Range: 0 45
// @Increment: 1
// @User: Advanced
2018-08-17 04:02:16 -03:00
AP_GROUPINFO ( " PITCH_MAX " , 15 , AP_TECS , _pitch_max , 15 ) ,
2014-08-05 22:46:01 -03:00
// @Param: PITCH_MIN
// @DisplayName: Minimum pitch in auto flight
2018-12-25 23:54:15 -04:00
// @Description: Overrides LIM_PITCH_MIN in automatic throttle modes to reduce descent rates. Uses LIM_PITCH_MIN if set to 0. For proper TECS tuning, set to the angle that the aircraft can descend at without overspeeding.
2015-09-15 22:17:28 -03:00
// @Range: -45 0
// @Increment: 1
// @User: Advanced
2014-08-05 22:46:01 -03:00
AP_GROUPINFO ( " PITCH_MIN " , 16 , AP_TECS , _pitch_min , 0 ) ,
2014-08-11 03:45:06 -03:00
// @Param: LAND_SINK
// @DisplayName: Sink rate for final landing stage
// @Description: The sink rate in meters/second for the final stage of landing.
2015-09-15 22:17:28 -03:00
// @Range: 0.0 2.0
// @Increment: 0.1
// @User: Advanced
2014-08-11 03:45:06 -03:00
AP_GROUPINFO ( " LAND_SINK " , 17 , AP_TECS , _land_sink , 0.25f ) ,
2014-11-28 04:14:00 -04:00
// @Param: LAND_TCONST
2014-08-27 01:28:39 -03:00
// @DisplayName: Land controller time constant (sec)
// @Description: This is the time constant of the TECS control algorithm when in final landing stage of flight. It should be smaller than TECS_TIME_CONST to allow for faster flare
2015-09-15 22:17:28 -03:00
// @Range: 1.0 5.0
// @Increment: 0.2
// @User: Advanced
2014-08-27 01:28:39 -03:00
AP_GROUPINFO ( " LAND_TCONST " , 18 , AP_TECS , _landTimeConst , 2.0f ) ,
2014-11-28 04:32:15 -04:00
// @Param: LAND_DAMP
// @DisplayName: Controller sink rate to pitch gain during flare
// @Description: This is the sink rate gain for the pitch demand loop when in final landing stage of flight. It should be larger than TECS_PTCH_DAMP to allow for better sink rate control during flare.
// @Range: 0.1 1.0
// @Increment: 0.1
// @User: Advanced
2014-11-28 04:47:17 -04:00
AP_GROUPINFO ( " LAND_DAMP " , 19 , AP_TECS , _landDamp , 0.5f ) ,
2014-11-28 04:32:15 -04:00
2015-03-19 00:46:32 -03:00
// @Param: LAND_PMAX
// @DisplayName: Maximum pitch during final stage of landing
// @Description: This limits the pitch used during the final stage of automatic landing. During the final landing stage most planes need to keep their pitch small to avoid stalling. A maximum of 10 degrees is usually good. A value of zero means to use the normal pitch limits.
2016-02-10 12:41:29 -04:00
// @Range: -5 40
2015-09-15 22:17:28 -03:00
// @Increment: 1
// @User: Advanced
2015-03-19 00:46:32 -03:00
AP_GROUPINFO ( " LAND_PMAX " , 20 , AP_TECS , _land_pitch_max , 10 ) ,
2016-01-30 03:23:05 -04:00
// @Param: APPR_SMAX
// @DisplayName: Sink rate max for landing approach stage
// @Description: The sink rate max for the landing approach stage of landing. This will need to be large for steep landing approaches especially when using reverse thrust. If 0, then use TECS_SINK_MAX.
// @Range: 0.0 20.0
// @Units: m/s
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO ( " APPR_SMAX " , 21 , AP_TECS , _maxSinkRate_approach , 0 ) ,
2016-02-12 15:38:40 -04:00
// @Param: LAND_SRC
// @DisplayName: Land sink rate change
// @Description: When zero, the flare sink rate (TECS_LAND_SINK) is a fixed sink demand. With this enabled the flare sinkrate will increase/decrease the flare sink demand as you get further beyond the LAND waypoint. Has no effect before the waypoint. This value is added to TECS_LAND_SINK proportional to distance traveled after wp. With an increasing sink rate you can still land in a given distance if you're traveling too fast and cruise passed the land point. A positive value will force the plane to land sooner proportional to distance passed land point. A negative number will tell the plane to slowly climb allowing for a pitched-up stall landing. Recommend 0.2 as initial value.
// @Range: -2.0 2.0
// @Units: m/s/m
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO ( " LAND_SRC " , 22 , AP_TECS , _land_sink_rate_change , 0 ) ,
2016-02-23 18:46:55 -04:00
// @Param: LAND_TDAMP
// @DisplayName: Controller throttle damping when landing
2021-03-01 19:10:42 -04:00
// @Description: Damping gain for the throttle demand loop during an auto-landing. Same as TECS_THR_DAMP but only in effect during an auto-land. Increase to add throttle activity to dampen oscillations in speed and height. When set to 0 landing throttle damping is controlled by TECS_THR_DAMP.
2016-02-23 18:46:55 -04:00
// @Range: 0.1 1.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO ( " LAND_TDAMP " , 23 , AP_TECS , _land_throttle_damp , 0 ) ,
2016-02-23 20:20:25 -04:00
// @Param: LAND_IGAIN
// @DisplayName: Controller integrator during landing
// @Description: This is the integrator gain on the control loop during landing. When set to 0 then TECS_INTEG_GAIN is used. Increase to increase the rate at which speed and height offsets are trimmed out. Typically values lower than TECS_INTEG_GAIN work best
// @Range: 0.0 0.5
// @Increment: 0.02
// @User: Advanced
AP_GROUPINFO ( " LAND_IGAIN " , 24 , AP_TECS , _integGain_land , 0 ) ,
// @Param: TKOFF_IGAIN
// @DisplayName: Controller integrator during takeoff
// @Description: This is the integrator gain on the control loop during takeoff. When set to 0 then TECS_INTEG_GAIN is used. Increase to increase the rate at which speed and height offsets are trimmed out. Typically values higher than TECS_INTEG_GAIN work best
// @Range: 0.0 0.5
// @Increment: 0.02
// @User: Advanced
AP_GROUPINFO ( " TKOFF_IGAIN " , 25 , AP_TECS , _integGain_takeoff , 0 ) ,
2016-02-24 22:18:14 -04:00
// @Param: LAND_PDAMP
2016-07-26 02:35:15 -03:00
// @DisplayName: Pitch damping gain when landing
// @Description: This is the damping gain for the pitch demand loop during landing. Increase to add damping to correct for oscillations in speed and height. If set to 0 then TECS_PTCH_DAMP will be used instead.
2016-02-24 22:18:14 -04:00
// @Range: 0.1 1.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO ( " LAND_PDAMP " , 26 , AP_TECS , _land_pitch_damp , 0 ) ,
2016-10-19 23:22:39 -03:00
// @Param: SYNAIRSPEED
// @DisplayName: Enable the use of synthetic airspeed
// @Description: This enable the use of synthetic airspeed for aircraft that don't have a real airspeed sensor. This is useful for development testing where the user is aware of the considerable limitations of the synthetic airspeed system, such as very poor estimates when a wind estimate is not accurate. Do not enable this option unless you fully understand the limitations of a synthetic airspeed estimate.
// @Values: 0:Disable,1:Enable
// @User: Advanced
AP_GROUPINFO ( " SYNAIRSPEED " , 27 , AP_TECS , _use_synthetic_airspeed , 0 ) ,
2019-03-02 07:30:06 -04:00
// @Param: OPTIONS
// @DisplayName: Extra TECS options
// @Description: This allows the enabling of special features in the speed/height controller
// @Bitmask: 0:GliderOnly
// @User: Advanced
AP_GROUPINFO ( " OPTIONS " , 28 , AP_TECS , _options , 0 ) ,
2019-10-26 00:35:49 -03:00
2018-10-18 07:42:57 -03:00
// @Param: PTCH_FF_V0
// @DisplayName: Baseline airspeed for pitch feed-forward.
2020-03-01 14:38:32 -04:00
// @Description: This parameter sets the airspeed at which no feed-forward is applied between demanded airspeed and pitch. It should correspond to the airspeed in metres per second at which the plane glides at neutral pitch including STAB_PITCH_DOWN.
// @Range: 5.0 50.0
2018-10-18 07:42:57 -03:00
// @User: Advanced
AP_GROUPINFO ( " PTCH_FF_V0 " , 29 , AP_TECS , _pitch_ff_v0 , 12.0 ) ,
// @Param: PTCH_FF_K
// @DisplayName: Gain for pitch feed-forward.
2020-03-01 14:38:32 -04:00
// @Description: This parameter sets the gain between demanded airspeed and pitch. It has units of radians per metre per second and should generally be negative. A good starting value is -0.04 for gliders and -0.08 for draggy airframes. The default (0.0) disables this feed-forward.
// @Range: -5.0 0.0
2018-10-18 07:42:57 -03:00
// @User: Advanced
AP_GROUPINFO ( " PTCH_FF_K " , 30 , AP_TECS , _pitch_ff_k , 0.0 ) ,
2013-06-26 05:38:40 -03:00
AP_GROUPEND
} ;
/*
* Written by Paul Riseborough 2013 to provide :
* - Combined control of speed and height using throttle to control
* total energy and pitch angle to control exchange of energy between
* potential and kinetic .
* Selectable speed or height priority modes when calculating pitch angle
* - Fallback mode when no airspeed measurement is available that
2015-09-15 22:17:28 -03:00
* sets throttle based on height rate demand and switches pitch angle control to
2013-06-26 05:38:40 -03:00
* height priority
2015-09-15 22:17:28 -03:00
* - Underspeed protection that demands maximum throttle and switches pitch angle control
2013-06-26 05:38:40 -03:00
* to speed priority mode
* - Relative ease of tuning through use of intuitive time constant , integrator and damping gains and the use
* of easy to measure aircraft performance data
*
*/
2016-04-21 03:45:57 -03:00
void AP_TECS : : update_50hz ( void )
2015-09-15 22:17:28 -03:00
{
// Implement third order complementary filter for height and height rate
2016-07-05 17:58:48 -03:00
// estimated height rate = _climb_rate
2015-09-15 22:17:28 -03:00
// estimated height above field elevation = _height
// Reference Paper :
2016-07-05 17:58:48 -03:00
// Optimizing the Gains of the Baro-Inertial Vertical Channel
2015-09-15 22:17:28 -03:00
// Widnall W.S, Sinha P.K,
// AIAA Journal of Guidance and Control, 78-1307R
2013-06-26 05:38:40 -03:00
2015-09-15 22:13:11 -03:00
/*
if we have a vertical position estimate from the EKF then use
it , otherwise use barometric altitude
*/
2017-01-30 15:07:48 -04:00
_ahrs . get_relative_position_D_home ( _height ) ;
_height * = - 1.0f ;
2015-09-15 22:17:28 -03:00
2013-06-26 05:38:40 -03:00
// Calculate time in seconds since last update
2016-05-13 01:48:55 -03:00
uint64_t now = AP_HAL : : micros64 ( ) ;
float DT = ( now - _update_50hz_last_usec ) * 1.0e-6 f ;
2015-09-15 22:17:28 -03:00
if ( DT > 1.0f ) {
_climb_rate = 0.0f ;
_height_filter . dd_height = 0.0f ;
DT = 0.02f ; // when first starting TECS, use a
// small time constant
}
_update_50hz_last_usec = now ;
// Use inertial nav verical velocity and height if available
Vector3f velned ;
if ( _ahrs . get_velocity_NED ( velned ) ) {
2015-09-15 22:13:11 -03:00
// if possible use the EKF vertical velocity
2015-09-15 22:17:28 -03:00
_climb_rate = - velned . z ;
} else {
2015-09-15 22:13:11 -03:00
/*
use a complimentary filter to calculate climb_rate . This is
designed to minimise lag
*/
2018-03-05 18:08:03 -04:00
const float baro_alt = AP : : baro ( ) . get_altitude ( ) ;
2015-09-15 22:17:28 -03:00
// Get height acceleration
float hgt_ddot_mea = - ( _ahrs . get_accel_ef ( ) . z + GRAVITY_MSS ) ;
// Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega
float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega ;
float hgt_err = baro_alt - _height_filter . height ;
float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega ;
_height_filter . dd_height + = integ1_input * DT ;
float integ2_input = _height_filter . dd_height + hgt_ddot_mea + hgt_err * omega2 * 3.0f ;
_climb_rate + = integ2_input * DT ;
float integ3_input = _climb_rate + hgt_err * _hgtCompFiltOmega * 3.0f ;
// If more than 1 second has elapsed since last update then reset the integrator state
// to the measured height
if ( DT > 1.0f ) {
_height_filter . height = _height ;
} else {
_height_filter . height + = integ3_input * DT ;
}
}
// Update and average speed rate of change
2014-02-07 04:08:33 -04:00
// Get DCM
2015-12-10 18:07:30 -04:00
const Matrix3f & rotMat = _ahrs . get_rotation_body_to_ned ( ) ;
2015-09-15 22:17:28 -03:00
// Calculate speed rate of change
2018-03-10 06:01:03 -04:00
float temp = rotMat . c . x * GRAVITY_MSS + AP : : ins ( ) . get_accel ( ) . x ;
2015-09-15 22:17:28 -03:00
// take 5 point moving average
2014-02-07 04:08:33 -04:00
_vel_dot = _vdot_filter . apply ( temp ) ;
2013-06-26 05:38:40 -03:00
}
2014-11-11 22:32:46 -04:00
void AP_TECS : : _update_speed ( float load_factor )
2013-06-26 05:38:40 -03:00
{
// Calculate time in seconds since last update
2016-05-13 01:48:55 -03:00
uint64_t now = AP_HAL : : micros64 ( ) ;
float DT = ( now - _update_speed_last_usec ) * 1.0e-6 f ;
2015-09-15 22:17:28 -03:00
_update_speed_last_usec = now ;
2013-06-26 05:38:40 -03:00
// Convert equivalent airspeeds to true airspeeds
2013-08-14 01:58:49 -03:00
float EAS2TAS = _ahrs . get_EAS2TAS ( ) ;
2016-04-13 13:10:29 -03:00
_TAS_dem = _EAS_dem * EAS2TAS ;
2016-08-07 21:48:18 -03:00
_TASmax = aparm . airspeed_max * EAS2TAS ;
_TASmin = aparm . airspeed_min * EAS2TAS ;
2014-11-12 23:05:33 -04:00
if ( aparm . stall_prevention ) {
// when stall prevention is active we raise the mimimum
// airspeed based on aerodynamic load factor
_TASmin * = load_factor ;
}
2016-02-08 22:58:05 -04:00
if ( _TASmax < _TASmin ) {
_TASmax = _TASmin ;
}
if ( _TASmin > _TAS_dem ) {
_TASmin = _TAS_dem ;
}
2013-06-26 05:38:40 -03:00
// Reset states of time since last update is too large
2014-10-22 02:15:43 -03:00
if ( DT > 1.0f ) {
2016-05-13 02:24:25 -03:00
_TAS_state = ( _EAS * EAS2TAS ) ;
_integDTAS_state = 0.0f ;
2015-09-15 22:17:28 -03:00
DT = 0.1f ; // when first starting TECS, use a
// small time constant
2013-06-26 05:38:40 -03:00
}
// Get airspeed or default to halfway between min and max if
// airspeed is not being used and set speed rate to zero
2016-10-19 23:22:39 -03:00
bool use_airspeed = _use_synthetic_airspeed_once | | _use_synthetic_airspeed . get ( ) | | _ahrs . airspeed_sensor_enabled ( ) ;
2020-01-06 20:47:11 -04:00
if ( ! use_airspeed | | ! _ahrs . airspeed_estimate ( _EAS ) ) {
2013-06-26 05:38:40 -03:00
// If no airspeed available use average of min and max
2016-08-07 21:48:18 -03:00
_EAS = 0.5f * ( aparm . airspeed_min . get ( ) + ( float ) aparm . airspeed_max . get ( ) ) ;
2013-06-26 05:38:40 -03:00
}
// Implement a second order complementary filter to obtain a
// smoothed airspeed estimate
2016-05-13 02:24:25 -03:00
// airspeed estimate is held in _TAS_state
float aspdErr = ( _EAS * EAS2TAS ) - _TAS_state ;
float integDTAS_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega ;
2013-06-26 05:38:40 -03:00
// Prevent state from winding up
2016-05-13 02:24:25 -03:00
if ( _TAS_state < 3.1f ) {
integDTAS_input = MAX ( integDTAS_input , 0.0f ) ;
2013-06-26 05:38:40 -03:00
}
2016-05-13 02:24:25 -03:00
_integDTAS_state = _integDTAS_state + integDTAS_input * DT ;
float TAS_input = _integDTAS_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f ;
_TAS_state = _TAS_state + TAS_input * DT ;
2013-06-26 05:38:40 -03:00
// limit the airspeed to a minimum of 3 m/s
2016-05-13 02:24:25 -03:00
_TAS_state = MAX ( _TAS_state , 3.0f ) ;
2013-06-26 05:38:40 -03:00
}
void AP_TECS : : _update_speed_demand ( void )
{
2015-09-15 22:17:28 -03:00
// Set the airspeed demand to the minimum value if an underspeed condition exists
// or a bad descent condition exists
// This will minimise the rate of descent resulting from an engine failure,
// enable the maximum climb rate to be achieved and prevent continued full power descent
// into the ground due to an unachievable airspeed value
2016-03-25 18:41:09 -03:00
if ( ( _flags . badDescent ) | | ( _flags . underspeed ) )
2015-09-15 22:17:28 -03:00
{
_TAS_dem = _TASmin ;
}
2014-11-11 22:32:46 -04:00
// Constrain speed demand, taking into account the load factor
2013-06-26 05:38:40 -03:00
_TAS_dem = constrain_float ( _TAS_dem , _TASmin , _TASmax ) ;
// calculate velocity rate limits based on physical performance limits
2015-09-15 22:17:28 -03:00
// provision to use a different rate limit if bad descent or underspeed condition exists
// Use 50% of maximum energy rate to allow margin for total energy contgroller
2019-03-31 19:28:42 -03:00
const float velRateMax = 0.5f * _STEdot_max / _TAS_state ;
const float velRateMin = 0.5f * _STEdot_min / _TAS_state ;
const float TAS_dem_previous = _TAS_dem_adj ;
// assume fixed 10Hz call rate
const float dt = 0.1 ;
2015-09-15 22:17:28 -03:00
2013-06-26 05:38:40 -03:00
// Apply rate limit
2019-03-31 19:28:42 -03:00
if ( ( _TAS_dem - TAS_dem_previous ) > ( velRateMax * dt ) )
2013-06-26 05:38:40 -03:00
{
2019-03-31 19:28:42 -03:00
_TAS_dem_adj = TAS_dem_previous + velRateMax * dt ;
2013-06-26 05:38:40 -03:00
_TAS_rate_dem = velRateMax ;
}
2019-03-31 19:28:42 -03:00
else if ( ( _TAS_dem - TAS_dem_previous ) < ( velRateMin * dt ) )
2013-06-26 05:38:40 -03:00
{
2019-03-31 19:28:42 -03:00
_TAS_dem_adj = TAS_dem_previous + velRateMin * dt ;
2013-06-26 05:38:40 -03:00
_TAS_rate_dem = velRateMin ;
}
else
{
2019-03-31 19:28:42 -03:00
_TAS_rate_dem = ( _TAS_dem - TAS_dem_previous ) / dt ;
2013-06-26 05:38:40 -03:00
_TAS_dem_adj = _TAS_dem ;
}
// Constrain speed demand again to protect against bad values on initialisation.
_TAS_dem_adj = constrain_float ( _TAS_dem_adj , _TASmin , _TASmax ) ;
}
void AP_TECS : : _update_height_demand ( void )
{
2015-09-15 22:17:28 -03:00
// Apply 2 point moving average to demanded height
_hgt_dem = 0.5f * ( _hgt_dem + _hgt_dem_in_old ) ;
_hgt_dem_in_old = _hgt_dem ;
2013-06-26 05:38:40 -03:00
2016-01-30 03:23:05 -04:00
float max_sink_rate = _maxSinkRate ;
2016-04-19 21:09:17 -03:00
if ( _maxSinkRate_approach > 0 & & _flags . is_doing_auto_land ) {
2016-01-30 03:23:05 -04:00
// special sink rate for approach to accommodate steep slopes and reverse thrust.
// A special check must be done to see if we're LANDing on approach but also if
// we're in that tiny window just starting NAV_LAND but still in NORMAL mode. If
// we have a steep slope with a short approach we'll want to allow acquiring the
// glide slope right away.
max_sink_rate = _maxSinkRate_approach ;
}
2013-06-26 05:38:40 -03:00
// Limit height rate of change
2015-09-15 22:17:28 -03:00
if ( ( _hgt_dem - _hgt_dem_prev ) > ( _maxClimbRate * 0.1f ) )
2013-06-26 05:38:40 -03:00
{
_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f ;
}
2016-01-30 03:23:05 -04:00
else if ( ( _hgt_dem - _hgt_dem_prev ) < ( - max_sink_rate * 0.1f ) )
2013-06-26 05:38:40 -03:00
{
2016-01-30 03:23:05 -04:00
_hgt_dem = _hgt_dem_prev - max_sink_rate * 0.1f ;
2013-06-26 05:38:40 -03:00
}
_hgt_dem_prev = _hgt_dem ;
2015-09-15 22:17:28 -03:00
// Apply first order lag to height demand
_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last ;
2014-08-11 03:45:06 -03:00
2017-01-10 01:46:51 -04:00
// when flaring force height rate demand to the
2014-11-28 03:39:34 -04:00
// configured sink rate and adjust the demanded height to
// be kinematically consistent with the height rate.
2017-01-10 01:46:51 -04:00
if ( _landing . is_flaring ( ) ) {
2016-05-13 02:24:25 -03:00
_integSEB_state = 0 ;
2014-08-27 07:13:01 -03:00
if ( _flare_counter = = 0 ) {
_hgt_rate_dem = _climb_rate ;
2014-11-28 03:39:34 -04:00
_land_hgt_dem = _hgt_dem_adj ;
2014-08-27 07:13:01 -03:00
}
2016-02-12 15:38:40 -04:00
// adjust the flare sink rate to increase/decrease as your travel further beyond the land wp
float land_sink_rate_adj = _land_sink + _land_sink_rate_change * _distance_beyond_land_wp ;
2014-08-29 16:13:18 -03:00
// bring it in over 1s to prevent overshoot
if ( _flare_counter < 10 ) {
2016-02-12 15:38:40 -04:00
_hgt_rate_dem = _hgt_rate_dem * 0.8f - 0.2f * land_sink_rate_adj ;
2014-08-27 07:13:01 -03:00
_flare_counter + + ;
} else {
2016-02-12 15:38:40 -04:00
_hgt_rate_dem = - land_sink_rate_adj ;
2014-08-27 07:13:01 -03:00
}
2014-11-28 03:39:34 -04:00
_land_hgt_dem + = 0.1f * _hgt_rate_dem ;
_hgt_dem_adj = _land_hgt_dem ;
} else {
2014-08-27 07:13:01 -03:00
_hgt_rate_dem = ( _hgt_dem_adj - _hgt_dem_adj_last ) / 0.1f ;
_flare_counter = 0 ;
}
2014-12-17 18:58:12 -04:00
// for landing approach we will predict ahead by the time constant
// plus the lag produced by the first order filter. This avoids a
// lagged height demand while constantly descending which causes
// us to consistently be above the desired glide slope. This will
// be replaced with a better zero-lag filter in the future.
float new_hgt_dem = _hgt_dem_adj ;
2016-04-19 21:09:17 -03:00
if ( _flags . is_doing_auto_land ) {
2016-07-15 17:38:59 -03:00
if ( hgt_dem_lag_filter_slew < 1 ) {
hgt_dem_lag_filter_slew + = 0.1f ; // increment at 10Hz to gradually apply the compensation at first
} else {
hgt_dem_lag_filter_slew = 1 ;
}
new_hgt_dem + = hgt_dem_lag_filter_slew * ( _hgt_dem_adj - _hgt_dem_adj_last ) * 10.0f * ( timeConstant ( ) + 1 ) ;
} else {
hgt_dem_lag_filter_slew = 0 ;
2014-12-17 18:58:12 -04:00
}
2014-12-13 16:25:18 -04:00
_hgt_dem_adj_last = _hgt_dem_adj ;
2014-12-17 18:58:12 -04:00
_hgt_dem_adj = new_hgt_dem ;
2013-06-26 05:38:40 -03:00
}
2015-09-15 22:17:28 -03:00
void AP_TECS : : _detect_underspeed ( void )
2013-06-26 05:38:40 -03:00
{
2016-03-25 17:43:39 -03:00
// see if we can clear a previous underspeed condition. We clear
// it if we are now more than 15% above min speed, and haven't
// been below min speed for at least 3 seconds.
2016-03-25 18:41:09 -03:00
if ( _flags . underspeed & &
2016-05-13 02:24:25 -03:00
_TAS_state > = _TASmin * 1.15f & &
2016-03-25 17:43:39 -03:00
AP_HAL : : millis ( ) - _underspeed_start_ms > 3000U ) {
2016-03-25 18:41:09 -03:00
_flags . underspeed = false ;
2016-03-25 17:43:39 -03:00
}
2016-12-13 22:19:56 -04:00
if ( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_VTOL ) {
2016-03-25 18:41:09 -03:00
_flags . underspeed = false ;
2016-05-13 02:24:25 -03:00
} else if ( ( ( _TAS_state < _TASmin * 0.9f ) & &
2015-09-15 22:17:28 -03:00
( _throttle_dem > = _THRmaxf * 0.95f ) & &
2017-01-10 01:46:51 -04:00
! _landing . is_flaring ( ) ) | |
2016-03-25 18:41:09 -03:00
( ( _height < _hgt_dem_adj ) & & _flags . underspeed ) )
2013-06-26 05:38:40 -03:00
{
2016-03-25 18:41:09 -03:00
_flags . underspeed = true ;
2016-05-13 02:24:25 -03:00
if ( _TAS_state < _TASmin * 0.9f ) {
2016-03-25 17:43:39 -03:00
// reset start time as we are still underspeed
_underspeed_start_ms = AP_HAL : : millis ( ) ;
}
2013-06-26 05:38:40 -03:00
}
else
{
2016-07-19 19:21:08 -03:00
// this clears underspeed if we reach our demanded height and
// we are either below 95% throttle or we above 90% of min
// airspeed
2016-03-25 18:41:09 -03:00
_flags . underspeed = false ;
2013-06-26 05:38:40 -03:00
}
}
2015-09-15 22:17:28 -03:00
void AP_TECS : : _update_energies ( void )
2013-06-26 05:38:40 -03:00
{
// Calculate specific energy demands
_SPE_dem = _hgt_dem_adj * GRAVITY_MSS ;
_SKE_dem = 0.5f * _TAS_dem_adj * _TAS_dem_adj ;
// Calculate specific energy rate demands
_SPEdot_dem = _hgt_rate_dem * GRAVITY_MSS ;
2016-05-13 02:24:25 -03:00
_SKEdot_dem = _TAS_state * _TAS_rate_dem ;
2013-06-26 05:38:40 -03:00
// Calculate specific energy
2015-09-15 22:13:11 -03:00
_SPE_est = _height * GRAVITY_MSS ;
2016-05-13 02:24:25 -03:00
_SKE_est = 0.5f * _TAS_state * _TAS_state ;
2013-06-26 05:38:40 -03:00
// Calculate specific energy rate
2014-08-27 07:13:01 -03:00
_SPEdot = _climb_rate * GRAVITY_MSS ;
2016-05-13 02:24:25 -03:00
_SKEdot = _TAS_state * _vel_dot ;
2014-03-08 17:50:39 -04:00
2013-06-26 05:38:40 -03:00
}
2014-08-27 01:28:39 -03:00
/*
current time constant . It is lower in landing to try to give a precise approach
*/
2015-03-14 23:52:17 -03:00
float AP_TECS : : timeConstant ( void ) const
2014-08-27 01:28:39 -03:00
{
2016-04-19 21:09:17 -03:00
if ( _flags . is_doing_auto_land ) {
2015-03-14 23:52:17 -03:00
if ( _landTimeConst < 0.1f ) {
return 0.1f ;
}
2014-08-27 01:28:39 -03:00
return _landTimeConst ;
}
2015-03-14 23:52:17 -03:00
if ( _timeConst < 0.1f ) {
return 0.1f ;
}
2014-08-27 01:28:39 -03:00
return _timeConst ;
}
2016-07-05 23:15:55 -03:00
/*
calculate throttle demand - airspeed enabled case
*/
void AP_TECS : : _update_throttle_with_airspeed ( void )
2013-06-26 05:38:40 -03:00
{
2014-03-08 17:50:39 -04:00
// Calculate limits to be applied to potential energy error to prevent over or underspeed occurring due to large height errors
float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem ;
float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem ;
2017-10-21 03:47:35 -03:00
if ( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_VTOL ) {
/*
when we are in a VTOL state then we ignore potential energy
errors as we have vertical motors that interfere with the
total energy calculation .
*/
SPE_err_max = SPE_err_min = 0 ;
}
2014-03-08 17:50:39 -04:00
// Calculate total energy error
_STE_error = constrain_float ( ( _SPE_dem - _SPE_est ) , SPE_err_min , SPE_err_max ) + _SKE_dem - _SKE_est ;
2013-06-26 05:38:40 -03:00
float STEdot_dem = constrain_float ( ( _SPEdot_dem + _SKEdot_dem ) , _STEdot_min , _STEdot_max ) ;
float STEdot_error = STEdot_dem - _SPEdot - _SKEdot ;
2015-09-15 22:17:28 -03:00
// Apply 0.5 second first order filter to STEdot_error
// This is required to remove accelerometer noise from the measurement
STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast ;
_STEdotErrLast = STEdot_error ;
2013-06-26 05:38:40 -03:00
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
2016-03-25 18:41:09 -03:00
if ( _flags . underspeed )
2013-06-26 05:38:40 -03:00
{
2014-11-04 16:55:18 -04:00
_throttle_dem = 1.0f ;
2013-06-26 05:38:40 -03:00
}
2018-10-18 09:51:21 -03:00
else if ( _flags . is_gliding )
{
_throttle_dem = 0.0f ;
}
2013-06-26 05:38:40 -03:00
else
{
2015-09-15 22:17:28 -03:00
// Calculate gain scaler from specific energy error to throttle
2017-08-11 23:27:30 -03:00
// (_STEdot_max - _STEdot_min) / (_THRmaxf - _THRminf) is the derivative of STEdot wrt throttle measured across the max allowed throttle range.
float K_STE2Thr = 1 / ( timeConstant ( ) * ( _STEdot_max - _STEdot_min ) / ( _THRmaxf - _THRminf ) ) ;
2013-06-26 05:38:40 -03:00
// Calculate feed-forward throttle
float ff_throttle = 0 ;
2014-02-22 13:54:37 -04:00
float nomThr = aparm . throttle_cruise * 0.01f ;
2015-12-10 18:07:30 -04:00
const Matrix3f & rotMat = _ahrs . get_rotation_body_to_ned ( ) ;
2015-09-15 22:17:28 -03:00
// Use the demanded rate of change of total energy as the feed-forward demand, but add
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf ( ( rotMat . a . y * rotMat . a . y ) + ( rotMat . b . y * rotMat . b . y ) ) ;
STEdot_dem = STEdot_dem + _rollComp * ( 1.0f / constrain_float ( cosPhi * cosPhi , 0.1f , 1.0f ) - 1.0f ) ;
ff_throttle = nomThr + STEdot_dem / ( _STEdot_max - _STEdot_min ) * ( _THRmaxf - _THRminf ) ;
2013-06-26 05:38:40 -03:00
2015-09-15 22:17:28 -03:00
// Calculate PD + FF throttle
2016-02-23 18:46:55 -04:00
float throttle_damp = _thrDamp ;
2016-03-25 18:41:09 -03:00
if ( _flags . is_doing_auto_land & & ! is_zero ( _land_throttle_damp ) ) {
2016-02-23 18:46:55 -04:00
throttle_damp = _land_throttle_damp ;
}
_throttle_dem = ( _STE_error + STEdot_error * throttle_damp ) * K_STE2Thr + ff_throttle ;
2013-06-26 05:38:40 -03:00
2014-11-03 17:59:51 -04:00
// Constrain throttle demand
_throttle_dem = constrain_float ( _throttle_dem , _THRminf , _THRmaxf ) ;
2016-03-03 17:41:34 -04:00
float THRminf_clipped_to_zero = constrain_float ( _THRminf , 0 , _THRmaxf ) ;
2014-11-03 17:59:51 -04:00
// Calculate integrator state upper and lower limits
// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
// Additionally constrain the integrator state amplitude so that the integrator comes off limits faster.
2016-03-03 17:41:34 -04:00
float maxAmp = 0.5f * ( _THRmaxf - THRminf_clipped_to_zero ) ;
2014-11-03 17:59:51 -04:00
float integ_max = constrain_float ( ( _THRmaxf - _throttle_dem + 0.1f ) , - maxAmp , maxAmp ) ;
float integ_min = constrain_float ( ( _THRminf - _throttle_dem - 0.1f ) , - maxAmp , maxAmp ) ;
2013-06-26 05:38:40 -03:00
2015-09-15 22:17:28 -03:00
// Calculate integrator state, constraining state
// Set integrator to a max throttle value during climbout
2016-05-13 02:24:25 -03:00
_integTHR_state = _integTHR_state + ( _STE_error * _get_i_gain ( ) ) * _DT * K_STE2Thr ;
2016-12-13 23:18:48 -04:00
if ( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND )
2015-09-15 22:17:28 -03:00
{
2016-07-05 23:15:55 -03:00
if ( ! _flags . reached_speed_takeoff ) {
// ensure we run at full throttle until we reach the target airspeed
_throttle_dem = MAX ( _throttle_dem , _THRmaxf - _integTHR_state ) ;
}
2016-05-13 02:24:25 -03:00
_integTHR_state = integ_max ;
2015-09-15 22:17:28 -03:00
}
else
{
2016-05-13 02:24:25 -03:00
_integTHR_state = constrain_float ( _integTHR_state , integ_min , integ_max ) ;
2015-09-15 22:17:28 -03:00
}
2020-03-03 14:12:59 -04:00
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
2020-07-01 06:56:54 -03:00
int8_t throttle_slewrate = aparm . throttle_slewrate ;
2020-07-17 17:28:46 -03:00
if ( _landing . is_on_approach ( ) ) {
const int8_t land_slewrate = _landing . get_throttle_slewrate ( ) ;
if ( land_slewrate > 0 ) {
throttle_slewrate = land_slewrate ;
}
2020-07-01 06:56:54 -03:00
}
if ( throttle_slewrate ! = 0 ) {
float thrRateIncr = _DT * ( _THRmaxf - THRminf_clipped_to_zero ) * throttle_slewrate * 0.01f ;
2020-03-03 14:12:59 -04:00
_throttle_dem = constrain_float ( _throttle_dem ,
_last_throttle_dem - thrRateIncr ,
_last_throttle_dem + thrRateIncr ) ;
_last_throttle_dem = _throttle_dem ;
}
2015-09-15 22:17:28 -03:00
// Sum the components.
2016-07-05 23:15:55 -03:00
_throttle_dem = _throttle_dem + _integTHR_state ;
2013-06-26 05:38:40 -03:00
}
// Constrain throttle demand
_throttle_dem = constrain_float ( _throttle_dem , _THRminf , _THRmaxf ) ;
}
2016-02-23 20:20:25 -04:00
float AP_TECS : : _get_i_gain ( void )
{
float i_gain = _integGain ;
2017-01-11 01:32:41 -04:00
if ( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND ) {
2016-02-23 20:20:25 -04:00
if ( ! is_zero ( _integGain_takeoff ) ) {
i_gain = _integGain_takeoff ;
}
2016-03-25 18:41:09 -03:00
} else if ( _flags . is_doing_auto_land ) {
2016-02-23 20:20:25 -04:00
if ( ! is_zero ( _integGain_land ) ) {
i_gain = _integGain_land ;
}
}
return i_gain ;
}
2016-07-05 23:15:55 -03:00
/*
calculate throttle , non - airspeed case
*/
void AP_TECS : : _update_throttle_without_airspeed ( int16_t throttle_nudge )
2013-07-04 18:48:28 -03:00
{
2015-09-15 22:17:28 -03:00
// Calculate throttle demand by interpolating between pitch and throttle limits
2014-02-22 13:54:37 -04:00
float nomThr ;
//If landing and we don't have an airspeed sensor and we have a non-zero
//TECS_LAND_THR param then use it
2016-04-19 21:09:17 -03:00
if ( _flags . is_doing_auto_land & & _landThrottle > = 0 ) {
2014-03-10 14:41:05 -03:00
nomThr = ( _landThrottle + throttle_nudge ) * 0.01f ;
2014-02-22 13:54:37 -04:00
} else { //not landing or not using TECS_LAND_THR parameter
2015-09-15 22:17:28 -03:00
nomThr = ( aparm . throttle_cruise + throttle_nudge ) * 0.01f ;
}
if ( _pitch_dem > 0.0f & & _PITCHmaxf > 0.0f )
{
_throttle_dem = nomThr + ( _THRmaxf - nomThr ) * _pitch_dem / _PITCHmaxf ;
}
else if ( _pitch_dem < 0.0f & & _PITCHminf < 0.0f )
{
_throttle_dem = nomThr + ( _THRminf - nomThr ) * _pitch_dem / _PITCHminf ;
}
else
{
_throttle_dem = nomThr ;
}
2013-07-31 07:16:37 -03:00
2018-10-18 09:51:21 -03:00
if ( _flags . is_gliding )
{
_throttle_dem = 0.0f ;
2021-05-17 13:23:26 -03:00
return ;
2018-10-18 09:51:21 -03:00
}
2013-07-31 07:16:37 -03:00
// Calculate additional throttle for turn drag compensation including throttle nudging
2015-12-10 18:07:30 -04:00
const Matrix3f & rotMat = _ahrs . get_rotation_body_to_ned ( ) ;
2015-09-15 22:17:28 -03:00
// Use the demanded rate of change of total energy as the feed-forward demand, but add
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf ( ( rotMat . a . y * rotMat . a . y ) + ( rotMat . b . y * rotMat . b . y ) ) ;
float STEdot_dem = _rollComp * ( 1.0f / constrain_float ( cosPhi * cosPhi , 0.1f , 1.0f ) - 1.0f ) ;
_throttle_dem = _throttle_dem + STEdot_dem / ( _STEdot_max - _STEdot_min ) * ( _THRmaxf - _THRminf ) ;
2013-07-04 18:48:28 -03:00
}
2015-09-15 22:17:28 -03:00
void AP_TECS : : _detect_bad_descent ( void )
2013-06-26 05:38:40 -03:00
{
2015-09-15 22:17:28 -03:00
// Detect a demanded airspeed too high for the aircraft to achieve. This will be
// evident by the the following conditions:
// 1) Underspeed protection not active
// 2) Specific total energy error > 200 (greater than ~20m height error)
// 3) Specific total energy reducing
// 4) throttle demand > 90%
// If these four conditions exist simultaneously, then the protection
// mode will be activated.
// Once active, the following condition are required to stay in the mode
// 1) Underspeed protection not active
// 2) Specific total energy error > 0
// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
float STEdot = _SPEdot + _SKEdot ;
2018-10-18 09:51:21 -03:00
if ( ( ( ! _flags . underspeed & & ( _STE_error > 200.0f ) & & ( STEdot < 0.0f ) & & ( _throttle_dem > = _THRmaxf * 0.9f ) ) | | ( _flags . badDescent & & ! _flags . underspeed & & ( _STE_error > 0.0f ) ) ) & & ! _flags . is_gliding )
2015-09-15 22:17:28 -03:00
{
2016-03-25 18:41:09 -03:00
_flags . badDescent = true ;
2015-09-15 22:17:28 -03:00
}
else
{
2016-03-25 18:41:09 -03:00
_flags . badDescent = false ;
2015-09-15 22:17:28 -03:00
}
2013-06-26 05:38:40 -03:00
}
2015-09-15 22:17:28 -03:00
void AP_TECS : : _update_pitch ( void )
2013-06-26 05:38:40 -03:00
{
2015-09-15 22:17:28 -03:00
// Calculate Speed/Height Control Weighting
2013-06-26 05:38:40 -03:00
// This is used to determine how the pitch control prioritises speed and height control
// A weighting of 1 provides equal priority (this is the normal mode of operation)
// A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available
2015-09-15 22:17:28 -03:00
// A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed
// rises above the demanded value, the pitch angle will be increased by the TECS controller.
2016-05-13 03:22:33 -03:00
float SKE_weighting = constrain_float ( _spdWeight , 0.0f , 2.0f ) ;
2019-10-12 11:28:32 -03:00
if ( ! ( _ahrs . airspeed_sensor_enabled ( ) | | _use_synthetic_airspeed ) ) {
2016-05-13 03:22:33 -03:00
SKE_weighting = 0.0f ;
2017-10-21 03:47:35 -03:00
} else if ( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_VTOL ) {
// if we are in VTOL mode then control pitch without regard to
// speed. Speed is also taken care of independently of
// height. This is needed as the usual relationship of speed
// and height is broken by the VTOL motors
SKE_weighting = 0.0f ;
2018-10-18 09:51:21 -03:00
} else if ( _flags . underspeed | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND | | _flags . is_gliding ) {
2016-05-13 03:22:33 -03:00
SKE_weighting = 2.0f ;
2016-04-19 21:09:17 -03:00
} else if ( _flags . is_doing_auto_land ) {
2016-01-08 20:13:08 -04:00
if ( _spdWeightLand < 0 ) {
// use sliding scale from normal weight down to zero at landing
2016-04-21 21:13:15 -03:00
float scaled_weight = _spdWeight * ( 1.0f - constrain_float ( _path_proportion , 0 , 1 ) ) ;
2016-05-13 03:22:33 -03:00
SKE_weighting = constrain_float ( scaled_weight , 0.0f , 2.0f ) ;
2016-01-08 20:13:08 -04:00
} else {
2016-05-13 03:22:33 -03:00
SKE_weighting = constrain_float ( _spdWeightLand , 0.0f , 2.0f ) ;
2016-01-08 20:13:08 -04:00
}
2014-04-09 13:50:52 -03:00
}
2015-09-15 22:17:28 -03:00
2016-05-13 03:22:33 -03:00
logging . SKE_weighting = SKE_weighting ;
float SPE_weighting = 2.0f - SKE_weighting ;
2013-06-26 05:38:40 -03:00
// Calculate Specific Energy Balance demand, and error
2016-05-13 03:22:33 -03:00
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting ;
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting ;
float SEB_error = SEB_dem - ( _SPE_est * SPE_weighting - _SKE_est * SKE_weighting ) ;
float SEBdot_error = SEBdot_dem - ( _SPEdot * SPE_weighting - _SKEdot * SKE_weighting ) ;
logging . SKE_error = _SKE_dem - _SKE_est ;
logging . SPE_error = _SPE_dem - _SPE_est ;
2013-06-26 05:38:40 -03:00
// Calculate integrator state, constraining input if pitch limits are exceeded
2016-05-13 02:24:25 -03:00
float integSEB_input = SEB_error * _get_i_gain ( ) ;
2015-09-15 22:17:28 -03:00
if ( _pitch_dem > _PITCHmaxf )
2013-06-26 05:38:40 -03:00
{
2016-05-13 02:24:25 -03:00
integSEB_input = MIN ( integSEB_input , _PITCHmaxf - _pitch_dem ) ;
2013-06-26 05:38:40 -03:00
}
2015-03-14 23:52:17 -03:00
else if ( _pitch_dem < _PITCHminf )
2013-06-26 05:38:40 -03:00
{
2016-05-13 02:24:25 -03:00
integSEB_input = MAX ( integSEB_input , _PITCHminf - _pitch_dem ) ;
2013-06-26 05:38:40 -03:00
}
2016-05-13 02:46:27 -03:00
float integSEB_delta = integSEB_input * _DT ;
2013-06-26 05:38:40 -03:00
2015-03-14 23:52:17 -03:00
#if 0
2017-01-10 01:46:51 -04:00
if ( _landing . is_flaring ( ) & & fabsf ( _climb_rate ) > 0.2f ) {
2015-03-14 23:52:17 -03:00
: : printf ( " _hgt_rate_dem=%.1f _hgt_dem_adj=%.1f climb=%.1f _flare_counter=%u _pitch_dem=%.1f SEB_dem=%.2f SEBdot_dem=%.2f SEB_error=%.2f SEBdot_error=%.2f \n " ,
_hgt_rate_dem , _hgt_dem_adj , _climb_rate , _flare_counter , degrees ( _pitch_dem ) ,
SEB_dem , SEBdot_dem , SEB_error , SEBdot_error ) ;
}
# endif
2015-09-15 22:17:28 -03:00
// Apply max and min values for integrator state that will allow for no more than
// 5deg of saturation. This allows for some pitch variation due to gusts before the
// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
2013-10-16 03:19:26 -03:00
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
2014-11-28 04:32:15 -04:00
// During flare a different damping gain is used
2016-05-13 02:24:25 -03:00
float gainInv = ( _TAS_state * timeConstant ( ) * GRAVITY_MSS ) ;
2018-10-18 10:53:37 -03:00
float temp = SEB_error + 0.5 * SEBdot_dem * timeConstant ( ) ;
2016-02-24 22:18:14 -04:00
float pitch_damp = _ptchDamp ;
2017-01-10 01:46:51 -04:00
if ( _landing . is_flaring ( ) ) {
2016-02-24 22:18:14 -04:00
pitch_damp = _landDamp ;
2016-04-19 21:09:17 -03:00
} else if ( ! is_zero ( _land_pitch_damp ) & & _flags . is_doing_auto_land ) {
2016-02-24 22:18:14 -04:00
pitch_damp = _land_pitch_damp ;
2014-11-28 04:32:15 -04:00
}
2016-02-24 22:18:14 -04:00
temp + = SEBdot_error * pitch_damp ;
2016-12-13 23:18:48 -04:00
if ( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND ) {
2014-11-28 04:32:15 -04:00
temp + = _PITCHminf * gainInv ;
2013-10-16 03:19:26 -03:00
}
2016-05-13 02:46:27 -03:00
float integSEB_min = ( gainInv * ( _PITCHminf - 0.0783f ) ) - temp ;
float integSEB_max = ( gainInv * ( _PITCHmaxf + 0.0783f ) ) - temp ;
float integSEB_range = integSEB_max - integSEB_min ;
2016-05-13 03:22:33 -03:00
logging . SEB_delta = integSEB_delta ;
2016-05-13 02:46:27 -03:00
// don't allow the integrator to rise by more than 20% of its full
// range in one step. This prevents single value glitches from
// causing massive integrator changes. See Issue#4066
integSEB_delta = constrain_float ( integSEB_delta , - integSEB_range * 0.1f , integSEB_range * 0.1f ) ;
2019-03-29 07:49:13 -03:00
// prevent the constraint on pitch integrator _integSEB_state from
// itself injecting step changes in the variable. We only want the
// constraint to prevent large changes due to integSEB_delta, not
// to cause step changes due to a change in the constrain
// limits. Large steps in _integSEB_state can cause long term
// pitch changes
integSEB_min = MIN ( integSEB_min , _integSEB_state ) ;
integSEB_max = MAX ( integSEB_max , _integSEB_state ) ;
2016-05-13 02:46:27 -03:00
// integrate
_integSEB_state = constrain_float ( _integSEB_state + integSEB_delta , integSEB_min , integSEB_max ) ;
2013-06-26 05:38:40 -03:00
// Calculate pitch demand from specific energy balance signals
2016-05-13 02:24:25 -03:00
_pitch_dem_unc = ( temp + _integSEB_state ) / gainInv ;
2013-06-26 05:38:40 -03:00
2018-10-18 07:42:57 -03:00
// Add a feedforward term from demanded airspeed to pitch.
2018-10-18 09:51:21 -03:00
if ( _flags . is_gliding ) {
_pitch_dem_unc + = ( _TAS_dem_adj - _pitch_ff_v0 ) * _pitch_ff_k ;
}
2018-10-18 07:42:57 -03:00
2013-06-26 05:38:40 -03:00
// Constrain pitch demand
_pitch_dem = constrain_float ( _pitch_dem_unc , _PITCHminf , _PITCHmaxf ) ;
// Rate limit the pitch demand to comply with specified vertical
// acceleration limit
2016-05-13 02:24:25 -03:00
float ptchRateIncr = _DT * _vertAccLim / _TAS_state ;
2013-06-26 05:38:40 -03:00
if ( ( _pitch_dem - _last_pitch_dem ) > ptchRateIncr )
{
2015-09-15 22:17:28 -03:00
_pitch_dem = _last_pitch_dem + ptchRateIncr ;
}
else if ( ( _pitch_dem - _last_pitch_dem ) < - ptchRateIncr )
{
_pitch_dem = _last_pitch_dem - ptchRateIncr ;
}
2015-03-14 23:52:17 -03:00
// re-constrain pitch demand
_pitch_dem = constrain_float ( _pitch_dem , _PITCHminf , _PITCHmaxf ) ;
2015-09-15 22:17:28 -03:00
_last_pitch_dem = _pitch_dem ;
2013-06-26 05:38:40 -03:00
}
2015-09-15 22:17:28 -03:00
void AP_TECS : : _initialise_states ( int32_t ptchMinCO_cd , float hgt_afe )
2013-06-26 05:38:40 -03:00
{
2015-09-15 22:17:28 -03:00
// Initialise states and variables if DT > 1 second or in climbout
2020-02-06 00:43:16 -04:00
if ( _DT > 1.0f | | _need_reset )
2015-09-15 22:17:28 -03:00
{
2016-05-13 02:24:25 -03:00
_integTHR_state = 0.0f ;
_integSEB_state = 0.0f ;
2015-09-15 22:17:28 -03:00
_last_throttle_dem = aparm . throttle_cruise * 0.01f ;
_last_pitch_dem = _ahrs . pitch ;
_hgt_dem_adj_last = hgt_afe ;
_hgt_dem_adj = _hgt_dem_adj_last ;
_hgt_dem_prev = _hgt_dem_adj_last ;
2013-06-26 05:38:40 -03:00
_hgt_dem_in_old = _hgt_dem_adj_last ;
_TAS_dem_adj = _TAS_dem ;
2016-03-25 18:41:09 -03:00
_flags . underspeed = false ;
_flags . badDescent = false ;
2016-07-05 23:15:55 -03:00
_flags . reached_speed_takeoff = false ;
2015-09-15 22:17:28 -03:00
_DT = 0.1f ; // when first starting TECS, use a
// small time constant
2020-02-06 00:43:16 -04:00
_need_reset = false ;
2015-09-15 22:17:28 -03:00
}
2016-12-13 23:18:48 -04:00
else if ( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND )
2015-09-15 22:17:28 -03:00
{
_PITCHminf = 0.000174533f * ptchMinCO_cd ;
_hgt_dem_adj_last = hgt_afe ;
_hgt_dem_adj = _hgt_dem_adj_last ;
_hgt_dem_prev = _hgt_dem_adj_last ;
2013-06-26 05:38:40 -03:00
_TAS_dem_adj = _TAS_dem ;
2016-03-25 18:41:09 -03:00
_flags . underspeed = false ;
_flags . badDescent = false ;
2015-09-15 22:17:28 -03:00
}
2016-07-05 23:15:55 -03:00
2016-12-13 23:18:48 -04:00
if ( _flight_stage ! = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF & & _flight_stage ! = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND ) {
2016-07-05 23:15:55 -03:00
// reset takeoff speed flag when not in takeoff
_flags . reached_speed_takeoff = false ;
}
2013-06-26 05:38:40 -03:00
}
2015-09-15 22:17:28 -03:00
void AP_TECS : : _update_STE_rate_lim ( void )
2013-06-26 05:38:40 -03:00
{
// Calculate Specific Total Energy Rate Limits
2015-09-15 22:17:28 -03:00
// This is a trivial calculation at the moment but will get bigger once we start adding altitude effects
2013-06-26 05:38:40 -03:00
_STEdot_max = _maxClimbRate * GRAVITY_MSS ;
_STEdot_min = - _minSinkRate * GRAVITY_MSS ;
}
2016-01-30 03:23:05 -04:00
2013-07-09 07:50:37 -03:00
void AP_TECS : : update_pitch_throttle ( int32_t hgt_dem_cm ,
2015-09-15 22:17:28 -03:00
int32_t EAS_dem_cm ,
2016-12-13 22:19:56 -04:00
enum AP_Vehicle : : FixedWing : : FlightStage flight_stage ,
2016-02-12 15:38:40 -04:00
float distance_beyond_land_wp ,
2015-09-15 22:17:28 -03:00
int32_t ptchMinCO_cd ,
int16_t throttle_nudge ,
float hgt_afe ,
2018-10-18 09:51:21 -03:00
float load_factor )
2013-06-26 05:38:40 -03:00
{
// Calculate time in seconds since last update
2016-05-13 01:48:55 -03:00
uint64_t now = AP_HAL : : micros64 ( ) ;
_DT = ( now - _update_pitch_throttle_last_usec ) * 1.0e-6 f ;
2015-09-15 22:17:28 -03:00
_update_pitch_throttle_last_usec = now ;
2013-06-26 05:38:40 -03:00
2018-10-18 09:51:21 -03:00
_flags . is_gliding = _flags . gliding_requested | | _flags . propulsion_failed | | aparm . throttle_max = = 0 ;
2017-01-11 01:29:03 -04:00
_flags . is_doing_auto_land = ( flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_LAND ) ;
2016-02-12 15:38:40 -04:00
_distance_beyond_land_wp = distance_beyond_land_wp ;
2016-02-23 20:20:25 -04:00
_flight_stage = flight_stage ;
2016-01-30 03:23:05 -04:00
2015-09-15 22:17:28 -03:00
// Convert inputs
2013-06-26 07:49:08 -03:00
_hgt_dem = hgt_dem_cm * 0.01f ;
2015-09-15 22:17:28 -03:00
_EAS_dem = EAS_dem_cm * 0.01f ;
2016-02-12 17:23:02 -04:00
// Update the speed estimate using a 2nd order complementary filter
_update_speed ( load_factor ) ;
2015-08-29 17:20:07 -03:00
if ( aparm . takeoff_throttle_max ! = 0 & &
2016-12-13 23:18:48 -04:00
( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND ) ) {
2015-04-15 19:52:29 -03:00
_THRmaxf = aparm . takeoff_throttle_max * 0.01f ;
} else {
_THRmaxf = aparm . throttle_max * 0.01f ;
}
2013-06-26 07:49:08 -03:00
_THRminf = aparm . throttle_min * 0.01f ;
2014-08-05 22:46:01 -03:00
2019-03-04 02:37:13 -04:00
// min of 1% throttle range to prevent a numerical error
_THRmaxf = MAX ( _THRmaxf , _THRminf + 0.01 ) ;
2015-09-15 22:17:28 -03:00
// work out the maximum and minimum pitch
// if TECS_PITCH_{MAX,MIN} isn't set then use
// LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be
// larger than LIM_PITCH_{MAX,MIN}
2019-03-02 07:05:08 -04:00
if ( _pitch_max = = 0 ) {
2015-09-15 22:17:28 -03:00
_PITCHmaxf = aparm . pitch_limit_max_cd * 0.01f ;
} else {
2015-11-27 13:11:58 -04:00
_PITCHmaxf = MIN ( _pitch_max , aparm . pitch_limit_max_cd * 0.01f ) ;
2015-09-15 22:17:28 -03:00
}
2016-01-09 06:50:40 -04:00
2015-09-15 22:17:28 -03:00
if ( _pitch_min > = 0 ) {
_PITCHminf = aparm . pitch_limit_min_cd * 0.01f ;
} else {
2015-11-27 13:11:58 -04:00
_PITCHminf = MAX ( _pitch_min , aparm . pitch_limit_min_cd * 0.01f ) ;
2015-09-15 22:17:28 -03:00
}
2016-04-02 05:53:16 -03:00
// apply temporary pitch limit and clear
if ( _pitch_max_limit < 90 ) {
_PITCHmaxf = constrain_float ( _PITCHmaxf , - 90 , _pitch_max_limit ) ;
_PITCHminf = constrain_float ( _PITCHminf , - _pitch_max_limit , _PITCHmaxf ) ;
_pitch_max_limit = 90 ;
}
2019-04-14 03:03:24 -03:00
if ( ! _landing . is_on_approach ( ) ) {
// reset land pitch min when not landing
_land_pitch_min = _PITCHminf ;
}
2017-01-10 01:46:51 -04:00
if ( _landing . is_flaring ( ) ) {
2014-08-27 07:11:48 -03:00
// in flare use min pitch from LAND_PITCH_CD
2016-11-23 06:18:05 -04:00
_PITCHminf = MAX ( _PITCHminf , _landing . get_pitch_cd ( ) * 0.01f ) ;
2014-08-29 16:14:57 -03:00
2015-03-19 00:46:32 -03:00
// and use max pitch from TECS_LAND_PMAX
2016-02-10 12:41:29 -04:00
if ( _land_pitch_max ! = 0 ) {
2019-04-10 00:45:27 -03:00
// note that this allows a flare pitch outside the normal TECS auto limits
_PITCHmaxf = _land_pitch_max ;
2015-03-19 00:46:32 -03:00
}
2015-09-15 22:17:28 -03:00
2014-08-29 16:14:57 -03:00
// and allow zero throttle
_THRminf = 0 ;
2017-01-10 01:46:51 -04:00
} else if ( _landing . is_on_approach ( ) & & ( - _climb_rate ) > _land_sink ) {
2015-03-14 23:52:17 -03:00
// constrain the pitch in landing as we get close to the flare
// point. Use a simple linear limit from 15 meters after the
// landing point
2016-11-23 06:18:05 -04:00
float time_to_flare = ( - hgt_afe / _climb_rate ) - _landing . get_flare_sec ( ) ;
2015-03-14 23:52:17 -03:00
if ( time_to_flare < 0 ) {
// we should be flaring already
2016-11-23 06:18:05 -04:00
_PITCHminf = MAX ( _PITCHminf , _landing . get_pitch_cd ( ) * 0.01f ) ;
2015-03-14 23:52:17 -03:00
} else if ( time_to_flare < timeConstant ( ) * 2 ) {
// smoothly move the min pitch to the flare min pitch over
// twice the time constant
float p = time_to_flare / ( 2 * timeConstant ( ) ) ;
2016-11-23 06:18:05 -04:00
float pitch_limit_cd = p * aparm . pitch_limit_min_cd + ( 1 - p ) * _landing . get_pitch_cd ( ) ;
2015-03-14 23:52:17 -03:00
#if 0
: : printf ( " ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f \n " ,
time_to_flare , hgt_afe , _PITCHminf , pitch_limit_cd * 0.01f , _climb_rate ) ;
# endif
2015-11-27 13:11:58 -04:00
_PITCHminf = MAX ( _PITCHminf , pitch_limit_cd * 0.01f ) ;
2015-03-14 23:52:17 -03:00
}
2014-08-27 07:11:48 -03:00
}
2015-03-19 00:46:32 -03:00
2019-04-14 03:03:24 -03:00
if ( _landing . is_on_approach ( ) ) {
// don't allow the lower bound of pitch to decrease, nor allow
// it to increase rapidly. This prevents oscillation of pitch
// demand while in landing approach based on rapidly changing
// time to flare estimate
if ( _land_pitch_min < = - 90 ) {
_land_pitch_min = _PITCHminf ;
}
const float flare_pitch_range = 20 ;
const float delta_per_loop = ( flare_pitch_range / _landTimeConst ) * _DT ;
_PITCHminf = MIN ( _PITCHminf , _land_pitch_min + delta_per_loop ) ;
_land_pitch_min = MAX ( _land_pitch_min , _PITCHminf ) ;
_PITCHminf = MAX ( _land_pitch_min , _PITCHminf ) ;
}
2019-10-26 00:35:49 -03:00
if ( _landing . is_flaring ( ) ) {
// ensure we don't violate the limits for flare pitch
if ( _land_pitch_max ! = 0 ) {
_PITCHmaxf = MIN ( _land_pitch_max , _PITCHmaxf ) ;
}
}
2016-12-13 23:18:48 -04:00
if ( flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND ) {
2016-07-05 23:15:55 -03:00
if ( ! _flags . reached_speed_takeoff & & _TAS_state > = _TAS_dem_adj ) {
// we have reached our target speed in takeoff, allow for
// normal throttle control
_flags . reached_speed_takeoff = true ;
}
}
2015-09-15 22:17:28 -03:00
// convert to radians
_PITCHmaxf = radians ( _PITCHmaxf ) ;
_PITCHminf = radians ( _PITCHminf ) ;
2013-06-26 05:38:40 -03:00
2019-04-10 00:45:27 -03:00
// don't allow max pitch to go below min pitch
_PITCHmaxf = MAX ( _PITCHmaxf , _PITCHminf ) ;
2015-09-15 22:17:28 -03:00
// initialise selected states and variables if DT > 1 second or in climbout
_initialise_states ( ptchMinCO_cd , hgt_afe ) ;
2013-06-26 05:38:40 -03:00
// Calculate Specific Total Energy Rate Limits
2015-09-15 22:17:28 -03:00
_update_STE_rate_lim ( ) ;
2013-06-26 05:38:40 -03:00
// Calculate the speed demand
_update_speed_demand ( ) ;
2015-09-15 22:17:28 -03:00
// Calculate the height demand
_update_height_demand ( ) ;
2013-06-26 05:38:40 -03:00
// Detect underspeed condition
_detect_underspeed ( ) ;
// Calculate specific energy quantitiues
_update_energies ( ) ;
2016-10-04 21:52:34 -03:00
// Calculate throttle demand - use simple pitch to throttle if no
// airspeed sensor.
// Note that caller can demand the use of
// synthetic airspeed for one loop if needed. This is required
// during QuadPlane transition when pitch is constrained
2016-10-19 23:22:39 -03:00
if ( _ahrs . airspeed_sensor_enabled ( ) | | _use_synthetic_airspeed | | _use_synthetic_airspeed_once ) {
2016-07-05 23:15:55 -03:00
_update_throttle_with_airspeed ( ) ;
2016-10-19 23:22:39 -03:00
_use_synthetic_airspeed_once = false ;
2015-09-15 22:17:28 -03:00
} else {
2016-07-05 23:15:55 -03:00
_update_throttle_without_airspeed ( throttle_nudge ) ;
2015-09-15 22:17:28 -03:00
}
2013-06-26 05:38:40 -03:00
2013-07-04 18:48:28 -03:00
// Detect bad descent due to demanded airspeed being too high
2015-09-15 22:17:28 -03:00
_detect_bad_descent ( ) ;
2013-06-26 05:38:40 -03:00
2018-10-18 09:51:21 -03:00
if ( _options & OPTION_GLIDER_ONLY ) {
2018-07-29 19:56:10 -03:00
_flags . badDescent = false ;
}
2015-09-15 22:17:28 -03:00
// Calculate pitch demand
_update_pitch ( ) ;
2013-06-26 05:38:40 -03:00
2019-01-18 00:23:42 -04:00
// log to AP_Logger
2019-05-13 07:25:35 -03:00
// @LoggerMessage: TECS
// @Vehicles: Plane
// @Description: Information about the Total Energy Control System
// @URL: http://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html
2020-04-07 04:31:21 -03:00
// @Field: TimeUS: Time since system startup
2019-05-13 07:25:35 -03:00
// @Field: h: height estimate (UP) currently in use by TECS
// @Field: dh: current climb rate ("delta-height")
// @Field: hdem: height TECS is currently trying to achieve
// @Field: dhdem: climb rate TECS is currently trying to achieve
// @Field: spdem: True AirSpeed TECS is currently trying to achieve
// @Field: sp: current estimated True AirSpeed
// @Field: dsp: x-axis acceleration estimate ("delta-speed")
// @Field: ith: throttle integrator value
// @Field: iph: Specific Energy Balance integrator value
// @Field: th: throttle output
// @Field: ph: pitch output
// @Field: dspdem: demanded acceleration output ("delta-speed demand")
2020-03-21 10:03:33 -03:00
// @Field: w: current TECS prioritization of height vs speed (0==100% height,2==100% speed, 1==50%height+50%speed
2019-05-13 07:25:35 -03:00
// @Field: f: flags
// @FieldBits: f: Underspeed,UnachievableDescent,AutoLanding,ReachedTakeoffSpd
2021-08-17 06:57:41 -03:00
AP : : logger ( ) . WriteStreaming (
2015-12-07 22:54:19 -04:00
" TECS " ,
" TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f " ,
" smnmnnnn----o-- " ,
" F0000000----0-- " ,
" QfffffffffffffB " ,
now ,
( double ) _height ,
( double ) _climb_rate ,
( double ) _hgt_dem_adj ,
( double ) _hgt_rate_dem ,
( double ) _TAS_dem_adj ,
( double ) _TAS_state ,
( double ) _vel_dot ,
( double ) _integTHR_state ,
( double ) _integSEB_state ,
( double ) _throttle_dem ,
( double ) _pitch_dem ,
( double ) _TAS_rate_dem ,
( double ) logging . SKE_weighting ,
_flags_byte ) ;
2019-05-13 07:25:35 -03:00
// @LoggerMessage: TEC2
// @Vehicles: Plane
// @Description: Additional Information about the Total Energy Control System
// @URL: http://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html
2020-04-07 04:31:21 -03:00
// @Field: TimeUS: Time since system startup
2020-03-20 23:44:44 -03:00
// @Field: pmax: maximum allowed pitch from parameter
// @Field: pmin: minimum allowed pitch from parameter
2019-05-13 07:25:35 -03:00
// @Field: KErr: difference between estimated kinetic energy and desired kinetic energy
// @Field: PErr: difference between estimated potential energy and desired potential energy
// @Field: EDelta: current error in speed/balance weighting
// @Field: LF: aerodynamic load factor
2021-08-17 06:57:41 -03:00
AP : : logger ( ) . WriteStreaming ( " TEC2 " , " TimeUS,pmax,pmin,KErr,PErr,EDelta,LF " ,
2019-04-14 03:03:24 -03:00
" s------ " ,
" F------ " ,
" Qffffff " ,
2019-04-13 17:42:47 -03:00
now ,
2019-04-14 03:03:24 -03:00
( double ) degrees ( _PITCHmaxf ) ,
( double ) degrees ( _PITCHminf ) ,
2019-04-13 17:42:47 -03:00
( double ) logging . SKE_error ,
( double ) logging . SPE_error ,
( double ) logging . SEB_delta ,
2019-04-14 03:03:24 -03:00
( double ) load_factor ) ;
2018-12-25 23:54:15 -04:00
}