diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 94181dc22b..5885b72646 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -165,6 +165,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, _integ4_state = _integ4_state + integ4_input * DT; float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f; _integ5_state = _integ5_state + integ5_input * DT; + // limit the airspeed to a minimum of 3 m/s _integ5_state = max(_integ5_state, 3.0f); _update_speed_last_usec = now; @@ -464,14 +465,14 @@ void TECS::_update_pitch(void) // During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the // integrator has to catch up before the nose can be raised to reduce speed during climbout. - float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); + float gainInv = _integ5_state * _timeConst * CONSTANTS_ONE_G; float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; - if (_climbOutDem) - { + if (_climbOutDem) { temp += _PITCHminf * gainInv; } - _integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); - + float integ7_err_min = (gainInv * (_PITCHminf - math::radians(5.0f))) - temp; + float integ7_err_max = (gainInv * (_PITCHmaxf + math::radians(5.0f))) - temp; + _integ7_state = constrain(_integ7_state, integ7_err_min, integ7_err_max); // Calculate pitch demand from specific energy balance signals _pitch_dem_unc = (temp + _integ7_state) / gainInv; @@ -493,7 +494,7 @@ void TECS::_update_pitch(void) _last_pitch_dem = _pitch_dem; } -void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad) +void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad, float EAS2TAS) { // Initialise states and variables if DT > 1 second or in climbout if (_update_pitch_throttle_last_usec == 0 || _DT > DT_MAX || !_in_air || !_states_initalized) { @@ -501,18 +502,22 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt _integ2_state = 0.0f; _integ3_state = baro_altitude; _integ4_state = 0.0f; - _integ5_state = _EAS; + _integ5_state = _EAS * EAS2TAS; _integ6_state = 0.0f; _integ7_state = 0.0f; + _last_throttle_dem = throttle_cruise; - _last_pitch_dem = pitch; + _last_pitch_dem = constrain(pitch, _PITCHminf, _PITCHmaxf); + _pitch_dem_unc = _last_pitch_dem; + _hgt_dem_adj_last = baro_altitude; _hgt_dem_adj = _hgt_dem_adj_last; _hgt_dem_prev = _hgt_dem_adj_last; _hgt_dem_in_old = _hgt_dem_adj_last; - _TAS_dem_last = _TAS_dem; - _TAS_dem_adj = _TAS_dem; - _pitch_dem_unc = pitch; + + _TAS_dem_last = _EAS * EAS2TAS; + _TAS_dem_adj = _TAS_dem_last; + _underspeed = false; _badDescent = false; @@ -523,11 +528,14 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt } else if (_climbOutDem) { _PITCHminf = ptchMinCO_rad; _THRminf = _THRmaxf - 0.01f; + _hgt_dem_adj_last = baro_altitude; _hgt_dem_adj = _hgt_dem_adj_last; _hgt_dem_prev = _hgt_dem_adj_last; - _TAS_dem_last = _TAS_dem; - _TAS_dem_adj = _TAS_dem; + + _TAS_dem_last = _EAS * EAS2TAS; + _TAS_dem_adj = _EAS * EAS2TAS; + _underspeed = false; _badDescent = false; } @@ -543,9 +551,9 @@ void TECS::_update_STE_rate_lim(void) _STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; } -void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, - float throttle_min, float throttle_max, float throttle_cruise, - float pitch_limit_min, float pitch_limit_max) +void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, + float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { // Calculate time in seconds since last update @@ -563,7 +571,7 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f _climbOutDem = climbOutDem; // initialise selected states and variables if DT > 1 second or in climbout - _initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO); + _initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO, EAS2TAS); if (!_in_air) { return; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index cbcc6ed9ae..7c20fea58f 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -456,7 +456,7 @@ private: void _update_pitch(void); // Initialise states and variables - void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad); + void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad, float EAS2TAS); // Calculate specific total energy rate limits void _update_STE_rate_lim(void); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index beb8639798..c514d99f14 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -68,6 +68,7 @@ #include #include "landingslope.h" + #include #include #include @@ -84,7 +85,6 @@ #include #include #include -#include #include #include #include @@ -98,7 +98,6 @@ #include #include #include -#include #include #include #include @@ -2405,7 +2404,6 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, unsigned mode) { - bool run_tecs = true; float dt = 0.01f; // prevent division with 0 if (_last_tecs_update > 0) { @@ -2415,7 +2413,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _last_tecs_update = hrt_absolute_time(); // do not run TECS if we are not in air - run_tecs &= !_vehicle_land_detected.landed; + bool run_tecs = !_vehicle_land_detected.landed; // do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition // (it should also not run during VTOL blending because airspeed is too low still) @@ -2442,14 +2440,10 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ // after transition we ramp up desired airspeed from the speed we had coming out of the transition _asp_after_transition += dt * 2; // increase 2m/s - - //throttle_cruise = - if (_asp_after_transition < v_sp && _ctrl_state.airspeed < v_sp) { v_sp = fmaxf(_asp_after_transition, _ctrl_state.airspeed); - } - else { + } else { _was_in_transition = false; _asp_after_transition = 0; } @@ -2479,7 +2473,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* Using tecs library */ - float pitch_for_tecs = _pitch; + float pitch_for_tecs = _pitch - _parameters.pitchsp_offset_rad; // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset // between multirotor and fixed wing flight