mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_TECS: added logging of speed_weight and state flags
This commit is contained in:
Andrew Tridgell
parent
8ff932b61c
commit
b4c6a0e30e
@ -407,7 +407,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 ((_badDescent) || (_underspeed))
|
||||
if ((_flags.badDescent) || (_flags.underspeed))
|
||||
{
|
||||
_TAS_dem = _TASmin;
|
||||
}
|
||||
@ -420,7 +420,7 @@ void AP_TECS::_update_speed_demand(void)
|
||||
// Use 50% of maximum energy rate to allow margin for total energy contgroller
|
||||
float velRateMax;
|
||||
float velRateMin;
|
||||
if ((_badDescent) || (_underspeed))
|
||||
if ((_flags.badDescent) || (_flags.underspeed))
|
||||
{
|
||||
velRateMax = 0.5f * _STEdot_max / _integ5_state;
|
||||
velRateMin = 0.5f * _STEdot_min / _integ5_state;
|
||||
@ -527,20 +527,20 @@ void AP_TECS::_detect_underspeed(void)
|
||||
// see if we can clear a previous underspeed condition. We clear
|
||||
// it if we are now more than 15% above min speed, and haven't
|
||||
// been below min speed for at least 3 seconds.
|
||||
if (_underspeed &&
|
||||
if (_flags.underspeed &&
|
||||
_integ5_state >= _TASmin * 1.15f &&
|
||||
AP_HAL::millis() - _underspeed_start_ms > 3000U) {
|
||||
_underspeed = false;
|
||||
_flags.underspeed = false;
|
||||
}
|
||||
|
||||
if (_flight_stage == AP_TECS::FLIGHT_VTOL) {
|
||||
_underspeed = false;
|
||||
_flags.underspeed = false;
|
||||
} else if (((_integ5_state < _TASmin * 0.9f) &&
|
||||
(_throttle_dem >= _THRmaxf * 0.95f) &&
|
||||
_flight_stage != AP_TECS::FLIGHT_LAND_FINAL) ||
|
||||
((_height < _hgt_dem_adj) && _underspeed))
|
||||
((_height < _hgt_dem_adj) && _flags.underspeed))
|
||||
{
|
||||
_underspeed = true;
|
||||
_flags.underspeed = true;
|
||||
if (_integ5_state < _TASmin * 0.9f) {
|
||||
// reset start time as we are still underspeed
|
||||
_underspeed_start_ms = AP_HAL::millis();
|
||||
@ -548,7 +548,7 @@ void AP_TECS::_detect_underspeed(void)
|
||||
}
|
||||
else
|
||||
{
|
||||
_underspeed = false;
|
||||
_flags.underspeed = false;
|
||||
}
|
||||
}
|
||||
|
||||
@ -609,7 +609,7 @@ void AP_TECS::_update_throttle(void)
|
||||
|
||||
// Calculate throttle demand
|
||||
// If underspeed condition is set, then demand full throttle
|
||||
if (_underspeed)
|
||||
if (_flags.underspeed)
|
||||
{
|
||||
_throttle_dem = 1.0f;
|
||||
}
|
||||
@ -631,7 +631,7 @@ void AP_TECS::_update_throttle(void)
|
||||
|
||||
// Calculate PD + FF throttle
|
||||
float throttle_damp = _thrDamp;
|
||||
if (_is_doing_auto_land && !is_zero(_land_throttle_damp)) {
|
||||
if (_flags.is_doing_auto_land && !is_zero(_land_throttle_damp)) {
|
||||
throttle_damp = _land_throttle_damp;
|
||||
}
|
||||
_throttle_dem = (_STE_error + STEdot_error * throttle_damp) * K_STE2Thr + ff_throttle;
|
||||
@ -691,7 +691,7 @@ float AP_TECS::_get_i_gain(void)
|
||||
if (!is_zero(_integGain_takeoff)) {
|
||||
i_gain = _integGain_takeoff;
|
||||
}
|
||||
} else if (_is_doing_auto_land) {
|
||||
} else if (_flags.is_doing_auto_land) {
|
||||
if (!is_zero(_integGain_land)) {
|
||||
i_gain = _integGain_land;
|
||||
}
|
||||
@ -750,13 +750,13 @@ 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 ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f)))
|
||||
if ((!_flags.underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_flags.badDescent && !_flags.underspeed && (_STE_error > 0.0f)))
|
||||
{
|
||||
_badDescent = true;
|
||||
_flags.badDescent = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
_badDescent = false;
|
||||
_flags.badDescent = false;
|
||||
}
|
||||
}
|
||||
|
||||
@ -771,7 +771,7 @@ void AP_TECS::_update_pitch(void)
|
||||
float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f);
|
||||
if (!_ahrs.airspeed_sensor_enabled()) {
|
||||
SKE_weighting = 0.0f;
|
||||
} else if ( _underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
|
||||
} else if ( _flags.underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
|
||||
SKE_weighting = 2.0f;
|
||||
} else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH ||
|
||||
_flight_stage == AP_TECS::FLIGHT_LAND_PREFLARE ||
|
||||
@ -785,6 +785,8 @@ void AP_TECS::_update_pitch(void)
|
||||
}
|
||||
}
|
||||
|
||||
log_tuning.speed_weight = SKE_weighting;
|
||||
|
||||
float SPE_weighting = 2.0f - SKE_weighting;
|
||||
|
||||
// Calculate Specific Energy Balance demand, and error
|
||||
@ -877,8 +879,8 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
|
||||
_hgt_dem_in_old = _hgt_dem_adj_last;
|
||||
_TAS_dem_last = _TAS_dem;
|
||||
_TAS_dem_adj = _TAS_dem;
|
||||
_underspeed = false;
|
||||
_badDescent = false;
|
||||
_flags.underspeed = false;
|
||||
_flags.badDescent = false;
|
||||
_DT = 0.1f; // when first starting TECS, use a
|
||||
// small time constant
|
||||
}
|
||||
@ -890,8 +892,8 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
|
||||
_hgt_dem_prev = _hgt_dem_adj_last;
|
||||
_TAS_dem_last = _TAS_dem;
|
||||
_TAS_dem_adj = _TAS_dem;
|
||||
_underspeed = false;
|
||||
_badDescent = false;
|
||||
_flags.underspeed = false;
|
||||
_flags.badDescent = false;
|
||||
}
|
||||
}
|
||||
|
||||
@ -907,7 +909,7 @@ void AP_TECS::_update_STE_rate_lim(void)
|
||||
// argument allows to be true while in normal stage just before switching to approach
|
||||
bool AP_TECS::is_on_land_approach(bool include_segment_between_NORMAL_and_APPROACH)
|
||||
{
|
||||
if (!_is_doing_auto_land) {
|
||||
if (!_flags.is_doing_auto_land) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -940,7 +942,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
|
||||
_DT = MAX((now - _update_pitch_throttle_last_usec), 0U) * 1.0e-6f;
|
||||
_update_pitch_throttle_last_usec = now;
|
||||
|
||||
_is_doing_auto_land = is_doing_auto_land;
|
||||
_flags.is_doing_auto_land = is_doing_auto_land;
|
||||
_distance_beyond_land_wp = distance_beyond_land_wp;
|
||||
_flight_stage = flight_stage;
|
||||
|
||||
@ -1059,6 +1061,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
|
||||
log_tuning.thr = _throttle_dem;
|
||||
log_tuning.ptch = _pitch_dem;
|
||||
log_tuning.dspd_dem = _TAS_rate_dem;
|
||||
log_tuning.flags = _flags_byte;
|
||||
log_tuning.time_us = AP_HAL::micros64();
|
||||
}
|
||||
|
||||
|
||||
@ -120,6 +120,8 @@ public:
|
||||
float thr;
|
||||
float ptch;
|
||||
float dspd_dem;
|
||||
float speed_weight;
|
||||
uint8_t flags;
|
||||
} log_tuning;
|
||||
|
||||
private:
|
||||
@ -247,14 +249,20 @@ private:
|
||||
// Total energy rate filter state
|
||||
float _STEdotErrLast;
|
||||
|
||||
struct flags {
|
||||
// Underspeed condition
|
||||
bool _underspeed:1;
|
||||
bool underspeed:1;
|
||||
|
||||
// Bad descent condition caused by unachievable airspeed demand
|
||||
bool _badDescent:1;
|
||||
bool badDescent:1;
|
||||
|
||||
// true when plane is in auto mode and executing a land mission item
|
||||
bool _is_doing_auto_land:1;
|
||||
bool is_doing_auto_land:1;
|
||||
};
|
||||
union {
|
||||
struct flags _flags;
|
||||
uint8_t _flags_byte;
|
||||
};
|
||||
|
||||
// time when underspeed started
|
||||
uint32_t _underspeed_start_ms;
|
||||
@ -348,4 +356,4 @@ private:
|
||||
};
|
||||
|
||||
#define TECS_LOG_FORMAT(msg) { msg, sizeof(AP_TECS::log_TECS_Tuning), \
|
||||
"TECS", "Qffffffffffff", "TimeUS,h,dh,h_dem,dh_dem,sp_dem,sp,dsp,ith,iph,th,ph,dsp_dem" }
|
||||
"TECS", "QfffffffffffffB", "TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f" }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user