@ -658,7 +658,7 @@ void AP_TECS::_update_throttle_with_airspeed(void)
@@ -658,7 +658,7 @@ 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_LAND_ ABORT )
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
@ -768,7 +768,7 @@ void AP_TECS::_update_pitch(void)
@@ -768,7 +768,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 ( _flags . underspeed | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_LAND_ ABORT ) {
} else if ( _flags . underspeed | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND ) {
SKE_weighting = 2.0f ;
} else if ( _flags . is_doing_auto_land ) {
if ( _spdWeightLand < 0 ) {
@ -832,7 +832,7 @@ void AP_TECS::_update_pitch(void)
@@ -832,7 +832,7 @@ void AP_TECS::_update_pitch(void)
}
temp + = SEBdot_error * pitch_damp ;
if ( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_LAND_ ABORT ) {
if ( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND ) {
temp + = _PITCHminf * gainInv ;
}
float integSEB_min = ( gainInv * ( _PITCHminf - 0.0783f ) ) - temp ;
@ -895,7 +895,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
@@ -895,7 +895,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
}
else if ( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_LAND_ ABORT )
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 ;
@ -907,7 +907,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
@@ -907,7 +907,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_flags . badDescent = false ;
}
if ( _flight_stage ! = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF & & _flight_stage ! = AP_Vehicle : : FixedWing : : FLIGHT_LAND_ ABORT ) {
if ( _flight_stage ! = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF & & _flight_stage ! = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND ) {
// reset takeoff speed flag when not in takeoff
_flags . reached_speed_takeoff = false ;
}
@ -948,7 +948,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
@@ -948,7 +948,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_update_speed ( load_factor ) ;
if ( aparm . takeoff_throttle_max ! = 0 & &
( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_LAND_ ABORT ) ) {
( _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | _flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND ) ) {
_THRmaxf = aparm . takeoff_throttle_max * 0.01f ;
} else {
_THRmaxf = aparm . throttle_max * 0.01f ;
@ -1010,7 +1010,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
@@ -1010,7 +1010,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
}
}
if ( flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_LAND_ ABORT ) {
if ( flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_TAKEOFF | | flight_stage = = AP_Vehicle : : FixedWing : : FLIGHT_ABORT_LAND ) {
if ( ! _flags . reached_speed_takeoff & & _TAS_state > = _TAS_dem_adj ) {
// we have reached our target speed in takeoff, allow for
// normal throttle control