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

tecs: speedrate: use p loop instead of pre calculated speed rate for now

This commit is contained in:
Thomas Gubler 2013-11-24 12:48:44 +01:00
parent 85a76a32c5
commit 3c6f01bea8
4 changed files with 46 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;
// 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;
// }
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;
}
_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)

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

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

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

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

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

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