Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: Unify coding style for if statements

Browse Source
This commit is contained in:
murata 2022-07-01 04:36:16 +09:00 committed by Tom Pittenger
parent 6bb567465a
commit 531e92db8f
1 changed files with 54 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
libraries/AP_TECS/AP_TECS.cpp
View File

@ -406,7 +406,7 @@ void AP_TECS::_update_speed(float load_factor)
float integDTAS_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega;
// Prevent state from winding up
if (_TAS_state < 3.1f) {
integDTAS_input = MAX(integDTAS_input , 0.0f);
integDTAS_input = MAX(integDTAS_input, 0.0f);
}
_integDTAS_state = _integDTAS_state + integDTAS_input * DT;
float TAS_input = _integDTAS_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
@ -423,8 +423,7 @@ void AP_TECS::_update_speed_demand(void)
// This will minimise the rate of descent resulting from an engine failure,
// enable the maximum climb rate to be achieved and prevent continued full power descent
// into the ground due to an unachievable airspeed value
if ((_flags.badDescent) || (_flags.underspeed))
{
if ((_flags.badDescent) || (_flags.underspeed)) {
_TAS_dem = _TASmin;
}
@ -442,18 +441,13 @@ void AP_TECS::_update_speed_demand(void)
const float dt = 0.1;
// Apply rate limit
if ((_TAS_dem - TAS_dem_previous) > (velRateMax * dt))
{
if ((_TAS_dem - TAS_dem_previous) > (velRateMax * dt)) {
_TAS_dem_adj = TAS_dem_previous + velRateMax * dt;
_TAS_rate_dem = velRateMax;
}
else if ((_TAS_dem - TAS_dem_previous) < (velRateMin * dt))
{
} else if ((_TAS_dem - TAS_dem_previous) < (velRateMin * dt)) {
_TAS_dem_adj = TAS_dem_previous + velRateMin * dt;
_TAS_rate_dem = velRateMin;
}
else
{
} else {
_TAS_rate_dem = (_TAS_dem - TAS_dem_previous) / dt;
_TAS_dem_adj = _TAS_dem;
}
@ -478,12 +472,9 @@ void AP_TECS::_update_height_demand(void)
}
// Limit height rate of change
if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f))
{
if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
}
else if ((_hgt_dem - _hgt_dem_prev) < (-max_sink_rate * 0.1f))
{
} else if ((_hgt_dem - _hgt_dem_prev) < (-max_sink_rate * 0.1f)) {
_hgt_dem = _hgt_dem_prev - max_sink_rate * 0.1f;
}
@ -565,16 +556,13 @@ void AP_TECS::_detect_underspeed(void)
} else if (((_TAS_state < _TASmin * 0.9f) &&
(_throttle_dem >= _THRmaxf * 0.95f) &&
!_landing.is_flaring()) ||
((_height < _hgt_dem_adj) && _flags.underspeed))
{
((_height < _hgt_dem_adj) && _flags.underspeed)) {
_flags.underspeed = true;
if (_TAS_state < _TASmin * 0.9f) {
// reset start time as we are still underspeed
_underspeed_start_ms = AP_HAL::millis();
}
}
else
{
} else {
// this clears underspeed if we reach our demanded height and
// we are either below 95% throttle or we above 90% of min
// airspeed
@ -649,16 +637,11 @@ void AP_TECS::_update_throttle_with_airspeed(void)
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
if (_flags.underspeed)
{
if (_flags.underspeed) {
_throttle_dem = 1.0f;
}
else if (_flags.is_gliding)
{
} else if (_flags.is_gliding) {
_throttle_dem = 0.0f;
}
else
{
} else {
// Calculate gain scaler from specific energy error to throttle
// (_STEdot_max - _STEdot_min) / (_THRmaxf - _THRminf) is the derivative of STEdot wrt throttle measured across the max allowed throttle range.
float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min) / (_THRmaxf - _THRminf));
@ -671,7 +654,7 @@ void AP_TECS::_update_throttle_with_airspeed(void)
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f);
ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
// Calculate PD + FF throttle
@ -704,16 +687,13 @@ void AP_TECS::_update_throttle_with_airspeed(void)
// Calculate integrator state, constraining state
// Set integrator to a max throttle value during climbout
_integTHR_state = _integTHR_state + (_STE_error * _get_i_gain()) * _DT * K_STE2Thr;
if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)
{
if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
if (!_flags.reached_speed_takeoff) {
// ensure we run at full throttle until we reach the target airspeed
_throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state);
}
_integTHR_state = integ_max;
}
else
{
} else {
_integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max);
}
@ -780,21 +760,15 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge)
nomThr = (aparm.throttle_cruise + throttle_nudge)* 0.01f;
}
if (_pitch_dem > 0.0f && _PITCHmaxf > 0.0f)
{
if (_pitch_dem > 0.0f && _PITCHmaxf > 0.0f) {
_throttle_dem = nomThr + (_THRmaxf - nomThr) * _pitch_dem / _PITCHmaxf;
}
else if (_pitch_dem < 0.0f && _PITCHminf < 0.0f)
{
} else if (_pitch_dem < 0.0f && _PITCHminf < 0.0f) {
_throttle_dem = nomThr + (_THRminf - nomThr) * _pitch_dem / _PITCHminf;
}
else
{
} else {
_throttle_dem = nomThr;
}
if (_flags.is_gliding)
{
if (_flags.is_gliding) {
_throttle_dem = 0.0f;
return;
}
@ -805,7 +779,7 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge)
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y));
float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f);
_throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf);
}
@ -824,12 +798,9 @@ void AP_TECS::_detect_bad_descent(void)
// 2) Specific total energy error > 0
// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
float STEdot = _SPEdot + _SKEdot;
if (((!_flags.underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_flags.badDescent && !_flags.underspeed && (_STE_error > 0.0f))) && !_flags.is_gliding)
{
if (((!_flags.underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_flags.badDescent && !_flags.underspeed && (_STE_error > 0.0f))) && !_flags.is_gliding) {
_flags.badDescent = true;
}
else
{
} else {
_flags.badDescent = false;
}
}
@ -878,12 +849,9 @@ void AP_TECS::_update_pitch(void)
// Calculate integrator state, constraining input if pitch limits are exceeded
float integSEB_input = SEB_error * _get_i_gain();
if (_pitch_dem > _PITCHmaxf)
{
if (_pitch_dem > _PITCHmaxf) {
integSEB_input = MIN(integSEB_input, _PITCHmaxf - _pitch_dem);
}
else if (_pitch_dem < _PITCHminf)
{
} else if (_pitch_dem < _PITCHminf) {
integSEB_input = MAX(integSEB_input, _PITCHminf - _pitch_dem);
}
float integSEB_delta = integSEB_input * _DT;
@ -957,12 +925,9 @@ void AP_TECS::_update_pitch(void)
// acceleration limit
float ptchRateIncr = _DT * _vertAccLim / _TAS_state;
if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr)
{
if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) {
_pitch_dem = _last_pitch_dem + ptchRateIncr;
}
else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr)
{
} else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) {
_pitch_dem = _last_pitch_dem - ptchRateIncr;
}
@ -975,8 +940,7 @@ void AP_TECS::_update_pitch(void)
void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
{
// Initialise states and variables if DT > 1 second or in climbout
if (_DT > 1.0f || _need_reset)
{
if (_DT > 1.0f || _need_reset) {
_integTHR_state = 0.0f;
_integSEB_state = 0.0f;
_last_throttle_dem = aparm.throttle_cruise * 0.01f;
@ -992,9 +956,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_DT = 0.1f; // when first starting TECS, use a
// small time constant
_need_reset = false;
}
else if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND)
{
} else if (_flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || _flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
_PITCHminf = 0.000174533f * ptchMinCO_cd;
_hgt_dem_adj_last = hgt_afe;
_hgt_dem_adj = _hgt_dem_adj_last;