forked from Archive/PX4-Autopilot
Merge branch 'stable' of github.com:swissfang/Firmware into swissfang
This commit is contained in:
commit
510750166a
|
@ -236,9 +236,8 @@ void TECS::_update_height_demand(float demand, float state)
|
|||
// // _hgt_rate_dem);
|
||||
|
||||
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
|
||||
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
|
||||
_hgt_dem_adj_last = _hgt_dem_adj;
|
||||
|
||||
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
|
||||
// Limit height rate of change
|
||||
if (_hgt_rate_dem > _maxClimbRate) {
|
||||
_hgt_rate_dem = _maxClimbRate;
|
||||
|
|
|
@ -47,6 +47,7 @@ public:
|
|||
_rollComp(0.0f),
|
||||
_spdWeight(0.5f),
|
||||
_heightrate_p(0.0f),
|
||||
_heightrate_ff(0.0f),
|
||||
_speedrate_p(0.0f),
|
||||
_throttle_dem(0.0f),
|
||||
_pitch_dem(0.0f),
|
||||
|
@ -220,6 +221,10 @@ public:
|
|||
_heightrate_p = heightrate_p;
|
||||
}
|
||||
|
||||
void set_heightrate_ff(float heightrate_ff) {
|
||||
_heightrate_ff = heightrate_ff;
|
||||
}
|
||||
|
||||
void set_speedrate_p(float speedrate_p) {
|
||||
_speedrate_p = speedrate_p;
|
||||
}
|
||||
|
@ -256,6 +261,7 @@ private:
|
|||
float _rollComp;
|
||||
float _spdWeight;
|
||||
float _heightrate_p;
|
||||
float _heightrate_ff;
|
||||
float _speedrate_p;
|
||||
|
||||
// throttle demand in the range from 0.0 to 1.0
|
||||
|
|
|
@ -211,6 +211,7 @@ private:
|
|||
float max_climb_rate;
|
||||
float climbout_diff;
|
||||
float heightrate_p;
|
||||
float heightrate_ff;
|
||||
float speedrate_p;
|
||||
float throttle_damp;
|
||||
float integrator_gain;
|
||||
|
@ -256,6 +257,7 @@ private:
|
|||
param_t max_climb_rate;
|
||||
param_t climbout_diff;
|
||||
param_t heightrate_p;
|
||||
param_t heightrate_ff;
|
||||
param_t speedrate_p;
|
||||
param_t throttle_damp;
|
||||
param_t integrator_gain;
|
||||
|
@ -494,6 +496,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.heightrate_ff = param_find("FW_T_HRATE_FF");
|
||||
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
|
@ -563,6 +566,7 @@ FixedwingPositionControl::parameters_update()
|
|||
param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
|
||||
|
||||
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
|
||||
param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff));
|
||||
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
|
||||
|
||||
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
|
||||
|
@ -600,6 +604,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_heightrate_ff(_parameters.heightrate_ff);
|
||||
_tecs.set_speedrate_p(_parameters.speedrate_p);
|
||||
|
||||
/* sanity check parameters */
|
||||
|
|
|
@ -357,6 +357,13 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
|
||||
|
||||
/**
|
||||
* Height rate FF factor
|
||||
*
|
||||
* @group Fixed Wing TECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Speed rate P factor
|
||||
*
|
||||
|
|
Loading…
Reference in New Issue