Пример #1
0
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();
}
Пример #2
0
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();
}
Пример #3
0
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);
}
Пример #4
0
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();
}