void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
int32_t EAS_dem_cm,
enum FlightStage flight_stage,
int32_t ptchMinCO_cd,
int16_t throttle_nudge,
float hgt_afe)
{
// Calculate time in seconds since last update
uint32_t now = hal.scheduler->micros();
_DT = max((now - _update_pitch_throttle_last_usec),0)*1.0e-6f;
_update_pitch_throttle_last_usec = now;
// Update the speed estimate using a 2nd order complementary filter
_update_speed();
// Convert inputs
_hgt_dem = hgt_dem_cm * 0.01f;
_EAS_dem = EAS_dem_cm * 0.01f;
_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);
}
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 allow zero throttle
_THRminf = 0;
}
// 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 = _integ3_state;
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_ms = hal.scheduler->millis();
}
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_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_throttle(int32_t hgt_dem_cm,
int32_t EAS_dem_cm,
enum FlightStage flight_stage,
int32_t ptchMinCO_cd,
int16_t throttle_nudge,
float hgt_afe)
{
// Calculate time in seconds since last update
uint32_t now = hal.scheduler->micros();
_DT = max((now - _update_pitch_throttle_last_usec),0)*1.0e-6f;
_update_pitch_throttle_last_usec = now;
// Update the speed estimate using a 2nd order complementary filter
_update_speed();
// Convert inputs
_hgt_dem = hgt_dem_cm * 0.01f;
_EAS_dem = EAS_dem_cm * 0.01f;
_THRmaxf = aparm.throttle_max * 0.01f;
_THRminf = aparm.throttle_min * 0.01f;
_PITCHmaxf = 0.000174533f * aparm.pitch_limit_max_cd;
_PITCHminf = 0.000174533f * aparm.pitch_limit_min_cd;
_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 = _integ3_state;
log_tuning.dhgt_dem = _hgt_rate_dem;
log_tuning.dhgt = _integ2_state;
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_ms = hal.scheduler->millis();
}
