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_rate_dem);
|
||||||
|
|
||||||
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
|
_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_dem_adj_last = _hgt_dem_adj;
|
||||||
|
|
||||||
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
|
|
||||||
// Limit height rate of change
|
// Limit height rate of change
|
||||||
if (_hgt_rate_dem > _maxClimbRate) {
|
if (_hgt_rate_dem > _maxClimbRate) {
|
||||||
_hgt_rate_dem = _maxClimbRate;
|
_hgt_rate_dem = _maxClimbRate;
|
||||||
|
|
|
@ -47,6 +47,7 @@ public:
|
||||||
_rollComp(0.0f),
|
_rollComp(0.0f),
|
||||||
_spdWeight(0.5f),
|
_spdWeight(0.5f),
|
||||||
_heightrate_p(0.0f),
|
_heightrate_p(0.0f),
|
||||||
|
_heightrate_ff(0.0f),
|
||||||
_speedrate_p(0.0f),
|
_speedrate_p(0.0f),
|
||||||
_throttle_dem(0.0f),
|
_throttle_dem(0.0f),
|
||||||
_pitch_dem(0.0f),
|
_pitch_dem(0.0f),
|
||||||
|
@ -220,6 +221,10 @@ public:
|
||||||
_heightrate_p = heightrate_p;
|
_heightrate_p = heightrate_p;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void set_heightrate_ff(float heightrate_ff) {
|
||||||
|
_heightrate_ff = heightrate_ff;
|
||||||
|
}
|
||||||
|
|
||||||
void set_speedrate_p(float speedrate_p) {
|
void set_speedrate_p(float speedrate_p) {
|
||||||
_speedrate_p = speedrate_p;
|
_speedrate_p = speedrate_p;
|
||||||
}
|
}
|
||||||
|
@ -256,6 +261,7 @@ private:
|
||||||
float _rollComp;
|
float _rollComp;
|
||||||
float _spdWeight;
|
float _spdWeight;
|
||||||
float _heightrate_p;
|
float _heightrate_p;
|
||||||
|
float _heightrate_ff;
|
||||||
float _speedrate_p;
|
float _speedrate_p;
|
||||||
|
|
||||||
// throttle demand in the range from 0.0 to 1.0
|
// throttle demand in the range from 0.0 to 1.0
|
||||||
|
|
|
@ -211,6 +211,7 @@ private:
|
||||||
float max_climb_rate;
|
float max_climb_rate;
|
||||||
float climbout_diff;
|
float climbout_diff;
|
||||||
float heightrate_p;
|
float heightrate_p;
|
||||||
|
float heightrate_ff;
|
||||||
float speedrate_p;
|
float speedrate_p;
|
||||||
float throttle_damp;
|
float throttle_damp;
|
||||||
float integrator_gain;
|
float integrator_gain;
|
||||||
|
@ -256,6 +257,7 @@ private:
|
||||||
param_t max_climb_rate;
|
param_t max_climb_rate;
|
||||||
param_t climbout_diff;
|
param_t climbout_diff;
|
||||||
param_t heightrate_p;
|
param_t heightrate_p;
|
||||||
|
param_t heightrate_ff;
|
||||||
param_t speedrate_p;
|
param_t speedrate_p;
|
||||||
param_t throttle_damp;
|
param_t throttle_damp;
|
||||||
param_t integrator_gain;
|
param_t integrator_gain;
|
||||||
|
@ -494,6 +496,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||||
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
|
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
|
||||||
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
|
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
|
||||||
_parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
|
_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");
|
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
|
||||||
|
|
||||||
/* fetch initial parameter values */
|
/* fetch initial parameter values */
|
||||||
|
@ -563,6 +566,7 @@ FixedwingPositionControl::parameters_update()
|
||||||
param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
|
param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
|
||||||
|
|
||||||
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
|
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.speedrate_p, &(_parameters.speedrate_p));
|
||||||
|
|
||||||
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
|
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_indicated_airspeed_max(_parameters.airspeed_max);
|
||||||
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
|
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
|
||||||
_tecs.set_heightrate_p(_parameters.heightrate_p);
|
_tecs.set_heightrate_p(_parameters.heightrate_p);
|
||||||
|
_tecs.set_heightrate_ff(_parameters.heightrate_ff);
|
||||||
_tecs.set_speedrate_p(_parameters.speedrate_p);
|
_tecs.set_speedrate_p(_parameters.speedrate_p);
|
||||||
|
|
||||||
/* sanity check parameters */
|
/* 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);
|
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
|
* Speed rate P factor
|
||||||
*
|
*
|
||||||
|
|
Loading…
Reference in New Issue