dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FW init TECS with PSP_OFF and constrain TAS error

This commit is contained in:
Daniel Agar 2016-09-25 16:04:11 -04:00 committed by Lorenz Meier
parent ebdfa2860b
commit fb75240ee9
3 changed files with 30 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -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;

2
src/lib/external_lgpl/tecs/tecs.h
View File

@ -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);

14
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -68,6 +68,7 @@
#include <px4_posix.h>
#include "landingslope.h"
#include <arch/board/board.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_hrt.h>
@ -84,7 +85,6 @@
#include <systemlib/perf_counter.h>
#include <systemlib/pid/pid.h>
#include <systemlib/systemlib.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/fw_virtual_attitude_setpoint.h>
@ -98,7 +98,6 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
#include <vtol_att_control/vtol_type.h>
@ -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