Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-05 22:24:47 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

TECS remove unused and fix style

Browse Source
This commit is contained in:
Daniel Agar
2017-02-27 18:57:15 -05:00
committed by Lorenz Meier
parent 22d0e507c6
commit 027e8ee8c0
2 changed files with 97 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/external_lgpl/tecs/tecs.cpp
+35 -37
View File
@@ -1,11 +1,14 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#include "tecs.h"
#include <ecl/ecl.h>
#include <systemlib/err.h>
#include <geo/geo.h>
using namespace math;
using math::constrain;
using math::max;
using math::min;
/**
* @file tecs.cpp
@@ -27,8 +30,8 @@ using namespace math;
*
*/
void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat,
const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air)
void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<3, 3> &rotMat,
const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air)
{
// Implement third order complementary filter for height and height rate
// estimted height rate = _integ2_state
@@ -42,10 +45,6 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
uint64_t now = ecl_absolute_time();
float DT = max((now - _update_50hz_last_usec), UINT64_C(0)) * 1.0e-6f;
// printf("dt: %10.6f baro alt: %6.2f eas: %6.2f R(0,0): %6.2f, R(1,1): %6.2f\naccel body: %6.2f %6.2f %6.2f\naccel earth: %6.2f %6.2f %6.2f\n",
// DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2),
// accel_earth(0), accel_earth(1), accel_earth(2));
bool reset_altitude = false;
if (_update_50hz_last_usec == 0 || DT > DT_MAX) {
@@ -74,6 +73,7 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
// Get height acceleration
float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G);
// Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega
float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega;
@@ -95,13 +95,11 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
// Update and average speed rate of change
// Only required if airspeed is being measured and controlled
float temp = 0;
if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) {
// Get DCM
// Calculate speed rate of change
// XXX check
temp = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0);
float temp = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0);
// take 5 point moving average
//_vel_dot = _vdot_filter.apply(temp);
// XXX resolve this properly
@@ -117,25 +115,23 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
}
void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS)
void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, float EAS2TAS)
{
// Calculate time in seconds since last update
uint64_t now = ecl_absolute_time();
float DT = max((now - _update_speed_last_usec), UINT64_C(0)) * 1.0e-6f;
// Convert equivalent airspeeds to true airspeeds
_EAS_dem = airspeed_demand;
_TAS_dem = _EAS_dem * EAS2TAS;
_TASmax = indicated_airspeed_max * EAS2TAS;
_TASmin = indicated_airspeed_min * EAS2TAS;
_TASmax = _indicated_airspeed_max * EAS2TAS;
_TASmin = _indicated_airspeed_min * EAS2TAS;
// Get airspeed or default to halfway between min and max if
// airspeed is not being used and set speed rate to zero
if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) {
// If no airspeed available use average of min and max
_EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max);
_EAS = 0.5f * (_indicated_airspeed_min + _indicated_airspeed_max);
} else {
_EAS = indicated_airspeed;
@@ -159,7 +155,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
// Prevent state from winding up
if (_integ5_state < 3.1f) {
integ4_input = max(integ4_input , 0.0f);
integ4_input = max(integ4_input, 0.0f);
}
_integ4_state = _integ4_state + integ4_input * DT;
@@ -216,15 +212,13 @@ void TECS::_update_speed_demand()
// _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / _DT;
// }
_TAS_dem_adj = constrain(_TAS_dem, _TASmin, _TASmax);;
_TAS_rate_dem = constrain((_TAS_dem_adj - _integ5_state) * _speedrate_p, velRateMin, velRateMax); //xxx: using a p loop for now
_TAS_dem_adj = constrain(_TAS_dem, _TASmin, _TASmax);
//xxx: using a p loop for now
_TAS_rate_dem = constrain((_TAS_dem_adj - _integ5_state) * _speedrate_p, velRateMin, velRateMax);
// _TAS_dem_last = _TAS_dem;
// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f",
// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin);
// warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u",
// (double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent , _underspeed);
}
void TECS::_update_height_demand(float demand, float state)
@@ -233,13 +227,16 @@ void TECS::_update_height_demand(float demand, float state)
if (PX4_ISFINITE(demand) && fabsf(_hgt_dem_in_old) < 0.1f) {
_hgt_dem_in_old = demand;
}
// Apply 2 point moving average to demanded height
// This is required because height demand is updated in steps
if (PX4_ISFINITE(demand)) {
_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
} else {
_hgt_dem = _hgt_dem_in_old;
}
_hgt_dem_in_old = _hgt_dem;
// Limit height demand
@@ -274,7 +271,9 @@ void TECS::_detect_underspeed()
return;
}
if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) {
if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj)
&& _underspeed)) {
_underspeed = true;
} else {
@@ -301,7 +300,7 @@ void TECS::_update_energies()
_SKEdot = _integ5_state * _vel_dot;
}
void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat)
void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3, 3> &rotMat)
{
// Calculate total energy values
_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
@@ -329,7 +328,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns.
float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi, 0.1f, 1.0f) - 1.0f);
if (STEdot_dem >= 0) {
ff_throttle = nomThr + STEdot_dem / _STEdot_max * (_THRmaxf - nomThr);
@@ -346,9 +345,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
// Calculate the throttle increment from the specified slew time
if (fabsf(_throttle_slewrate) > 0.01f) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
_throttle_dem = constrain(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_throttle_dem = constrain(_throttle_dem, _last_throttle_dem - thrRateIncr, _last_throttle_dem + thrRateIncr);
}
// Ensure _last_throttle_dem is always initialized properly
@@ -486,7 +483,8 @@ void TECS::_update_pitch()
_last_pitch_dem = _pitch_dem;
}
void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad, float EAS2TAS)
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) {
@@ -543,18 +541,15 @@ void TECS::_update_STE_rate_lim()
_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
uint64_t now = ecl_absolute_time();
_DT = max((now - _update_pitch_throttle_last_usec), UINT64_C(0)) * 1.0e-6f;
// printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n",
// _DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max);
// Convert inputs
_THRmaxf = throttle_max;
_THRminf = throttle_min;
@@ -570,7 +565,7 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
}
// Update the speed estimate using a 2nd order complementary filter
_update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS);
_update_speed(EAS_dem, indicated_airspeed, EAS2TAS);
// Calculate Specific Total Energy Rate Limits
_update_STE_rate_lim();
@@ -600,10 +595,13 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
if (_underspeed) {
_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
} else if (_badDescent) {
_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
} else if (_climbOutDem) {
_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
} else {
// If no error flag applies, conclude normal
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
src/lib/external_lgpl/tecs/tecs.h
+62 -55
View File
@@ -45,7 +45,7 @@ public:
_integGain(0.0f),
_vertAccLim(0.0f),
_rollComp(0.0f),
_spdWeight(0.5f),
_spdWeight(1.0f),
_heightrate_p(0.0f),
_heightrate_ff(0.0f),
_speedrate_p(0.0f),
@@ -106,53 +106,38 @@ public:
_throttle_slewrate(0.0f),
_indicated_airspeed_min(3.0f),
_indicated_airspeed_max(30.0f)
{
}
bool airspeed_sensor_enabled() {
bool airspeed_sensor_enabled()
{
return _airspeed_enabled;
}
void enable_airspeed(bool enabled) {
void enable_airspeed(bool enabled)
{
_airspeed_enabled = enabled;
}
// Update of the estimated height and height rate internal state
// Update of the inertial speed rate internal state
// Should be called at 50Hz or greater
void update_state(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat,
const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air);
void update_state(float baro_altitude, float airspeed, const math::Matrix<3, 3> &rotMat,
const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth, bool altitude_lock, bool in_air);
// Update the control loop calculations
void 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,
void 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);
// demanded throttle in percentage
// should return 0 to 100
float get_throttle_demand(void) {
return _throttle_dem;
}
int32_t get_throttle_demand_percent(void) {
return get_throttle_demand();
}
void reset_state() {
_states_initalized = false;
}
float get_throttle_demand(void) { return _throttle_dem; }
float get_pitch_demand() { return _pitch_dem; }
float get_speed_weight() { return _spdWeight; }
// demanded pitch angle in centi-degrees
// should return between -9000 to +9000
int32_t get_pitch_demand_cd() { return int32_t(get_pitch_demand() * 5729.5781f);}
// Rate of change of velocity along X body axis in m/s^2
float get_VXdot(void) { return _vel_dot; }
float get_speed_weight() {
return _spdWeight;
void reset_state()
{
_states_initalized = false;
}
enum ECL_TECS_MODE {
@@ -183,93 +168,115 @@ public:
enum ECL_TECS_MODE mode;
};
void get_tecs_state(struct tecs_state& state) {
void get_tecs_state(struct tecs_state &state)
{
state = _tecs_state;
}
void set_time_const(float time_const) {
void set_time_const(float time_const)
{
_timeConst = time_const;
}
void set_time_const_throt(float time_const_throt) {
void set_time_const_throt(float time_const_throt)
{
_timeConstThrot = time_const_throt;
}
void set_min_sink_rate(float rate) {
void set_min_sink_rate(float rate)
{
_minSinkRate = rate;
}
void set_max_sink_rate(float sink_rate) {
void set_max_sink_rate(float sink_rate)
{
_maxSinkRate = sink_rate;
}
void set_max_climb_rate(float climb_rate) {
void set_max_climb_rate(float climb_rate)
{
_maxClimbRate = climb_rate;
}
void set_throttle_damp(float throttle_damp) {
void set_throttle_damp(float throttle_damp)
{
_thrDamp = throttle_damp;
}
void set_integrator_gain(float gain) {
void set_integrator_gain(float gain)
{
_integGain = gain;
}
void set_vertical_accel_limit(float limit) {
void set_vertical_accel_limit(float limit)
{
_vertAccLim = limit;
}
void set_height_comp_filter_omega(float omega) {
void set_height_comp_filter_omega(float omega)
{
_hgtCompFiltOmega = omega;
}
void set_speed_comp_filter_omega(float omega) {
void set_speed_comp_filter_omega(float omega)
{
_spdCompFiltOmega = omega;
}
void set_roll_throttle_compensation(float compensation) {
void set_roll_throttle_compensation(float compensation)
{
_rollComp = compensation;
}
void set_speed_weight(float weight) {
void set_speed_weight(float weight)
{
_spdWeight = weight;
}
void set_pitch_damping(float damping) {
void set_pitch_damping(float damping)
{
_ptchDamp = damping;
}
void set_throttle_slewrate(float slewrate) {
void set_throttle_slewrate(float slewrate)
{
_throttle_slewrate = slewrate;
}
void set_indicated_airspeed_min(float airspeed) {
void set_indicated_airspeed_min(float airspeed)
{
_indicated_airspeed_min = airspeed;
}
void set_indicated_airspeed_max(float airspeed) {
void set_indicated_airspeed_max(float airspeed)
{
_indicated_airspeed_max = airspeed;
}
void set_heightrate_p(float heightrate_p) {
void set_heightrate_p(float heightrate_p)
{
_heightrate_p = heightrate_p;
}
void set_heightrate_ff(float heightrate_ff) {
void set_heightrate_ff(float heightrate_ff)
{
_heightrate_ff = heightrate_ff;
}
void set_speedrate_p(float speedrate_p) {
void set_speedrate_p(float speedrate_p)
{
_speedrate_p = speedrate_p;
}
void set_detect_underspeed_enabled(bool enabled) {
void set_detect_underspeed_enabled(bool enabled)
{
_detect_underspeed_enabled = enabled;
}
// in case of a height reset driven by the estimator we need
// to allow TECS to swallow the step in height and demanded height instantaneously
void handle_alt_step(float delta_alt, float altitude) {
void handle_alt_step(float delta_alt, float altitude)
{
// add height reset delta to all variables involved
// in filtering the demanded height
_hgt_dem_in_old += delta_alt;
@@ -278,7 +285,8 @@ public:
// reset height states
_integ3_state = altitude;
_integ1_state = _integ2_state = 0.0f;
_integ1_state = 0.0f;
_integ2_state = 0.0f;
}
private:
@@ -445,8 +453,7 @@ private:
float _indicated_airspeed_max;
// Update the airspeed internal state using a second order complementary filter
void _update_speed(float airspeed_demand, float indicated_airspeed,
float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS);
void _update_speed(float airspeed_demand, float indicated_airspeed, float EAS2TAS);
// Update the demanded airspeed
void _update_speed_demand(void);
@@ -461,7 +468,7 @@ private:
void _update_energies(void);
// Update Demanded Throttle
void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat);
void _update_throttle(float throttle_cruise, const math::Matrix<3, 3> &rotMat);
// Detect Bad Descent
void _detect_bad_descent(void);