diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2013-11-24 12:48:44 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-11-24 12:48:44 +0100 |
commit | 3c6f01bea8a65e2c347d1b893b3fe0d152bff69c (patch) | |
tree | 8b1f9b2e33a5732586f8e45834386eec6ff28017 /src | |
parent | 85a76a32c5be43e8f1a4d82041e1b860dc21e217 (diff) | |
download | px4-firmware-3c6f01bea8a65e2c347d1b893b3fe0d152bff69c.tar.gz px4-firmware-3c6f01bea8a65e2c347d1b893b3fe0d152bff69c.tar.bz2 px4-firmware-3c6f01bea8a65e2c347d1b893b3fe0d152bff69c.zip |
tecs: speedrate: use p loop instead of pre calculated speed rate for now
Diffstat (limited to 'src')
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 60 | ||||
-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 | 5 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 1 |
4 files changed, 46 insertions, 25 deletions
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a2ac6507e..d089be080 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -169,35 +169,45 @@ void TECS::_update_speed_demand(void) // calculate velocity rate limits based on physical performance limits // 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 - float velRateMax; - float velRateMin; - - if ((_badDescent) || (_underspeed)) { - velRateMax = 0.5f * _STEdot_max / _integ5_state; - velRateMin = 0.5f * _STEdot_min / _integ5_state; - - } else { - velRateMax = 0.5f * _STEdot_max / _integ5_state; - velRateMin = 0.5f * _STEdot_min / _integ5_state; - } - - // Apply rate limit - if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { - _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; - _TAS_rate_dem = velRateMax; - - } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { - _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; - _TAS_rate_dem = velRateMin; +// float velRateMax; +// float velRateMin; +// +// if ((_badDescent) || (_underspeed)) { +// velRateMax = 0.5f * _STEdot_max / _integ5_state; +// velRateMin = 0.5f * _STEdot_min / _integ5_state; +// +// } else { +// velRateMax = 0.5f * _STEdot_max / _integ5_state; +// velRateMin = 0.5f * _STEdot_min / _integ5_state; +// } +// +// // Apply rate limit +// if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; +// _TAS_rate_dem = velRateMax; +// +// } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { +// _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; +// _TAS_rate_dem = velRateMin; +// +// } else { +// _TAS_dem_adj = _TAS_dem; +// +// +// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; +// } - } else { - _TAS_dem_adj = _TAS_dem; - _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; - } + _TAS_dem_adj = _TAS_dem; + _TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now // Constrain speed demand again to protect against bad values on initialisation. _TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax); - _TAS_dem_last = _TAS_dem; +// _TAS_dem_last = _TAS_dem; + +// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f", +// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin); +// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u", +// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent , _underspeed); } void TECS::_update_height_demand(float demand, float state) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 06e1c8ad3..4fc009da9 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -184,6 +184,10 @@ public: _heightrate_p = heightrate_p; } + void set_speedrate_p(float speedrate_p) { + _speedrate_p = speedrate_p; + } + private: // Last time update_50Hz was called uint64_t _update_50hz_last_usec; @@ -208,6 +212,7 @@ private: float _rollComp; float _spdWeight; float _heightrate_p; + float _speedrate_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 2b1c01c83..8b78bfbc4 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 @@ -217,6 +217,7 @@ private: float loiter_hold_radius; float heightrate_p; + float speedrate_p; float land_slope_angle; float land_slope_length; @@ -260,6 +261,7 @@ private: param_t loiter_hold_radius; param_t heightrate_p; + param_t speedrate_p; param_t land_slope_angle; param_t land_slope_length; @@ -431,6 +433,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _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"); + _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); /* fetch initial parameter values */ parameters_update(); @@ -497,6 +500,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); @@ -523,6 +527,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); _tecs.set_max_climb_rate(_parameters.max_climb_rate); _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_speedrate_p(_parameters.speedrate_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 f206d808e..10a0c00fc 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 @@ -113,6 +113,7 @@ 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); +PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f); PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f); |