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: added logging of speed_weight and state flags

Browse Source
This commit is contained in:
Andrew Tridgell 2016-03-26 08:41:09 +11:00
parent 8ff932b61c
commit b4c6a0e30e
2 changed files with 39 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_TECS/AP_TECS.cpp
View File

@ -407,7 +407,7 @@ void AP_TECS::_update_speed_demand(void)
// This will minimise the rate of descent resulting from an engine failure, // 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 // enable the maximum climb rate to be achieved and prevent continued full power descent
// into the ground due to an unachievable airspeed value // into the ground due to an unachievable airspeed value
if ((_badDescent) || (_underspeed)) if ((_flags.badDescent) || (_flags.underspeed))
{ {
_TAS_dem = _TASmin; _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 // Use 50% of maximum energy rate to allow margin for total energy contgroller
float velRateMax; float velRateMax;
float velRateMin; float velRateMin;
if ((_badDescent) || (_underspeed)) if ((_flags.badDescent) || (_flags.underspeed))
{ {
velRateMax = 0.5f * _STEdot_max / _integ5_state; velRateMax = 0.5f * _STEdot_max / _integ5_state;
velRateMin = 0.5f * _STEdot_min / _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 // 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 // it if we are now more than 15% above min speed, and haven't
// been below min speed for at least 3 seconds. // been below min speed for at least 3 seconds.
if (_underspeed && if (_flags.underspeed &&
_integ5_state >= _TASmin * 1.15f && _integ5_state >= _TASmin * 1.15f &&
AP_HAL::millis() - _underspeed_start_ms > 3000U) { AP_HAL::millis() - _underspeed_start_ms > 3000U) {
_underspeed = false; _flags.underspeed = false;
} }
if (_flight_stage == AP_TECS::FLIGHT_VTOL) { if (_flight_stage == AP_TECS::FLIGHT_VTOL) {
_underspeed = false; _flags.underspeed = false;
} else if (((_integ5_state < _TASmin * 0.9f) && } else if (((_integ5_state < _TASmin * 0.9f) &&
(_throttle_dem >= _THRmaxf * 0.95f) && (_throttle_dem >= _THRmaxf * 0.95f) &&
_flight_stage != AP_TECS::FLIGHT_LAND_FINAL) || _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) { if (_integ5_state < _TASmin * 0.9f) {
// reset start time as we are still underspeed // reset start time as we are still underspeed
_underspeed_start_ms = AP_HAL::millis(); _underspeed_start_ms = AP_HAL::millis();
@ -548,7 +548,7 @@ void AP_TECS::_detect_underspeed(void)
} }
else else
{ {
_underspeed = false; _flags.underspeed = false;
} }
} }
@ -609,7 +609,7 @@ void AP_TECS::_update_throttle(void)
// Calculate throttle demand // Calculate throttle demand
// If underspeed condition is set, then demand full throttle // If underspeed condition is set, then demand full throttle
if (_underspeed) if (_flags.underspeed)
{ {
_throttle_dem = 1.0f; _throttle_dem = 1.0f;
} }
@ -631,7 +631,7 @@ void AP_TECS::_update_throttle(void)
// Calculate PD + FF throttle // Calculate PD + FF throttle
float throttle_damp = _thrDamp; 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_damp = _land_throttle_damp;
} }
_throttle_dem = (_STE_error + STEdot_error * throttle_damp) * K_STE2Thr + ff_throttle; _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)) { if (!is_zero(_integGain_takeoff)) {
i_gain = _integGain_takeoff; i_gain = _integGain_takeoff;
} }
} else if (_is_doing_auto_land) { } else if (_flags.is_doing_auto_land) {
if (!is_zero(_integGain_land)) { if (!is_zero(_integGain_land)) {
i_gain = _integGain_land; i_gain = _integGain_land;
} }
@ -750,13 +750,13 @@ void AP_TECS::_detect_bad_descent(void)
// 2) Specific total energy error > 0 // 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 // 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; 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 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); float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f);
if (!_ahrs.airspeed_sensor_enabled()) { if (!_ahrs.airspeed_sensor_enabled()) {
SKE_weighting = 0.0f; 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; SKE_weighting = 2.0f;
} else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH || } else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH ||
_flight_stage == AP_TECS::FLIGHT_LAND_PREFLARE || _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; float SPE_weighting = 2.0f - SKE_weighting;
// Calculate Specific Energy Balance demand, and error // 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; _hgt_dem_in_old = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem; _TAS_dem_last = _TAS_dem;
_TAS_dem_adj = _TAS_dem; _TAS_dem_adj = _TAS_dem;
_underspeed = false; _flags.underspeed = false;
_badDescent = false; _flags.badDescent = false;
_DT = 0.1f; // when first starting TECS, use a _DT = 0.1f; // when first starting TECS, use a
// small time constant // 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; _hgt_dem_prev = _hgt_dem_adj_last;
_TAS_dem_last = _TAS_dem; _TAS_dem_last = _TAS_dem;
_TAS_dem_adj = _TAS_dem; _TAS_dem_adj = _TAS_dem;
_underspeed = false; _flags.underspeed = false;
_badDescent = 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 // 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) 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; 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; _DT = MAX((now - _update_pitch_throttle_last_usec), 0U) * 1.0e-6f;
_update_pitch_throttle_last_usec = now; _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; _distance_beyond_land_wp = distance_beyond_land_wp;
_flight_stage = flight_stage; _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.thr = _throttle_dem;
log_tuning.ptch = _pitch_dem; log_tuning.ptch = _pitch_dem;
log_tuning.dspd_dem = _TAS_rate_dem; log_tuning.dspd_dem = _TAS_rate_dem;
log_tuning.flags = _flags_byte;
log_tuning.time_us = AP_HAL::micros64(); log_tuning.time_us = AP_HAL::micros64();
} }

22
libraries/AP_TECS/AP_TECS.h
View File

@ -120,6 +120,8 @@ public:
float thr; float thr;
float ptch; float ptch;
float dspd_dem; float dspd_dem;
float speed_weight;
uint8_t flags;
} log_tuning; } log_tuning;
private: private:
@ -247,14 +249,20 @@ private:
// Total energy rate filter state // Total energy rate filter state
float _STEdotErrLast; float _STEdotErrLast;
// Underspeed condition struct flags {
bool _underspeed:1; // Underspeed condition
bool underspeed:1;
// Bad descent condition caused by unachievable airspeed demand // 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 // 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 // time when underspeed started
uint32_t _underspeed_start_ms; uint32_t _underspeed_start_ms;
@ -348,4 +356,4 @@ private:
}; };
#define TECS_LOG_FORMAT(msg) { msg, sizeof(AP_TECS::log_TECS_Tuning), \ #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" }