From b4c6a0e30e699f192ca62e334564dfe99b34a5de Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Mar 2016 08:41:09 +1100 Subject: [PATCH] AP_TECS: added logging of speed_weight and state flags --- libraries/AP_TECS/AP_TECS.cpp | 45 +++++++++++++++++++---------------- libraries/AP_TECS/AP_TECS.h | 22 +++++++++++------ 2 files changed, 39 insertions(+), 28 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 83c1f0bac3..72db49f11e 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -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(); } diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 377e680b9b..70bfe81319 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -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; - // Underspeed condition - bool _underspeed:1; + struct flags { + // Underspeed condition + bool underspeed:1; - // Bad descent condition caused by unachievable airspeed demand - bool _badDescent:1; + // Bad descent condition caused by unachievable airspeed demand + bool badDescent:1; - // true when plane is in auto mode and executing a land mission item - bool _is_doing_auto_land:1; + // true when plane is in auto mode and executing a land mission item + 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" }