dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

add heightrate ff for tecs

This commit is contained in:
Thomas Gubler 2014-09-04 10:59:35 +02:00 committed by Lorenz Meier
parent 421b1b5725
commit e056fbb703
4 changed files with 78 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -238,7 +238,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)*_heightrate_p;
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
// Limit height rate of change
if (_hgt_rate_dem > _maxClimbRate) {
_hgt_rate_dem = _maxClimbRate;

6
src/lib/external_lgpl/tecs/tecs.h
View File

@ -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

5
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -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 */

7
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -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
*