From fc2e0fad4731ef543be4c3da73de6b670d40d804 Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 31 Jul 2014 15:23:30 +0200 Subject: [PATCH 1/6] TECS: Added separate gain / time constant for the throttle loop to allow independent tuning --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- src/lib/external_lgpl/tecs/tecs.h | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6386e37a03..16c7e5ffad 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -298,7 +298,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } else { // Calculate gain scaler from specific energy error to throttle - float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min)); + float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min)); // Calculate feed-forward throttle float ff_throttle = 0; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 5cafb1c791..10c9e9344f 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -123,6 +123,10 @@ public: _timeConst = time_const; } + void set_time_const_throt(float time_const_throt) { + _timeConstThrot = time_const_throt; + } + void set_min_sink_rate(float rate) { _minSinkRate = rate; } @@ -204,6 +208,7 @@ private: float _minSinkRate; float _maxSinkRate; float _timeConst; + float _timeConstThrot; float _ptchDamp; float _thrDamp; float _integGain; From 8e33278c4cfc979e6ca69994186b3a6144847d6a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 13:04:28 +0200 Subject: [PATCH 2/6] Move the last throttle demand setting to a better position --- src/lib/external_lgpl/tecs/tecs.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 16c7e5ffad..579fb5d42c 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -327,9 +327,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM _throttle_dem = constrain(_throttle_dem, _last_throttle_dem - thrRateIncr, _last_throttle_dem + thrRateIncr); - _last_throttle_dem = _throttle_dem; } + // Ensure _last_throttle_dem is always initialized properly + // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!! + _last_throttle_dem = _throttle_dem; + // Calculate integrator state upper and lower limits // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand From 3b1c92e42f57277a12d40963c22fbea9820ee0ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 13:10:07 +0200 Subject: [PATCH 3/6] Make throttle time constant tunable separately, group TECS params correctly, make climbout alt configurable --- .../fw_pos_control_l1_main.cpp | 11 ++++- .../fw_pos_control_l1_params.c | 49 ++++++++++++++----- 2 files changed, 46 insertions(+), 14 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index eadb63f40d..f05f20cd00 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -203,9 +203,11 @@ private: float l1_damping; float time_const; + float time_const_throt; float min_sink_rate; float max_sink_rate; float max_climb_rate; + float climbout_diff; float heightrate_p; float speedrate_p; float throttle_damp; @@ -245,9 +247,11 @@ private: param_t l1_damping; param_t time_const; + param_t time_const_throt; param_t min_sink_rate; param_t max_sink_rate; param_t max_climb_rate; + param_t climbout_diff; param_t heightrate_p; param_t speedrate_p; param_t throttle_damp; @@ -471,9 +475,11 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.climbout_diff = param_find("FW_CLMBOUT_DIFF"); _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); @@ -536,6 +542,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt)); param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); @@ -547,6 +554,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); @@ -564,6 +572,7 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); _tecs.set_time_const(_parameters.time_const); + _tecs.set_time_const_throt(_parameters.time_const_throt); _tecs.set_min_sink_rate(_parameters.min_sink_rate); _tecs.set_max_sink_rate(_parameters.max_sink_rate); _tecs.set_throttle_damp(_parameters.throttle_damp); @@ -1095,7 +1104,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi usePreTakeoffThrust = false; /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 15.0f) { + if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 52128e1b7a..a0d9309b92 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -154,6 +154,18 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); */ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); +/** + * Climbout Altitude difference + * + * If the altitude error exceeds this parameter, the system will climb out + * with maximum throttle and minimum airspeed until it is closer than this + * distance to the desired altitude. Mostly used for takeoff waypoints / modes. + * Set to zero to disable climbout mode (not recommended). + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); + /** * Maximum climb rate * @@ -181,7 +193,7 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); @@ -194,7 +206,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); * exceeding the lower pitch angle limit and without over-speeding * the aircraft. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); @@ -205,17 +217,28 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); * Smaller values make it faster to respond, larger values make it slower * to respond. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); +/** + * TECS Throttle time constant + * + * This is the time constant of the TECS throttle control algorithm (in seconds). + * Smaller values make it faster to respond, larger values make it slower + * to respond. + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); + /** * Throttle damping factor * * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); @@ -227,7 +250,7 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); * and height offsets are trimmed out, but reduces damping and * increases overshoot. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); @@ -240,7 +263,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); @@ -253,7 +276,7 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); @@ -266,7 +289,7 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); @@ -282,7 +305,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); @@ -300,7 +323,7 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); @@ -312,21 +335,21 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); * will work well provided the pitch to servo controller has been tuned * properly. * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); /** * Height rate P factor * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); /** * Speed rate P factor * - * @group L1 Control + * @group Fixed Wing TECS */ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); From 2e4dff3fea2177538f3549cedd8f84fbbbecd4ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 13:21:30 +0200 Subject: [PATCH 4/6] removed debug statement from mTECS --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index afc411a60a..749f57a2ba 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -76,7 +76,6 @@ mTecs::mTecs() : _counter(0), _debug(false) { - warnx("starting"); } mTecs::~mTecs() From 32e32d92bdad3eb4e92e9e53c112fcdcf848c7e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 14:39:24 +0200 Subject: [PATCH 5/6] Add throttle slew rate param --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 +++++ .../fw_pos_control_l1/fw_pos_control_l1_params.c | 11 +++++++++++ 2 files changed, 16 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f05f20cd00..189a19d48f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -229,6 +229,7 @@ private: float throttle_min; float throttle_max; float throttle_cruise; + float throttle_slew_max; float throttle_land_max; @@ -273,6 +274,7 @@ private: param_t throttle_min; param_t throttle_max; param_t throttle_cruise; + param_t throttle_slew_max; param_t throttle_land_max; @@ -464,6 +466,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_limit = param_find("FW_R_LIM"); _parameter_handles.throttle_min = param_find("FW_THR_MIN"); _parameter_handles.throttle_max = param_find("FW_THR_MAX"); + _parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); @@ -538,6 +541,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min)); param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max)); param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); @@ -576,6 +580,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_min_sink_rate(_parameters.min_sink_rate); _tecs.set_max_sink_rate(_parameters.max_sink_rate); _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_throttle_slewrate(_parameters.throttle_slew_max); _tecs.set_integrator_gain(_parameters.integrator_gain); _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index a0d9309b92..41c3744076 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -82,6 +82,17 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); */ PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); +/** + * Throttle max slew rate + * + * Maximum slew rate for the commanded throttle + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); + /** * Negative pitch limit * From 491c6c29acef343819f32655e6e87334531af8cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Aug 2014 14:43:56 +0200 Subject: [PATCH 6/6] Init values --- src/lib/external_lgpl/tecs/tecs.h | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 10c9e9344f..752b0ddd9c 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,6 +28,26 @@ class __EXPORT TECS { public: TECS() : + _update_50hz_last_usec(0), + _update_speed_last_usec(0), + _update_pitch_throttle_last_usec(0), + // TECS tuning parameters + _hgtCompFiltOmega(0.0f), + _spdCompFiltOmega(0.0f), + _maxClimbRate(2.0f), + _minSinkRate(1.0f), + _maxSinkRate(2.0f), + _timeConst(5.0f), + _timeConstThrot(8.0f), + _ptchDamp(0.0f), + _thrDamp(0.0f), + _integGain(0.0f), + _vertAccLim(0.0f), + _rollComp(0.0f), + _spdWeight(0.5f), + _heightrate_p(0.0f), + _speedrate_p(0.0f), + _throttle_dem(0.0f), _pitch_dem(0.0f), _integ1_state(0.0f), _integ2_state(0.0f),