forked from Archive/PX4-Autopilot
tecs: speedrate: use p loop instead of pre calculated speed rate for now
This commit is contained in:
parent
85a76a32c5
commit
3c6f01bea8
|
@ -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)
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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 ||
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue