void AP_TECS::_update_height_demand(void) { // Apply 2 point moving average to demanded height // This is required because height demand is only updated at 5Hz _hgt_dem = 0.5f * (_hgt_dem + _hgt_dem_in_old); _hgt_dem_in_old = _hgt_dem; // Limit height rate of change if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; } _hgt_dem_prev = _hgt_dem; // Apply first order lag to height demand _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; // in final landing stage force height rate demand to the // configured sink rate and adjust the demanded height to // be kinematically consistent with the height rate. if (_flight_stage == FLIGHT_LAND_FINAL) { _integ7_state = 0; if (_flare_counter == 0) { _hgt_rate_dem = _climb_rate; _land_hgt_dem = _hgt_dem_adj; } // bring it in over 1s to prevent overshoot if (_flare_counter < 10) { _hgt_rate_dem = _hgt_rate_dem * 0.8f - 0.2f * _land_sink; _flare_counter++; } else { _hgt_rate_dem = - _land_sink; } _land_hgt_dem += 0.1f * _hgt_rate_dem; _hgt_dem_adj = _land_hgt_dem; } else { _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; _flare_counter = 0; } // for landing approach we will predict ahead by the time constant // plus the lag produced by the first order filter. This avoids a // lagged height demand while constantly descending which causes // us to consistently be above the desired glide slope. This will // be replaced with a better zero-lag filter in the future. float new_hgt_dem = _hgt_dem_adj; if (_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage == FLIGHT_LAND_FINAL) { new_hgt_dem += (_hgt_dem_adj - _hgt_dem_adj_last)*10.0f*(timeConstant()+1); } _hgt_dem_adj_last = _hgt_dem_adj; _hgt_dem_adj = new_hgt_dem; }
void AP_TECS::_update_pitch(void) { // Calculate Speed/Height Control Weighting // This is used to determine how the pitch control prioritises speed and height control // A weighting of 1 provides equal priority (this is the normal mode of operation) // A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available // A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed // rises above the demanded value, the pitch angle will be increased by the TECS controller. 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) { SKE_weighting = 2.0f; } else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH || _flight_stage == AP_TECS::FLIGHT_LAND_FINAL) { SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f); } float SPE_weighting = 2.0f - SKE_weighting; // Calculate Specific Energy Balance demand, and error float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); // Calculate integrator state, constraining input if pitch limits are exceeded float integ7_input = SEB_error * _integGain; if (_pitch_dem_unc > _PITCHmaxf) { integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc); } else if (_pitch_dem_unc < _PITCHminf) { integ7_input = max(integ7_input, _PITCHminf - _pitch_dem_unc); } _integ7_state = _integ7_state + integ7_input * _DT; // Apply max and min values for integrator state that will allow for no more than // 5deg of saturation. This allows for some pitch variation due to gusts before the // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence // 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 * timeConstant() * GRAVITY_MSS); float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * timeConstant(); if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF) { temp += _PITCHminf * gainInv; } _integ7_state = constrain_float(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); // Calculate pitch demand from specific energy balance signals _pitch_dem_unc = (temp + _integ7_state) / gainInv; // Constrain pitch demand _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); // Rate limit the pitch demand to comply with specified vertical // acceleration limit float ptchRateIncr = _DT * _vertAccLim / _integ5_state; if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) { _pitch_dem = _last_pitch_dem + ptchRateIncr; } else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) { _pitch_dem = _last_pitch_dem - ptchRateIncr; } _last_pitch_dem = _pitch_dem; }
void AP_TECS::_update_throttle(void) { // Calculate limits to be applied to potential energy error to prevent over or underspeed occurring due to large height errors float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem; float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem; // Calculate total energy error _STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est; float STEdot_dem = constrain_float((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; // Apply 0.5 second first order filter to STEdot_error // This is required to remove accelerometer noise from the measurement STEdot_error = 0.2f*STEdot_error + 0.8f*_STEdotErrLast; _STEdotErrLast = STEdot_error; // Calculate throttle demand // If underspeed condition is set, then demand full throttle if (_underspeed) { _throttle_dem_unc = 1.0f; } else { // Calculate gain scaler from specific energy error to throttle float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min)); // Calculate feed-forward throttle float ff_throttle = 0; float nomThr = aparm.throttle_cruise * 0.01f; const Matrix3f &rotMat = _ahrs.get_dcm_matrix(); // Use the demanded rate of change of total energy as the feed-forward demand, but add // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced // drag increase during turns. float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f); ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf); // Calculate PD + FF throttle _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time if (aparm.throttle_slewrate != 0) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * aparm.throttle_slewrate * 0.01f; _throttle_dem = constrain_float(_throttle_dem, _last_throttle_dem - thrRateIncr, _last_throttle_dem + thrRateIncr); _last_throttle_dem = _throttle_dem; } // Calculate integrator state upper and lower limits // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand float integ_max = (_THRmaxf - _throttle_dem + 0.1f); float integ_min = (_THRminf - _throttle_dem - 0.1f); // Calculate integrator state, constraining state // Set integrator to a max throttle value during climbout _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF) { _integ6_state = integ_max; } else { _integ6_state = constrain_float(_integ6_state, integ_min, integ_max); } // Sum the components. // Only use feed-forward component if airspeed is not being used if (_ahrs.airspeed_sensor_enabled()) { _throttle_dem = _throttle_dem + _integ6_state; } else { _throttle_dem = ff_throttle; } } // Constrain throttle demand _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf); }
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, enum FlightStage flight_stage, bool is_doing_auto_land, float distance_beyond_land_wp, int32_t ptchMinCO_cd, int16_t throttle_nudge, float hgt_afe, float load_factor) { // Calculate time in seconds since last update uint64_t now = AP_HAL::micros64(); _DT = (now - _update_pitch_throttle_last_usec) * 1.0e-6f; _update_pitch_throttle_last_usec = now; _flags.is_doing_auto_land = is_doing_auto_land; _distance_beyond_land_wp = distance_beyond_land_wp; _flight_stage = flight_stage; // Convert inputs _hgt_dem = hgt_dem_cm * 0.01f; _EAS_dem = EAS_dem_cm * 0.01f; // Update the speed estimate using a 2nd order complementary filter _update_speed(load_factor); if (aparm.takeoff_throttle_max != 0 && (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)) { _THRmaxf = aparm.takeoff_throttle_max * 0.01f; } else { _THRmaxf = aparm.throttle_max * 0.01f; } _THRminf = aparm.throttle_min * 0.01f; // work out the maximum and minimum pitch // if TECS_PITCH_{MAX,MIN} isn't set then use // LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be // larger than LIM_PITCH_{MAX,MIN} if (_pitch_max <= 0) { _PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f; } else { _PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f); } if (_pitch_min >= 0) { _PITCHminf = aparm.pitch_limit_min_cd * 0.01f; } else { _PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min_cd * 0.01f); } // apply temporary pitch limit and clear if (_pitch_max_limit < 90) { _PITCHmaxf = constrain_float(_PITCHmaxf, -90, _pitch_max_limit); _PITCHminf = constrain_float(_PITCHminf, -_pitch_max_limit, _PITCHmaxf); _pitch_max_limit = 90; } if (flight_stage == FLIGHT_LAND_FINAL) { // in flare use min pitch from LAND_PITCH_CD _PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f); // and use max pitch from TECS_LAND_PMAX if (_land_pitch_max != 0) { _PITCHmaxf = MIN(_PITCHmaxf, _land_pitch_max); } // and allow zero throttle _THRminf = 0; } else if ((flight_stage == FLIGHT_LAND_APPROACH || flight_stage == FLIGHT_LAND_PREFLARE) && (-_climb_rate) > _land_sink) { // constrain the pitch in landing as we get close to the flare // point. Use a simple linear limit from 15 meters after the // landing point float time_to_flare = (- hgt_afe / _climb_rate) - aparm.land_flare_sec; if (time_to_flare < 0) { // we should be flaring already _PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f); } else if (time_to_flare < timeConstant()*2) { // smoothly move the min pitch to the flare min pitch over // twice the time constant float p = time_to_flare/(2*timeConstant()); float pitch_limit_cd = p*aparm.pitch_limit_min_cd + (1-p)*aparm.land_pitch_cd; #if 0 ::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n", time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate); #endif _PITCHminf = MAX(_PITCHminf, pitch_limit_cd*0.01f); } } if (flight_stage == FLIGHT_TAKEOFF || flight_stage == FLIGHT_LAND_ABORT) { if (!_flags.reached_speed_takeoff && _TAS_state >= _TAS_dem_adj) { // we have reached our target speed in takeoff, allow for // normal throttle control _flags.reached_speed_takeoff = true; } } // convert to radians _PITCHmaxf = radians(_PITCHmaxf); _PITCHminf = radians(_PITCHminf); // initialise selected states and variables if DT > 1 second or in climbout _initialise_states(ptchMinCO_cd, hgt_afe); // Calculate Specific Total Energy Rate Limits _update_STE_rate_lim(); // Calculate the speed demand _update_speed_demand(); // Calculate the height demand _update_height_demand(); // Detect underspeed condition _detect_underspeed(); // Calculate specific energy quantitiues _update_energies(); // Calculate throttle demand - use simple pitch to throttle if no // airspeed sensor. // Note that caller can demand the use of // synthetic airspeed for one loop if needed. This is required // during QuadPlane transition when pitch is constrained if (_ahrs.airspeed_sensor_enabled() || _use_synthetic_airspeed || _use_synthetic_airspeed_once) { _update_throttle_with_airspeed(); _use_synthetic_airspeed_once = false; } else { _update_throttle_without_airspeed(throttle_nudge); } // Detect bad descent due to demanded airspeed being too high _detect_bad_descent(); // Calculate pitch demand _update_pitch(); // log to DataFlash DataFlash_Class::instance()->Log_Write("TECS", "TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f", "QfffffffffffffB", now, (double)_height, (double)_climb_rate, (double)_hgt_dem_adj, (double)_hgt_rate_dem, (double)_TAS_dem_adj, (double)_TAS_state, (double)_vel_dot, (double)_integTHR_state, (double)_integSEB_state, (double)_throttle_dem, (double)_pitch_dem, (double)_TAS_rate_dem, (double)logging.SKE_weighting, _flags_byte); DataFlash_Class::instance()->Log_Write("TEC2", "TimeUS,KErr,PErr,EDelta,LF", "Qffff", now, (double)logging.SKE_error, (double)logging.SPE_error, (double)logging.SEB_delta, (double)load_factor); }
void AP_TECS::_update_pitch(void) { // Calculate Speed/Height Control Weighting // This is used to determine how the pitch control prioritises speed and height control // A weighting of 1 provides equal priority (this is the normal mode of operation) // A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available // A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed // rises above the demanded value, the pitch angle will be increased by the TECS controller. 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_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { SKE_weighting = 2.0f; } else if (_flags.is_doing_auto_land) { if (_spdWeightLand < 0) { // use sliding scale from normal weight down to zero at landing float scaled_weight = _spdWeight * (1.0f - constrain_float(_path_proportion,0,1)); SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f); } else { SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f); } } logging.SKE_weighting = SKE_weighting; float SPE_weighting = 2.0f - SKE_weighting; // Calculate Specific Energy Balance demand, and error float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); logging.SKE_error = _SKE_dem - _SKE_est; logging.SPE_error = _SPE_dem - _SPE_est; // Calculate integrator state, constraining input if pitch limits are exceeded float integSEB_input = SEB_error * _get_i_gain(); if (_pitch_dem > _PITCHmaxf) { integSEB_input = MIN(integSEB_input, _PITCHmaxf - _pitch_dem); } else if (_pitch_dem < _PITCHminf) { integSEB_input = MAX(integSEB_input, _PITCHminf - _pitch_dem); } float integSEB_delta = integSEB_input * _DT; #if 0 if (_flight_stage == FLIGHT_LAND_FINAL && fabsf(_climb_rate) > 0.2f) { ::printf("_hgt_rate_dem=%.1f _hgt_dem_adj=%.1f climb=%.1f _flare_counter=%u _pitch_dem=%.1f SEB_dem=%.2f SEBdot_dem=%.2f SEB_error=%.2f SEBdot_error=%.2f\n", _hgt_rate_dem, _hgt_dem_adj, _climb_rate, _flare_counter, degrees(_pitch_dem), SEB_dem, SEBdot_dem, SEB_error, SEBdot_error); } #endif // Apply max and min values for integrator state that will allow for no more than // 5deg of saturation. This allows for some pitch variation due to gusts before the // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence // 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. // During flare a different damping gain is used float gainInv = (_TAS_state * timeConstant() * GRAVITY_MSS); float temp = SEB_error + SEBdot_dem * timeConstant(); float pitch_damp = _ptchDamp; if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) { pitch_damp = _landDamp; } else if (!is_zero(_land_pitch_damp) && _flags.is_doing_auto_land) { pitch_damp = _land_pitch_damp; } temp += SEBdot_error * pitch_damp; if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { temp += _PITCHminf * gainInv; } float integSEB_min = (gainInv * (_PITCHminf - 0.0783f)) - temp; float integSEB_max = (gainInv * (_PITCHmaxf + 0.0783f)) - temp; float integSEB_range = integSEB_max - integSEB_min; logging.SEB_delta = integSEB_delta; // don't allow the integrator to rise by more than 20% of its full // range in one step. This prevents single value glitches from // causing massive integrator changes. See Issue#4066 integSEB_delta = constrain_float(integSEB_delta, -integSEB_range*0.1f, integSEB_range*0.1f); // integrate _integSEB_state = constrain_float(_integSEB_state + integSEB_delta, integSEB_min, integSEB_max); // Calculate pitch demand from specific energy balance signals _pitch_dem_unc = (temp + _integSEB_state) / gainInv; // Constrain pitch demand _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); // Rate limit the pitch demand to comply with specified vertical // acceleration limit float ptchRateIncr = _DT * _vertAccLim / _TAS_state; if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) { _pitch_dem = _last_pitch_dem + ptchRateIncr; } else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) { _pitch_dem = _last_pitch_dem - ptchRateIncr; } // re-constrain pitch demand _pitch_dem = constrain_float(_pitch_dem, _PITCHminf, _PITCHmaxf); _last_pitch_dem = _pitch_dem; }
/* calculate throttle demand - airspeed enabled case */ void AP_TECS::_update_throttle_with_airspeed(void) { // Calculate limits to be applied to potential energy error to prevent over or underspeed occurring due to large height errors float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem; float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem; // Calculate total energy error _STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est; float STEdot_dem = constrain_float((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; // Apply 0.5 second first order filter to STEdot_error // This is required to remove accelerometer noise from the measurement STEdot_error = 0.2f*STEdot_error + 0.8f*_STEdotErrLast; _STEdotErrLast = STEdot_error; // Calculate throttle demand // If underspeed condition is set, then demand full throttle if (_flags.underspeed) { _throttle_dem = 1.0f; } else { // Calculate gain scaler from specific energy error to throttle float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min)); // Calculate feed-forward throttle float ff_throttle = 0; float nomThr = aparm.throttle_cruise * 0.01f; const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned(); // Use the demanded rate of change of total energy as the feed-forward demand, but add // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced // drag increase during turns. float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f); ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf); // Calculate PD + FF throttle float throttle_damp = _thrDamp; 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; // Constrain throttle demand _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf); float THRminf_clipped_to_zero = constrain_float(_THRminf, 0, _THRmaxf); // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time if (aparm.throttle_slewrate != 0) { float thrRateIncr = _DT * (_THRmaxf - THRminf_clipped_to_zero) * aparm.throttle_slewrate * 0.01f; _throttle_dem = constrain_float(_throttle_dem, _last_throttle_dem - thrRateIncr, _last_throttle_dem + thrRateIncr); _last_throttle_dem = _throttle_dem; } // Calculate integrator state upper and lower limits // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand // Additionally constrain the integrator state amplitude so that the integrator comes off limits faster. float maxAmp = 0.5f*(_THRmaxf - THRminf_clipped_to_zero); float integ_max = constrain_float((_THRmaxf - _throttle_dem + 0.1f),-maxAmp,maxAmp); float integ_min = constrain_float((_THRminf - _throttle_dem - 0.1f),-maxAmp,maxAmp); // 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_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { if (!_flags.reached_speed_takeoff) { // ensure we run at full throttle until we reach the target airspeed _throttle_dem = MAX(_throttle_dem, _THRmaxf - _integTHR_state); } _integTHR_state = integ_max; } else { _integTHR_state = constrain_float(_integTHR_state, integ_min, integ_max); } // Sum the components. _throttle_dem = _throttle_dem + _integTHR_state; } // Constrain throttle demand _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf); }
void AP_TECS::_update_height_demand(void) { // Apply 2 point moving average to demanded height _hgt_dem = 0.5f * (_hgt_dem + _hgt_dem_in_old); _hgt_dem_in_old = _hgt_dem; float max_sink_rate = _maxSinkRate; if (_maxSinkRate_approach > 0 && _flags.is_doing_auto_land) { // special sink rate for approach to accommodate steep slopes and reverse thrust. // A special check must be done to see if we're LANDing on approach but also if // we're in that tiny window just starting NAV_LAND but still in NORMAL mode. If // we have a steep slope with a short approach we'll want to allow acquiring the // glide slope right away. max_sink_rate = _maxSinkRate_approach; } // Limit height rate of change if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; } else if ((_hgt_dem - _hgt_dem_prev) < (-max_sink_rate * 0.1f)) { _hgt_dem = _hgt_dem_prev - max_sink_rate * 0.1f; } _hgt_dem_prev = _hgt_dem; // Apply first order lag to height demand _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; // in final landing stage force height rate demand to the // configured sink rate and adjust the demanded height to // be kinematically consistent with the height rate. if (_flight_stage == FLIGHT_LAND_FINAL) { _integSEB_state = 0; if (_flare_counter == 0) { _hgt_rate_dem = _climb_rate; _land_hgt_dem = _hgt_dem_adj; } // adjust the flare sink rate to increase/decrease as your travel further beyond the land wp float land_sink_rate_adj = _land_sink + _land_sink_rate_change*_distance_beyond_land_wp; // bring it in over 1s to prevent overshoot if (_flare_counter < 10) { _hgt_rate_dem = _hgt_rate_dem * 0.8f - 0.2f * land_sink_rate_adj; _flare_counter++; } else { _hgt_rate_dem = - land_sink_rate_adj; } _land_hgt_dem += 0.1f * _hgt_rate_dem; _hgt_dem_adj = _land_hgt_dem; } else { _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; _flare_counter = 0; } // for landing approach we will predict ahead by the time constant // plus the lag produced by the first order filter. This avoids a // lagged height demand while constantly descending which causes // us to consistently be above the desired glide slope. This will // be replaced with a better zero-lag filter in the future. float new_hgt_dem = _hgt_dem_adj; if (_flags.is_doing_auto_land) { if (hgt_dem_lag_filter_slew < 1) { hgt_dem_lag_filter_slew += 0.1f; // increment at 10Hz to gradually apply the compensation at first } else { hgt_dem_lag_filter_slew = 1; } new_hgt_dem += hgt_dem_lag_filter_slew*(_hgt_dem_adj - _hgt_dem_adj_last)*10.0f*(timeConstant()+1); } else { hgt_dem_lag_filter_slew = 0; } _hgt_dem_adj_last = _hgt_dem_adj; _hgt_dem_adj = new_hgt_dem; }
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, enum FlightStage flight_stage, uint8_t mission_cmd_id, int32_t ptchMinCO_cd, int16_t throttle_nudge, float hgt_afe, float load_factor) { // Calculate time in seconds since last update uint32_t now = AP_HAL::micros(); _DT = MAX((now - _update_pitch_throttle_last_usec), 0U) * 1.0e-6f; _update_pitch_throttle_last_usec = now; // store the mission cmd. This is needed due to the delay between // starting an approach, and switching to flight stage approach. // This delay can become a problem with short approaches on steep // landings using reverse thrust. _mission_cmd_id = mission_cmd_id; // Update the speed estimate using a 2nd order complementary filter _update_speed(load_factor); // Convert inputs _hgt_dem = hgt_dem_cm * 0.01f; _EAS_dem = EAS_dem_cm * 0.01f; if (aparm.takeoff_throttle_max != 0 && (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)) { _THRmaxf = aparm.takeoff_throttle_max * 0.01f; } else { _THRmaxf = aparm.throttle_max * 0.01f; } _THRminf = aparm.throttle_min * 0.01f; // work out the maximum and minimum pitch // if TECS_PITCH_{MAX,MIN} isn't set then use // LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be // larger than LIM_PITCH_{MAX,MIN} if (_pitch_max <= 0) { _PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f; } else { _PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f); } // apply temporary pitch limit and clear _PITCHmaxf = constrain_float(_PITCHmaxf, -90, _pitch_max_limit); _pitch_max_limit = 90; if (_pitch_min >= 0) { _PITCHminf = aparm.pitch_limit_min_cd * 0.01f; } else { _PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min_cd * 0.01f); } if (flight_stage == FLIGHT_LAND_FINAL) { // in flare use min pitch from LAND_PITCH_CD _PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f); // and use max pitch from TECS_LAND_PMAX if (_land_pitch_max > 0) { _PITCHmaxf = MIN(_PITCHmaxf, _land_pitch_max); } // and allow zero throttle _THRminf = 0; } else if ((flight_stage == FLIGHT_LAND_APPROACH || flight_stage == FLIGHT_LAND_PREFLARE) && (-_climb_rate) > _land_sink) { // constrain the pitch in landing as we get close to the flare // point. Use a simple linear limit from 15 meters after the // landing point float time_to_flare = (- hgt_afe / _climb_rate) - aparm.land_flare_sec; if (time_to_flare < 0) { // we should be flaring already _PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f); } else if (time_to_flare < timeConstant()*2) { // smoothly move the min pitch to the flare min pitch over // twice the time constant float p = time_to_flare/(2*timeConstant()); float pitch_limit_cd = p*aparm.pitch_limit_min_cd + (1-p)*aparm.land_pitch_cd; #if 0 ::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n", time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate); #endif _PITCHminf = MAX(_PITCHminf, pitch_limit_cd*0.01f); } } // convert to radians _PITCHmaxf = radians(_PITCHmaxf); _PITCHminf = radians(_PITCHminf); _flight_stage = flight_stage; // initialise selected states and variables if DT > 1 second or in climbout _initialise_states(ptchMinCO_cd, hgt_afe); // Calculate Specific Total Energy Rate Limits _update_STE_rate_lim(); // Calculate the speed demand _update_speed_demand(); // Calculate the height demand _update_height_demand(); // Detect underspeed condition _detect_underspeed(); // Calculate specific energy quantitiues _update_energies(); // Calculate throttle demand - use simple pitch to throttle if no airspeed sensor if (_ahrs.airspeed_sensor_enabled()) { _update_throttle(); } else { _update_throttle_option(throttle_nudge); } // Detect bad descent due to demanded airspeed being too high _detect_bad_descent(); // Calculate pitch demand _update_pitch(); // Write internal variables to the log_tuning structure. This // structure will be logged in dataflash at 10Hz log_tuning.hgt_dem = _hgt_dem_adj; log_tuning.hgt = _height; log_tuning.dhgt_dem = _hgt_rate_dem; log_tuning.dhgt = _climb_rate; log_tuning.spd_dem = _TAS_dem_adj; log_tuning.spd = _integ5_state; log_tuning.dspd = _vel_dot; log_tuning.ithr = _integ6_state; log_tuning.iptch = _integ7_state; log_tuning.thr = _throttle_dem; log_tuning.ptch = _pitch_dem; log_tuning.dspd_dem = _TAS_rate_dem; log_tuning.time_us = AP_HAL::micros64(); }
void AP_TECS::_update_height_demand(void) { // Apply 2 point moving average to demanded height // This is required because height demand is only updated at 5Hz _hgt_dem = 0.5f * (_hgt_dem + _hgt_dem_in_old); _hgt_dem_in_old = _hgt_dem; float max_sink_rate = _maxSinkRate; if (_maxSinkRate_approach > 0 && is_on_land_approach(true)) { // special sink rate for approach to accommodate steep slopes and reverse thrust. // A special check must be done to see if we're LANDing on approach but also if // we're in that tiny window just starting NAV_LAND but still in NORMAL mode. If // we have a steep slope with a short approach we'll want to allow acquiring the // glide slope right away. max_sink_rate = _maxSinkRate_approach; } // Limit height rate of change if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; } else if ((_hgt_dem - _hgt_dem_prev) < (-max_sink_rate * 0.1f)) { _hgt_dem = _hgt_dem_prev - max_sink_rate * 0.1f; } _hgt_dem_prev = _hgt_dem; // Apply first order lag to height demand _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; // in final landing stage force height rate demand to the // configured sink rate and adjust the demanded height to // be kinematically consistent with the height rate. if (_flight_stage == FLIGHT_LAND_FINAL) { _integ7_state = 0; if (_flare_counter == 0) { _hgt_rate_dem = _climb_rate; _land_hgt_dem = _hgt_dem_adj; } // bring it in over 1s to prevent overshoot if (_flare_counter < 10) { _hgt_rate_dem = _hgt_rate_dem * 0.8f - 0.2f * _land_sink; _flare_counter++; } else { _hgt_rate_dem = - _land_sink; } _land_hgt_dem += 0.1f * _hgt_rate_dem; _hgt_dem_adj = _land_hgt_dem; } else { _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; _flare_counter = 0; } // for landing approach we will predict ahead by the time constant // plus the lag produced by the first order filter. This avoids a // lagged height demand while constantly descending which causes // us to consistently be above the desired glide slope. This will // be replaced with a better zero-lag filter in the future. float new_hgt_dem = _hgt_dem_adj; if (_flight_stage == FLIGHT_LAND_APPROACH || _flight_stage == FLIGHT_LAND_FINAL || _flight_stage == FLIGHT_LAND_PREFLARE) { new_hgt_dem += (_hgt_dem_adj - _hgt_dem_adj_last)*10.0f*(timeConstant()+1); } _hgt_dem_adj_last = _hgt_dem_adj; _hgt_dem_adj = new_hgt_dem; }