diff options
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 2 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.h | 5 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 |
4 files changed, 16 insertions, 1 deletions
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 5cbc19dbb..a94a06dda 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -231,7 +231,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (_hgt_dem_adj-state)*0.1f; //xxx: parameter + _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 224f35366..d07bd00dd 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -178,6 +178,10 @@ public: _indicated_airspeed_max = airspeed; } + void set_heightrate_p(float heightrate_p) { + _heightrate_p = heightrate_p; + } + private: // Last time update_50Hz was called uint64_t _update_50hz_last_usec; @@ -201,6 +205,7 @@ private: float _vertAccLim; float _rollComp; float _spdWeight; + float _heightrate_p; // throttle demand in the range from 0.0 to 1.0 float _throttle_dem; 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 ffa7915a7..dbdab5961 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 @@ -206,6 +206,8 @@ private: float throttle_land_max; float loiter_hold_radius; + + float heightrate_p; } _parameters; /**< local copies of interesting parameters */ struct { @@ -240,6 +242,8 @@ private: param_t throttle_land_max; param_t loiter_hold_radius; + + param_t heightrate_p; } _parameter_handles; /**< handles for interesting parameters */ @@ -370,6 +374,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); /* fetch initial parameter values */ parameters_update(); @@ -435,6 +440,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -453,6 +460,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); _tecs.set_indicated_airspeed_max(_parameters.airspeed_min); _tecs.set_max_climb_rate(_parameters.max_climb_rate); + _tecs.set_heightrate_p(_parameters.heightrate_p); /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || 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 3bb872405..a6aa45258 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 @@ -111,3 +111,5 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); + +PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); |